%Task: Run MPC for thermal storage plant with one consumer and one source
%Who : Cristina Zotica (NTNU)
%Date: 24.02.2021
%Ver : ESCAPE 30 extended paper with q=DV.
%Info: requires casadi

clc;close all;clear
% path to CasAdi
addpath('M:\Documents\phd\Software\casadi-matlabR2014b-v3.3.0')
import casadi.*

%% Test model
[F,f] = TEScasadi;

u0 = [500;0;0;0];                       % [qSP,qD, qTP, qEP] nominal inputs
x0 = [2500;5;0];                                 % initial cond diff states
d0 = [500;500];                                     % [qS, qsp] disturbance
Fs = F('x0',x0,'p',[u0;d0]);                          % integrate with idas

p0=5; % pressure
%% NMPC
st  = 1/60;                                            % [hr] sampling time
Ph  = 24;%180/60;                                          % [hr] prediction horizon
N   = ceil(Ph/st);                                  % number of control intervals
qsp = MX.sym('qsp');                              % [m3/h] demand set point
qS  = MX.sym('qS');                                      % [m3/h] heat load
nu = length(u0);                                         % number of inputs
nx = length(x0);                            % number of differential states


% Bounds
lbu = zeros(nu,1);                                    % lower bounds inputs
ubu = ones(nu,1)*1000;                                % upper bounds inputs
lbx = [0;3;-inf];                                 % lower bound diff state
ubx = [5000;6;inf];                            % upper bound diff state

% Collocation
d = 3;                                 % Degree of interpolating polynomial
tau_root = collocation_points(d,'radau');    % Get collocation points. dae
tau_root = [0 tau_root];
C = zeros(d+1,d+1);              % Coefficients of the collocation equation
D = zeros(d+1, 1);                % Coefficients of the continuity equation
B = zeros(d+1, 1);                % Coefficients of the quadrature function
% Construct polynomial basis
for j=1:d+1
     % Construct Lagrange polynomials to get the polynomial basis
     % at the collocation point
     coeff = 1;
     for r=1:d+1
          if r ~= j
               coeff = conv(coeff, [1, -tau_root(r)]);
               coeff = coeff / (tau_root(j)-tau_root(r));
          end
     end
     % Evaluate the polynomial at the final time to get the coefficients
     % of the continuity equation
     D(j) = polyval(coeff, 1.0);
     
     % Evaluate the time derivative of the polynomial at all collocation
     % points to get the coefficients of the continuity equation
     pder = polyder(coeff);
     for r=1:d+1
          C(j,r) = polyval(pder, tau_root(r));
     end
     
     % Evaluate the integral of the polynomial to get the coefficients of
     % the quadrature function
     pint = polyint(coeff);
     B(j) = polyval(pint, 1.0);
end

% Start with an empty NLP
w   = {};                                                %inputs and states
w0  = [];
lbw = [];
ubw = [];
J   = 0;
g   = {};                                    % Constraints; including model
lbg = [];
ubg = [];

% Initial conditions
X0  = MX.sym('X0', nx);
w   = {w{:}, X0};
lbw = [lbw; lbx];
ubw = [ubw;  ubx];
w0  = [w0; x0];

X0_par = MX.sym('X0_par', nx);


g   = {g{:}, X0 - X0_par};                     % enforce initial constraint
lbg = [lbg; zeros(nx,1)];
ubg = [ubg; zeros(nx,1)];

U0      = MX.sym('U0', nu);

% Formulate the NLP
Xk = X0;

