function nlp_solution

    global N x0 h delta
    
    
    
    % bounds on omega
    omega_min = -pi;
    omega_max = pi;

    % bounds on the controls
    u_min = -1.1;
    u_max = 1.1;
    
    N = 60;
    x0 = [-pi;0];
    u0 = 0;
    h = 0.2;
    delta = 1e-4;
    
    
    fprintf('Norm of G(y) from simulate: %f\n', norm(G(simulate([0,0.5]',ones(N,1)))));
    
    
    
    y0=repmat([x0;u0],N,1);
    y0 = [y0;x0];
    ny = numel(y0);
    
    
    lby = repmat([-inf;omega_min;u_min],N,1);
    lby = [lby;[-inf;omega_min]];
    
    uby = repmat([inf;omega_max;u_max],N,1);
    uby = [uby;[inf;omega_max]];
    


    tic
    options=optimoptions(@fmincon,'GradConstr','off', 'display','final','algorithm','interior-point','MaxFunEvals',100000);
    y = fmincon(@objective,y0,[],[],[],[],...
            lby,uby, @constraints, options);
       
    fprintf('Time to converge: %.2fs\n', toc);
    
    phi = y(1:3:end);
    omega = y(2:3:end);
    x = [phi';omega'];
    u = y(3:3:end);
        
    for k=1:N
       
        plot_pendulum(x(:,k));
        pause(h)
    end
    
    
%     x_cur = x0;
%     for k=1:N
%        
%         x_cur = integrate_rk4(x_cur,u(k),h);
%         plot_pendulum(x_cur);
%         pause(h)
%         
%     end

end



function y = simulate(x0,U)
    
    global N h
    
    y = x0;
    x = x0;
    for k=1:N
        x = integrate_rk4(x,U(k),h);
        y = [y;U(k);x];
    end

end


function f = objective(y)
    phi = y(1:3:end);
    u = y(3:3:end);
    f = phi'*phi + u'*u;
end

function ceq = G(y)

    global N x0 h;
    
    x = @(k) y(3*(k-1)+1:3*(k-1)+2);
    u = @(k) y(3*(k-1)+3);

    ceq = x0-x(1);
    
    for k=1:N
        ceq = [ceq;integrate_rk4(x(k),u(k),h)-x(k+1)];
    end

end

function [c,ceq] = constraints(y)
    c = [];    
    ceq = G(y);
end


function [ x_next, q ] = integrate_rk4(x,u,h)

    [k1,q1] = ode(x,u);
    [k2,q2] = ode(x+h/2.*k1,u);
    [k3,q3] = ode(x+h/2.*k2,u);
    [k4,q4] = ode(x+h*k3,u);
    x_next = x + h/6. * (k1+2*k2+2*k3+k4);
    q = h/6. * (q1+2*q2+2*q3+q4);
end

function [dx,dq]=ode(x,u)
    dx = [x(2); 2*sin(x(1)) + u]; 
    dq = x(1)^2 + u^2;
end

function plot_pendulum(x)
    phi = x(1);

    plot([0;sin(phi)], [0;cos(phi)], '-o')
    xlim([-1,1])
    ylim([-1,1])

end