function [tt,xx,uu,g] = mpc_SemSiev(N,T,x0,mpciterations,ls,myprint)
% Haunschmied et al. example with dynamics as ODE
%
% Parameters:
%
%    N  = horizon for open loop optimization
%    T  = time step (sampling interval length)
%    x0 = initial value
%    mpciterations = number of iterations for simulation
%    ls = color of closed loop trajectory (in MATLAB linespec format)
%    myprint = control of graphical output
%              1: only closed loop trajectory
%              2: open and closed loop trajectories
%
% Output:
%
%    tt: time grid of closed loop solution
%    xx: states of closed loop solution
%    uu: control of closed loop solution
%    g:  stage cost at final point of closed loop solution
%
% Example calls:
%
% lower equilibrium with open loop trajectories, low accuracy:
% mpc_haunschmiedetal_ODE(10,0.5,[0.1, 0.1],50,'-k',2); 
%
% lower equilibrium without open loop trajectories, low accuracy:
% mpc_haunschmiedetal_ODE(10,0.5,[0.1, 0.1],50,'-k',1); 
%
% upper equilibrium with open loop trajectories, medium accuracy:
% mpc_haunschmiedetal_ODE(30,0.5,[4.8, 1.3],50,'-k',2); 
%
% upper equilibrium without open loop trajectories, medium accuracy:
% mpc_haunschmiedetal_ODE(30,0.5,[4.8, 1.3],50,'-k',1);
%Take this:  mpc_SemSiev(8,0.5,[0.1, 0.15],250,'-k',1); 
%
% use 'axis([0,6,0,1.5]);' after the computation to set the 
% graphic window to the constraint set
%
% note: needs nmpc.m, available from www.nmpc-book.com/matlab_nmpc.html 
%       the file nmpc.m must be contained in the same directory as this
%       file


    t0            = 0.0;                    % initial time
    u0            = 0.5*ones(1,N);          % initial guess for control
                                            % in first NMPC step
    tol_opt       = 1e-5;                  % tolerance for optimizer
    opt_option    = 0;                      % choice of optimizer
    iprint        = 5;                      % controls verbosity
    type          = 'differential equation';% type of dynamics
    atol_ode_real = 1e-10;                  % tolerances for ODE solver in
    rtol_ode_real = 1e-10;                  % simulation of MPC closed loop
    atol_ode_sim  = 1e-2;                   % tolerances for ODE solver in
    rtol_ode_sim  = 1e-2;                   % optimization

    % model parameters
    global del;
    del = 0.7;
    global eps1;
    eps1 = 0.045;
    global eps2;
    eps2 = 0.078;
    global gam;
    gam = 0.07;
    global bet;
    bet = 0.6;
    global alf;
    alf = 0.1;
    
    % discount factor
    global lambda;
    lambda = exp(-T*0.04);  
        
    % some global variables for internal use
    global gN;
    gN = N;
    
    global gprint;
    gprint = myprint;
    
    global guu;
    guu = [];
    
    global gtt;
    gtt = [ t0 ];
    
    global gxx;
    gxx = [ x0 ];
    
    global gls;
    gls = ls;
    
    % start of the nmpc routine
    nmpc(@runningcosts, @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);

    % for output of some statistical values 
    % (time, state, control along closed loop trajectory, 
    %  stage cost at final point of closed loop trajectory)
    tt = gtt;
    xx = gxx;
    uu = guu;
    
    g = runningcosts(tt(length(tt)),xx(length(xx),:),uu(length(uu)));
    
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of the NMPC functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cost = runningcosts(t, x, u)

    global k1;
    global k2;
    global c1;
    global c2;
    global alpha;
    global lambda;

    %R = k1*sqrt(x(1)) - x(1)/(1.0+k2*x(1)^4);
    %c = c1*x(2) + c2*x(2)^2/2.0;

    % discount factoe lambda is specified above
    cost = -lambda^t*(log(u(1)));

end

function cost = terminalcosts(t, x)
    cost = 0.0;
end

function [c,ceq] = constraints(t, x, u)
    
    % constraints are chosen identical to the dynamic programming 
    % solution in the "Using dynamic programming..." paper
    c(1) = -x(1);
    c(2) = x(1) - 6.0;
    c(3) = -x(2);
    c(4) = x(2) - 1.5;
    ceq = [];
end

function [c,ceq] = terminalconstraints(t, x)
    global gconstr;
    
    % constraints are chosen identical to the dynamic programming 
    % solution in the "Using dynamic programming..." paper
    c(1) = -x(1);
    c(2) = x(1) - 6.0;
    c(3) = -x(2);
    c(4) = x(2) - 1.5;
    ceq = [];
end

function [A, b, Aeq, beq, lb, ub] = linearconstraints(t, x, u)
    A   = [];
    b   = [];
    Aeq = [];
    beq = [];
    % upper and lower bound on control values
    lb = -0.5;
    ub =  0.5;
end

function y = system(t, x, u, T)
    global del; 
    global alf;
    global bet;
    global eps1;
    global eps2;
    global gam;
    
    % ODE model from Haunschmied et al.
    y = zeros(2,1);
    y(1) = x(1)* (alf-bet*x(2)-0*eps1*x(1));
    y(2) = x(2)*(-gam+del*x(1)-0*eps2*x(2)-0*u(1));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of output format, called from within the NMPC routine at
% each step of the iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function printHeader()
    fprintf('   k  |      u(k)        x(1)    0    cost     Time\n');
    fprintf('--------------------------------------------------\n');
end

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

function plotTrajectories(dynamic, system, T, t0, x0, u, ...
                          atol_ode, rtol_ode, type)
    global gN;
    global gprint;
    global gtt;
    global gxx;
    global guu;
    global gls;
    
    gtt = [gtt; t0+T];
    
    if (gprint>=1)
        figure(1);
        hold on;  
    end;
        x = x0;
        for k=1:gN
            [x,t_intermediate, x_intermediate] = ...
                dynamic(system, T, t0+k-1, x, u(:,k), ...
                             atol_ode, rtol_ode, type);
            if (k==1)
                if (gprint>=1)
                    plot(x_intermediate(:,1),x_intermediate(:,2),gls);
                end;
                gxx = [gxx; x];
                guu = [guu, u(1,1)];
            else
                if (gprint>=2)
                    plot(x_intermediate(:,1),x_intermediate(:,2),'--r');
                end;
            end 
        end
        if (gprint>=1)
            if (gprint>=2)
               % title(['open loop trajectories (dashed) and closed loop trajectory (solid)']);
            else
               % title(['closed loop trajectory (solid)']);
            end;
            xlabel('x_1(n)');
            ylabel('x_2(n)');
            grid on;
        end;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
