function pendulum_template

    global N_phi N_w N_u phi_min phi_max w_min w_max u_min u_max
    global phiZ_to_phi wZ_to_w uZ_to_u

    % 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;

    phiZ_to_phi = phi_min + (0:N_phi-1)*(phi_max-phi_min) / (N_phi);
    wZ_to_w = w_min + (0:N_w-1)*(w_max-w_min) / (N_w-1);
    uZ_to_u = u_min + (0:N_u-1)*(u_max-u_min) / (N_u-1);
    
    
    % check if convertion from real numbers to integer numbers works
    assert(w_to_wZ(wZ_to_w(N_w)) == N_w);
    assert(w_to_wZ(wZ_to_w(1)) == 1);
    assert(phi_to_phiZ(phiZ_to_phi(N_phi)) == N_phi);
    assert(phi_to_phiZ(phiZ_to_phi(1)) == 1);




    % Task 1: simulate in discrete state space using the function
    %         integrate_Z and plot the evolution of phi, omega in time
    



    % 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
    
    
    % Task 2:
    % Do the same simulation as in task one using PhiNext and WNext
    
    
    

    % 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
                    
                    % Task3:
                    % update the cost-to-go for this state.
                    % use the lookup tables PhiNext,WNext,L for efficiency

                    
                end
                
            end
        end
 
    end
    
    % Task 4:
    % Implement the forward iteration to get the controls for the pendulum
    % swing-up
    
    x0 = [-pi;0];
    
    phiZ = phi_to_phiZ(x0(1));
    wZ = w_to_wZ(x0(2));
    
    

    
end


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

    global phiZ_to_phi wZ_to_w uZ_to_u
    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_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 [ 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






