function [tt,xx,uu,g] = mpc_adaptation4(N,T,x0,mpciterations,ls,myprint)
% NMPC Adaptation
%
% 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 call:  
%  this works: mpc_adaptation4(4,0.5,[1.3, 0.5, 0.8, 1.5, 1.3],50,'-k',1);
%   not his one: mpc_adaptation4(5,0.5,[2, 0.5, 0.8, 0.9, 1],50,'-k',2); (plot with predictions)
%
%   this works:  mpc_adaptation4(4,0.5,[1.3, 0.5, 0.8, 1.5, 1.3],20,'-k',1); (plot without predictions)
% 
%
% 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(3,N);          % initial guess for cont
                                            % in first NMPC step
                                          
    %u0            = 0.5;
    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 are specified in the subroutines, below
    
    % discount factor
    roh = 0.03;
    n = 0.015; 
    global lambda;
    lambda = exp(-T*(roh-n));  
        
    % 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;
    
    length(uu)
    g = runningcosts(tt(length(tt)),xx(length(xx),:),uu(length(uu),:));
    
end
 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of the NMPC functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cost = runningcosts(t, x, u)
 
    
    global lambda;
    
    global ep;
    ep = 0.11;
    
    alpha2 = 0.7;               % share of public resources used for transfers and public consumption
    eta = 0.1;
    sigma = 1.1;
    omega = 0.05;
    epsilon = 2;              % strenght of utility loss by GHG concentration
    Mtilde = 1;                 % pre-industrial level of GHG concentration 
    
    %global z1;                 % secure private capital x(1) to be positive
    %z1 = max(0.0001,x(1));                 
    
    %global v1;                  % share of public capital used to support market activity
    %v1 = 0.9-u(2);
    
                     % share of public capital used for adaptation to climate change
    v2 = (1-u(2))/2;
                      
    govc = (alpha2*ep)^eta;
    GHG = (x(5)-Mtilde)^(-epsilon);
    adapt=(v2*x(2))^omega;
 
    % discount factor lambda is specified above
   
    cost = - lambda^t*(((u(1)*govc*GHG*adapt)^(1-sigma))-1)/(1-sigma);		% objective function
    
  
end
 
function cost = terminalcosts(t, x)
    cost = 0.0;
end
 
function [c,ceq] = constraints(t, x, u)
    
    % state constraints for n=0..N-1
    c(1) = -x(1) + 0.0;
    c(2) =  x(1) - 9.0;
    c(3) = -x(2) - 0.0;
    c(4) =  x(2) - 9.0;
    c(5) = -x(3) - 0.0;
    c(6) =  x(3) - 9.0;
    c(7) = -x(4) - 0.0;
    c(8) =  x(4) - 9.0;
    c(9) = -x(5) - 0.0;
    c(10)=  x(5) - 9.0;
    ceq = [];
end
 
function [c,ceq] = terminalconstraints(t, x)
    global gconstr;
    
    % state constraints for n=N
    c(1) = -x(1) + 0.0;
    c(2) =  x(1) - 9.0;
    c(3) = -x(2) - 0.0;
    c(4) =  x(2) - 9.0;
    c(5) = -x(3) - 0.0;
    c(6) =  x(3) - 9.0;
    c(7) = -x(4) - 0.0;
    c(8) =  x(4) - 9.0;
    c(9) = -x(5) - 0.0;
    c(10)=  x(5) - 9.0;
    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(1) =  0.0001;
    ub(1) =  2.5;
    lb(2) =  0.1;
    ub(2) = 0.9999;
    lb(3) = 0.0001;
    ub(3) = 2.0;
end
 
