function lqr_template

    % discretization grid size 
    % (for cost-to-go of the dynamic programming comparison)
    global N_phi N_w N_u phi_min phi_max w_min w_max u_min u_max

    % discretization grid sizes
    N_phi = 120; 
    N_w = 73;
    N_u = 5;

    % range of phi
    phi_min = -pi;
    phi_max = +pi; 

    % bounds on omega
    w_min = -pi;
    w_max = pi;

    % bounds on the controls
    u_min = -1.1;
    u_max = 1.1;


    % horizon and timestep for simulation
    N = 60;
    h = 0.12;

    

    % Task 1:
    % continous time linearization
    Ac = ...
    Bc = ...
    
    % convert to discrete time system, ts=0.1
    sysc = ss(Ac,Bc,eye(2),0);
    sysd = c2d(sysc,0.1);
    A = sysd.a;
    B = sysd.b;
    
    
    
    % Task 2:
    Q = ...
    R = ...
    S = ...
    
    % Task 3:
    % Calculate Pk matrices

    
    % Task 4:
    % compute J0

    ...
    
    figure(1)
    contour3(J0);
%     only in Matlab 2014b or newer
%     [C,handle] = contour3(J0);
%     set(handle,'LevelStep',get(handle,'LevelStep')*0.2)
    
    figure(2)
    contour3(get_first_J());
%     only in Matlab 2014b or newer
%     [C,handle] = contour3(get_first_J());
%     set(handle,'LevelStep',get(handle,'LevelStep')*0.2)
    
    

    % Task 5:
    % simulate linear system with optimal feedback
    x0 = [-pi/8;2.];
    
    
    % Task 6:
    % Simulate non-linear system with open-loop controls
    
    
    % Task 7:
    % simulate non-linear dynamics w feedback
    
    
    % Task 8:
    % solve the algebraic Ricatti Equation
    
    
    % Task 9:
    % LQR: non-linear dynamics w feedback by algebraic ricatti equation

    
end


%%%%%%%%%%%%%%%%%%%%%% LOCAL FUNCTIONS %%%%%%%%%%%%%%%%%%

function first_J = get_first_J()

    global N_phi N_w N_u
    
    h = 0.12;
    N = 60;


    % precalculate all integrations
    % for each state and control combination
    % compute next state, and immediate cost
    tic
    PhiNext = zeros(N_phi,N_w,N_u);
    WNext = zeros(N_phi,N_w,N_u);
    L = zeros(N_phi,N_w,N_u);
    for phiZ=1:N_phi
        for wZ=1:N_w
            for u_int=1:N_u
                [phi_int_next, w_int_next, cost] = integrate_Z(phiZ, wZ, u_int,h);
                PhiNext(phiZ,wZ,u_int) = phi_int_next;
                WNext(phiZ,wZ,u_int) = w_int_next;
                L(phiZ,wZ,u_int) = cost;
            end
        end
    end
    toc


    % DYNAMIC PROGRAMMING

    % backward recursion
    
    % initialize with infinite cost-to-go for all iterations
    J = inf*ones(N_phi,N_w,N); 
    
    % initialize final costs to be 0
    J(:,:,N) = 0; 
    
    % iterate backwards over time from N-1 to 1
    for k=N-1:-1:1
        
        % iterate over all states on the grid
        for phiZ=1:N_phi
            for wZ=1:N_w
                
                % iterate over all controls
                for u_int=1:N_u

                    % update the cost-to-go for this state
                    % use the lookup tables for efficiency

                    phi_int_next = PhiNext(phiZ,wZ,u_int);
                    w_int_next = WNext(phiZ,wZ,u_int);
                    
                    sum_cost = J(phi_int_next,w_int_next, k+1) + L(phiZ,wZ,u_int);
                    
                    if (sum_cost < J(phiZ,wZ, k))
                        J(phiZ,wZ, k) = sum_cost;
                    end
                    
                end
                
            end
        end
 
    end
    
    first_J = J(:,:,1);

end




function [phiZ_next, wZ_next, cost] = integrate_Z(phiZ,wZ,uZ,h)

    global N_phi w_min w_max

    phi = phiZ_to_phi(phiZ);
    w = wZ_to_w(wZ);
    u = uZ_to_u(uZ);

    x = [phi;w];
    
    [x_next,cost] = integrate_rk4(x,u,h);

    phi_next = x_next(1);
    w_next = x_next(2);

    if w_next > w_max
        w_next=w_min;
        cost = inf;
    elseif w_next < w_min
        w_next=w_min;
        cost = inf;
    end

    phiZ_next = mod(phi_to_phiZ(phi_next)-1, N_phi)+1;

    wZ_next = w_to_wZ(w_next);

end

function phi = phiZ_to_phi(phiZ)
    global phi_min phi_max N_phi
    phi = phi_min + (phiZ-1)*(phi_max-phi_min) / (N_phi);
end

function w = wZ_to_w(wZ)
    global w_min w_max N_w
    w = w_min + (wZ-1)*(w_max-w_min) / (N_w-1);
end

function u = uZ_to_u(uZ)
    global u_min u_max N_u
    u = u_min + (uZ-1)*(u_max-u_min) / (N_u-1);
end

function phi_int = phi_to_phiZ(phi)
    global phi_min phi_max N_phi
    phi_int = floor(((phi - phi_min) / (phi_max-phi_min)) * (N_phi) + (phi_max-phi_min)/N_phi)+1;
end

function w_int = w_to_wZ(w)
    global w_min w_max N_w
    w_int = round(((w - w_min) / (w_max-w_min)) * (N_w-1))+1;
end

function u_int = u_to_uZ(u)
    global u_min u_max N_u
    u_int = round(((u- u_min) / (u_max-u_min)) * (N_u-1))+1;
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