for k=0:N-1                              % N = number of control intervals
     % New NLP variable for the control
     Uk  = MX.sym(['U_' num2str(k)],nu,1);
     w   = {w{:}, Uk};                   % first uk that goes into the loop
     lbw = [lbw; lbu];                                  % input lower bound
     ubw = [ubw; ubu];                                  % input upper bound
     w0  = [w0;  u0];                                % input initial values
     
     % State at collocation points
     Xkj  = {};
     for j=1:d  %d is the degree of interpolating polynomial = 3
          Xkj{j} = MX.sym(['X_' num2str(k) '_' num2str(j)], nx,1);
          w      = {w{:}, Xkj{j}};
          lbw    = [lbw; lbx];                    % lower bounds for states
          ubw    = [ubw;  ubx];                   % upper bounds for states
          w0     = [w0; x0];             % initial values for states (diff)
     end
     
     % Loop over collocation points
     Xk_end = D(1)*Xk;
     for j=1:d
          % Expression for the state derivative at the collocation point
          xp = C(1,j+1)*Xk;
          for r=1:d
               xp = xp + C(r+1,j+1)*Xkj{r};
          end
          
          % Append collocation equations
          [fj,qj] = f(Xkj{j},[Uk;qS;qsp]);
          g          = {g{:}, st*fj - xp};     % slope matching constraint
          lbg        = [lbg; zeros(nx,1)];
          ubg        = [ubg; zeros(nx,1)];
          
          % Add contribution to the end state
          Xk_end = Xk_end + D(j+1)*Xkj{j};
          
          J = J+qj;                                            % stage cost
          
     end
     
     % New NLP variable for state at end of interval
     Xk  = MX.sym(['X_' num2str(k+1)],nx,1);
     w   = {w{:}, Xk};
     lbw = [lbw; lbx];
     ubw = [ubw;  ubx];
     w0  = [w0; x0];
     
     % Add equality constraint
     g   = {g{:}, Xk_end-Xk};                     % shooting gap constraint
     lbg = [lbg; zeros(nx,1)];
     ubg = [ubg; zeros(nx,1)];
     
     g = {g{:}, Uk(1)-qS};                            % demand constraint
     lbg = [lbg; -500];                                       % lower limit
     ubg = [ubg; 0];                                          % upper limit
     
end

% Create an NLP solver
nlp = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:}),'p',vertcat(X0_par,U0,qS,qsp));

% IPOT options
options = struct('warn_initial_bounds',false,'print_time',false);
options.ipopt.print_level = 0;
%options.ipopt.acceptable_constr_viol_tol = 1e-15;
%options.ipopt.acceptable_tol = 1e-10;

solver = nlpsol('solver', 'ipopt', nlp);

%% Solve the NLP
ts   = [1;12;15;50]/st;                                    % [h] steps time
time = 30;                                         % [h] simulation time
it   = ceil(time/st);                                    % number of interations
x_0  = x0;
n_w  = numel(w0);
u_in = u0;
dq   = 500;                       % [m3/h] step change in supply or demand
qnom = 500;                                                 % initial demand
for i=1:it
     % Steps in swing boiler (q_S) and demand flowrate (q_sp)
     if i<ts(1)
          q_S  = qnom;
          q_sp = qnom;
     end
     
     if i>=ts(1) && i<ts(2)
          q_S  = qnom+dq;
          q_sp = qnom;
     end
     
     if i>=ts(2) && i<ts(3)
          q_S  = qnom+dq;
          q_sp = qnom+dq;
     end
     
     if i>=ts(3) && i<ts(4)
          q_S  = qnom;
          q_sp = qnom+dq;
     end
     
     if i>=ts(4)
          q_S  = qnom;
          q_sp = qnom-dq/2;
     end
     
     sol    = solver('p',vertcat(x_0,u_in,q_S,q_sp),'x0', w0, 'lbx', lbw,...
          'ubx', ubw,'lbg', lbg, 'ubg', ubg); %open loop solution
     w_opt  = full(sol.x);
     w0     = w_opt;
     u_in   = w_opt(nx+1:nx+nu);
     
     u1_opt = w_opt(nx+1:nu+(d+1)*nx:n_w);
     u2_opt = w_opt(nx+2:nu+(d+1)*nx:n_w);
     u3_opt = w_opt(nx+3:nu+(d+1)*nx:n_w);
     u4_opt = w_opt(nx+4:nu+(d+1)*nx:n_w);
     
     
     % Integrate plant dynamics with IDAS
     Fsol = F('x0',x_0,'p',[u_in;q_S;q_sp]);
     x_plant    = full(Fsol.xf);
     x_0        = x_plant;
     
     u_vec(i,:) = u_in';                                           % inputs
     x_vec(i,:) = x_plant';               % closed loop solution diff state
     
     %figure(1492)
     subplot(3,1,1);plot(u_vec);
     subplot(3,1,2);plot(x_vec(:,1));
     subplot(3,1,3);plot(x_vec(:,2));
     
end
figure(1492)
subplot(3,1,1);legend('qSP','qD','qTP','qEP')
subplot(3,1,2);legend('V')
subplot(3,1,3);legend('p')


%% Save results
save('ResultsMPC_Ph10','time','u_vec','x_vec','st','N','Ph') % save variables in given filename

for i=1:4
     TV(i,1) = sum(abs(diff(u_vec(:,i))));
end
TV/1000;

x_vec(end,3)