function y = system(t, x, u, T)
    
    % Parameters
    A = 1;                  % productivity of output production
    alpha = 0.5;            % exponent private capital k in production function
    beta = 0.2;             % exponent public capital g in production function  
    n = 0.015;              % population growth rate
    alpha1 = 0.1;           % share of public resources used to build up new public capital g
    alpha2 = 0.7;           % share of public resources used for transfers and public consumption
    alpha3 = 0.1;           % share of public resources used for the functioning of the administration
    r = 0.07;               % borrowing rate
    deltak = 0.075;         % depreciation of private capital
    deltag = 0.05;          % depreciation of public capital
    psi = 1;
    tau = 2;
    %Rtilde = 30;           % initial stock of non-renewable resource
    gamma = 0.9;            % factor which transforms extraction in emissions
    mu = 0.01;              % strength of carbon concentration decay
    Mtilde = 1;             % pre-industrial carbon dioxide concentration
    kappa = 2;              % parameter before M0
    Ak = 1;                 % productivity of private capital
    Au = 40;                % productivity of resource
    ipf = 0.05;             % foreign aid 
    theta = 0.01;
    %global z1;
    %global v1;              % share of public capital used to support market activity
    %global v2;              % share of public capital used for adaptation to climate change
    %v2 = (1-u(2))/2
    v3 = (1-u(2))/2;               % share of public capital used to install cleaner technology
    %v3 = 1-v1-v2;          % share of public capital used to install cleaner technology
    %ipb = 0.05;            % net borrowing
     z1 = max(x(1),0);
    %cost = 0.1;
    cost = psi*((x(4))^(-tau));         % marginal resource extraction cost
    Yk = (Ak*z1+Au*u(3))^alpha;         % contribution of private capital and resource in production function
    Yg = (u(2)*x(2))^beta;                % contribution of share of public capital in production
    Y = A*Yk*Yg;                        % production function
    global ep;
    
    % ODE model
    y  = zeros(5,1);
    
    %y(1) = 1;
    %y(2) = 1;
    %y(3) = 1;
    %y(4) = 1;
    %y(5) = 1;
    y(1) = Y-u(1)-u(2)-cost*u(3)-z1*(deltak+n);                % change in private capital 
    y(2) = ipf+alpha1*u(2)-(deltag+n)*x(2);                     % change in public capital
    y(3) = (r-n)*x(3)-(1-alpha1-alpha2-alpha3)*u(2);            % change in public debt
    y(4) = -u(3);                                               % change in stock of non-renewable resource
    y(5) = gamma*u(3)-mu*(x(5)-kappa*Mtilde)-theta*(v3*x(2));   % change in GHG concentration
    
    %y(2) = ipf+alpha1*u(2)-(deltag+n)*x(2)+ipb;                % change in public capital allowing for net borrowing
    %y(3) = (r-n)*x(3)-(1-alpha1-alpha2-alpha3)*u(2)+ipb;       % change in public debt allowing for net borrowing
    
end
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of output format, called from within the NMPC routine at
% each step of the iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function printHeader()
    fprintf('   t  |  u(1)   u(2)   u(3) |  x(1)   x(2)   x(3)   x(4)   x(5) |  cost   Time\n');
    fprintf('-----------------------------------------------------------------------------------------------------\n');
end
 
function printClosedloopData(mpciter, u, x, t_Elapsed)
   
     fprintf(' %3d  | %+5.2f  %+5.4f  %+5.2f | %+5.2f  %+5.2f  %+5.2f  %+5.2f  %+5.2f | %+7.3f', mpciter, ...
            u(1,1), u(2,1), u(3,1), x(1), x(2), x(3), x(4), x(5), runningcosts(mpciter, x, u(:,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+T*(k-1), x, u(:,k), ...
                             atol_ode, rtol_ode, type);
            if (k==1)
                if (gprint>=1)
                    %plot(t_intermediate, x_intermediate(:,1), 'k-');		
                    %plot(t_intermediate, x_intermediate(:,2), 'c-');		
                   	%plot(t_intermediate, x_intermediate(:,3),'b-');     	
                    %plot(t_intermediate, x_intermediate(:,4),'m-');
                    %plot(t_intermediate, x_intermediate(:,5),'g-');		
                   	%plot(t_intermediate, u(1), 'r-');               		
                    plot(t_intermediate, u(2), 'c-');
                    %plot(t_intermediate, u(3), 'r-');               		
                 end;
               
                gxx = [gxx; x];
                guu = [guu; u(:,1)'];
            else
                if (gprint>=2)
                    plot(t_intermediate,x_intermediate(:,1),'--r');
                    plot(t_intermediate,x_intermediate(:,2),'--b');
                  % plot(t_intermediate,x_intermediate(:,3),'--k');
                    plot(u(2), 'g*');
                 
                end;
            end 
        end
                
        figure(2);
        hold on;
        x = x0;
        for k=1:gN
       
            [x,t_intermediate, x_intermediate] = ...
                dynamic(system, T, t0+T*(k-1), x, u(:,k), ...
                             atol_ode, rtol_ode, type);
            if (k==1)
                if (gprint>=1)
                    plot(t_intermediate, x_intermediate(:,1), 'k-');		
                    plot(t_intermediate, x_intermediate(:,2), 'c-');		
                   	plot(t_intermediate, x_intermediate(:,3),'b-');     	
                    plot(t_intermediate, x_intermediate(:,4),'m-');
                    plot(t_intermediate, x_intermediate(:,5),'g-');		
                   	plot(t_intermediate, u(1), 'r-');               		
                    plot(t_intermediate, u(2), 'g-');
                    plot(t_intermediate, u(3), 'm-');

                 end;
                 
                gxx = [gxx; x];
                guu = [guu; u(:,1)']