function sqp_template

    global N x0 h delta lby uby ny
    
    animation = true;
    
    % 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;
    
    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','sqp','MaxFunEvals',100000);
    y = fmincon(@objective,y0,[],[],[],[],...
            lby,uby, @constraints, options);
       
    fprintf('Time to converge without passing Jacobian: %.2fs\n', toc);

    
    % Task 1:
    % run the fmincon solver and pass the finite-difference Jacobian to your non-linear constraints
    
    tic
    options=optimoptions(@fmincon,'GradConstr','on', 'display','final','algorithm','sqp','MaxFunEvals',100000);
    y = fmincon(@objective,y0,[],[],[],[],...
            lby,uby, @constraints_slow, options);
       
    fprintf('Time to converge with passing slow Jacobian: %.2fs\n', toc);
    
    % Task 2/3:
    % run the fmincon solver and pass the manually computed Jacobian to your non-linear constraints
    
    tic
    options=optimoptions(@fmincon,'GradConstr','on', 'display','final','algorithm','sqp','MaxFunEvals',100000);
    y = fmincon(@objective,y0,[],[],[],[],...
            lby,uby, @constraints_fast, options);
       
    fprintf('Time to converge with passing fast Jacobian: %.2fs\n', toc);
    
    
    if animation
        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
    end
    
    
    % Task 4-7:
    % SQP with quadprog using GN_step
   
end



function [y_next]=GN_step(y_bar)

    global lby uby
    
    % Task 6: ...
    
    % compute H
    
    % compute Aeq
    
    % compute beq
    
    options = optimoptions(@quadprog,'Algorithm','interior-point-convex','Display','none');
    y_next = quadprog(H,[],[],[],Aeq, beq, lby, uby, [], options);

end


function [jac,Gy] = GJacFast(y)

    global N
    
    Gy = G(y);
    
    % Task 3: ...
    
    
    

end


function [A,B] = rk4step_jac(x,u)
    
    global h delta
    
    % Task 3: ...
    

end


function [jac,Gy] = GJacSlow(y)

    global delta
    
    Gy = G(y);
    
    % Task 1: ...
    

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 [c,ceq,gradc,gradceq] = constraints_fast(y)
    c = [];    
    ceq = G(y);
    
    if nargout > 2
        gradc = [];
        gradceq = GJacFast(y);
    end
    
end

function [c,ceq,gradc,gradceq] = constraints_slow(y)
    c = [];    
    ceq = G(y);
    
    if nargout > 2
        gradc = [];
        gradceq = GJacSlow(y);
    end
    
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