function double_integrator
% double_integrator
%
% Template for the use of the MATLAB routine nmpc.m 
% (available from www.nmpc-book.com)                 
% Further information will be provided in the course 
%                                                   (C) Lars Gruene 2012

    clear all;
    close all;

    N             = 5;               % prediction horizon
    T             = 1.0;             % sampling time
    mpciterations = 30;              % number of iterations for simulation
    t0            = 0.0;             % initial time (should be 0.0)
    x0            = [-1.0, 1.0];     % initial value:
                                     % the dimension of x0 must fit the
                                     % dimension of the system
    u0            = ones(1,N);       % initial guess for u for first 
                                     % optimization iteration
    tol_opt       = 1e-8;            % accuracy for optimizer
    opt_option    = 0;               % choice of optimization algorithm
    iprint        = 5;               % verbosity level
    type          = 'difference equation'; % type of equation:
                                     % 'difference equation' or
                                     % 'differential equation'
    atol_ode_real = 1e-12;           % absolute accuracy of ode solver
                                     % for simulation
    rtol_ode_real = 1e-12;           % relative accuracy of ode solver
                                     % for simulation
    atol_ode_sim  = 1e-4;            % absolute accuracy of ode solver
                                     % for optimization
    rtol_ode_sim  = 1e-4;            % relative accuracy of ode solver
                                     % for optimization

    nmpc(@stagecosts, @terminalcosts, @constraints, ...
         @terminalconstraints, @linearconstraints, @system, ...
         mpciterations, N, T, t0, x0, u0, ...
         tol_opt, opt_option, ...
         type, atol_ode_real, rtol_ode_real, atol_ode_sim, rtol_ode_sim, ...
         iprint, @printHeader, @printClosedloopData, @plotTrajectories);

end







%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of the NMPC functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cost = stagecosts(t, x, u)  % defnines the stage costs
    cost = x(1)^2+x(2)^2+5*u(1)^2;
end

function cost = terminalcosts(t, x)  % defines the terminal costs
    cost = 0.0;
end

function [c,ceq] = constraints(t, x, u)  
% defines the nonlineat constraints which will be imposed for 
% x and u at all times k=0,...,N-1

    % inequality constraints: 

    c(1) =  x(1)-1;     %  x(1)-1<=0  <=>  x(1) <=  1
    c(2) = -x(1)-1;     % -x(1)-1<=0  <=>  x(1) >= -1
    c(3) =  x(2)-1;     %  x(2)-1<=0  <=>  x(2) <=  1
    c(4) = -x(2)-1;     % -x(2)-1<=0  <=>  x(2) >= -1
    
    % equality constraints:
    
    ceq  = [];          % no equality constraints in this example
end

function [c,ceq] = terminalconstraints(t, x) 
% defines the terminal constraints. This is analogous to "constraints"
% but only for the state x and only at time N 

    c(1) =  x(1)-1;     % same as in constraints      
    c(2) = -x(1)-1;
    c(3) =  x(2)-1;
    c(4) = -x(2)-1;
    ceq  = [];
end

function [A, b, Aeq, beq, lb, ub] = linearconstraints(t, x, u)
% defines linear constraints on u at each time k=0,...,N-1
% if constraints on u can be expressd by linear (in)equalities, it is 
% algorithmically preferable to use this function instead of "constraints"
% for further information on the syntax see the help page of "fmincon" 

    A   = [];
    b   = [];
    Aeq = [];
    beq = [];
    lb  = -1;   % lower bound for u
    ub  =  1;   % upper bound for u 
end

function y = system(t, x, u, T)      
% defines the discrete time dynamics
    y(1) = x(1)+T*x(2)+T^2*u(1)/2;   % exact 2d sampled data model of the 
    y(2) = x(2)+T*u(1);              % double integrator x'' = u
end

function dx = system_ode(t, x, u, T) 
% defines the ODE dynamics. This can be used as an alternative to the
% discrete time model (but not together). To use this, change "@system" in
% the parameter list of nmpc to "@system_ode" and change the type in the
% parameter list at the beginning to 'differential equation'
    dx    = zeros(2,1);
    dx(1) = x(2);
    dx(2) = u(1);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of output format
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function printHeader()
    fprintf('   k  |      u(k)        x(1)        x(2)     Time\n');
    fprintf('--------------------------------------------------\n');
end

function printClosedloopData(mpciter, u, x, t_Elapsed)
    fprintf(' %3d  | %+11.6f %+11.6f %+11.6f  %+6.3f', mpciter, ...
            u(1,1), x(1), x(2), t_Elapsed);
end

function plotTrajectories(dynamic, system, T, t0, x0, u, ...
                          atol_ode, rtol_ode, type)
    [x, t_intermediate, x_intermediate] = dynamic(system, T, t0, ...
                                          x0, u, atol_ode, rtol_ode, type);
    
    style = ['-k '; '--k'; '-r '; '--r'];                            
    select = [1, 2];        % components for phase portrait            
    
    if (length(select)==2)  % plot phase portrait
        figure(1);
        title('phase portrait of closed loop trajectory');
        xlabel('x_1');
        ylabel('x_2');
        grid on;
        hold on;
        plot(x_intermediate(:,select(1)),x_intermediate(:,select(2)),'-k');
        plot([-1 -1 1 1 -1], [-1 1 1 -1 -1], 'k');
        axis([-1.1 1.1 -1.1 1.1]);
        axis square;
    end;
    
    figure(2);
        title(['closed loop trajectory']);
        xlabel('n');
        ylabel('x_i(n), u(n)');
        grid on;
        hold on;
        for i= 1:length(x0)
           % style(i,:)
            plot(t_intermediate,x_intermediate(:,i),style(i,:));
        end;
        plot(t_intermediate,u(1).*ones(size(t_intermediate)),'-b');
        axis([0 30 -0.5 1.1]);
        axis square;

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
