% Dec 2017
% Linear MPC for cooler. Obj = w1(Th-Thsp)^2+w2(1-zh)^2
% no complementarity constraints. changed idas to cvodes
% no terminal cost to make sure zc=1 for a large disturbance
% scaled model wrt Tinh (largest T)
% linearized model arround the nominal point, to be able to use a linear
% MPC


clear;clc;  %close all;
%addpath('C:\Documentos\Casadi\casadi-matlabR2014b-v3.2.3')
%addpath('C:\Documentos\Casadi\casadi-matlabR2014b-v3.3.0')

addpath('M:\Documents\phd\Software\casadi-matlabR2014b-v3.2.0')
%addpath('M:\Documents\phd\Software\casadi-matlabR2014b-v3.3.0')
import casadi.*

parameters % load all parameters from a separate file


%% casadi definitions

xc = MX.sym('xc',10,1);    % x(1), cold side temperature
xh = MX.sym('xh',10,1);    % x(2), hot side temperature
x = [xc;xh];               % Temperatures
u = MX.sym('u',2,1);       % control input;  valve opening
F_c = u(1)*F_c_max;        % cold side mass flow
F_h = u(2)*F_h_max;        % hot side mass flow
Tinc = MX.sym('Tinc',1,1); % disturbance
wF = MX.sym('wt', 1);      % weight for hot flow in the objective functions

%% model derivatives
xc_bar = [xc(2:10);Tinc]; %inlet to each control volume
xh_bar = [Tinh;xh(1:9)];
xc_dot = F_c/rho_c/V_c*(xc_bar-xc) + (xh-xc)*U*A/rho_c/V_c/cp_c; % cold side
xh_dot = F_h/rho_h/V_h*(xh_bar-xh) - (xh-xc)*U*A/rho_h/V_h/cp_h; % hot side
diff = vertcat(xc_dot,xh_dot);

%% defining casadi function for the model
f = Function('f', {x,u,Tinc}, {diff});

%% Initial values, scaled for temperature
Tc0 = [314.1411  310.3426  307.0824  304.2841  301.8823  299.8208  298.0514  296.5326  295.2291  294.1102]'/Tmax;
Th0 = [335.2388  328.4495  322.6229  317.6224  313.3307  309.6473  306.4859  303.7724  301.4434  299.4442]'/Tmax;
x0 = [Tc0;Th0];
u0 = [u10;u20;Tinc0]; %u, d

%% 
fode= Function('fode', {x,u,Tinc}, {HX_ODE_scaled(x,u,Tinc)});
r=rootfinder('r', 'newton', fode);
x02 = full(r(x0, u0(1:2),u0(3)));
x0 = x02;

%% Set points and weights
Th_sp = x0(end); % temperature
Fh_sp = 3;       % hot flow
wT = Tmax*1e2;   % weight for temperature
wF_val = 1e-2;   % weight for flow

%% Control discretization
N = 40;         % number of control intervals
st = 10;        % sampling time

%% Linearize model. xdot = Ax+Bu; y=Cx+Du
Ajac = Function('Ajac', {x,u,Tinc}, {jacobian(diff,x)});    % dx/df
Bjac = Function('Bjac', {x,u,Tinc}, {jacobian(diff,u)});    % du/df
Cjac = Function('Cjac', {x,u,Tinc}, {jacobian(xh(10),x)});
Djac = Function('Cjac', {x,u,Tinc}, {jacobian(xh(10),u)});

%% Substitute at thenominal point

Ajac =  full(Ajac(x0,u0(1:2),u0(3)));
Bjac =  full(Bjac(x0,u0(1:2),u0(3)));
Cjac =  full(Cjac(x0,u0(1:2),u0(3)));
Djac =  full(Djac(x0,u0(1:2),u0(3)));

%% Make a linear function
flin = Function('flin', {x,u}, {Ajac*(x-x0)+Bjac*(u-u0(1:2))});

%% casadi integrator
ode = struct('x',x,'p',[u;Tinc],'ode', f(x,u,Tinc));
op = struct('t0',0,'tf',st);
F = integrator('F','cvodes',ode,op); % cvodes = ODE systems; idas = DAE systems

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Linear MPC

% Collocation

% Degree of interpolating polynomial
d = 3;
% Get collocation points
tau_root = collocation_points(d,'legendre');
tau_root = [0 tau_root];
% Coefficients of the collocation equation
C = zeros(d+1,d+1);
% Coefficients of the continuity equation
D = zeros(d+1, 1);
% Coefficients of the quadrature function
B = zeros(d+1, 1);
% 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 QP
w={};           %input, slack and states
w0 = [];
lbw = [];
ubw = [];
J = 0;
g={};           %Constraints; including model
lbg = [];
ubg = [];


% Initial conditions

X0 = MX.sym('X0', 20);
w = {w{:}, X0};
lbw = [lbw; 0*ones(20,1)];
ubw = [ubw;  2*ones(20,1)];
w0 = [w0; x0];

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

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

U0 = MX.sym('U0', 2);
Uk_prev = MX.sym('Uk_prev', 2);

Uk_prev = U0;


% Formulate the QP
Xk = X0;
for k=0:N-1   % N= number of control intervals
    
    
    % New QP variable for the control
    Uk = MX.sym(['U_' num2str(k)],2,1);
    w = {w{:}, Uk};      %first uk that goes into the loop
    lbw = [lbw; 0 ; 0];  %input lower bound is 0;  for both u
    ubw = [ubw; 1 ; 1];  %input upper bound is 1;  for both u
    w0 = [w0;  [0.5;1]]; %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)], 20,1); %20 temperatures; 10 cold and 10 hot
        w = {w{:}, Xkj{j}};
        lbw = [lbw; 0*ones(20,1)];                               %lower bounds for temperatures
        ubw = [ubw;  2*ones(20,1)];                              %upper bounds for temperatures
        w0 =  [w0; x0];                                           %initial values for temperatures
    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
        flinj = flin(Xkj{j},Uk);%,Tinc);
        g = {g{:}, st*flinj - xp}; % slope matching constraint
        lbg = [lbg; zeros(20,1)];
        ubg = [ubg; zeros(20,1)];
        
        % Add contribution to the end state
        Xk_end = Xk_end + D(j+1)*Xkj{j};
        
    end
    
    % New QP variable for state at end of interval
    Xk = MX.sym(['X_' num2str(k+1)], 20,1);
    w = {w{:}, Xk};
    lbw = [lbw; 0*ones(20,1)];
    ubw = [ubw;  2*ones(20,1)];
    w0 = [w0; x0];
    
    % Add equality constraint
    g = {g{:}, Xk_end-Xk};  % shooting gap constraint
    lbg = [lbg; zeros(20,1)];
    ubg = [ubg; zeros(20,1)];
    
    Q = diag([wT wF]);
    
    obj= ([(Xk(20)-Th_sp);(Uk(2)*F_h_max - Fh_sp)]);
    J = J + obj'*Q*obj;% Uk'*R*Uk; % stage cost
    
    % add constraint for the rate of change
    g = {g{:}, Uk - Uk_prev};
    lbg = [lbg; -0.1; -0.1];
    ubg = [ubg; 0.1; 0.1];
    
    Uk_prev = MX.sym(['Uprev_' num2str(k+1)],2);
    Uk_prev = Uk;
    
end


% Create a QP solver

probs = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:}),'p',vertcat(X0_par,U0,Tinc,wF));

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

qp = probs;
nlp = probs;
%qp.g = substitute(qp.g,Tinc,Tinc0);
solver = nlpsol('solver', 'ipopt', nlp,options);
%solver = qpsol('solver', 'qpoases', qp);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the QP
t_f = 40;
x_0 = x0;


nx = 20; % # of states
nu = 2;  % # of inputs


progbar = waitbar(0,'Please wait...');
n_w = numel(w0);
u_in = u0(1:2); % u that goes into the plant
dist0 = Tinc0;
for i = 1:t_f
    i
    waitbar(i/t_f);    
    if i<2 | i >25
        dist = Tinc0+0;        % no change in cold fluid inlet temperature
    elseif i>=2 && i <10
        dist = Tinc0+2./Tmax;  % hot valve constraint should be active
    else i>=15 && i <20
        dist = Tinc0+6/Tmax;  % hot valve constraint is no longer active    
    end
    
    sol = solver('p',vertcat(x_0,u_in,dist,wF_val),'x0', w0, 'lbx', lbw, 'ubx', ubw,...
        'lbg', lbg, 'ubg', ubg); %open loop soln
    
    w_opt = full(sol.x);
    w0 = w_opt;
    u_in = w_opt(nx+1:nx+2)
    
    
    flag = solver.stats();
    flag.return_status
    
    u1_opt = w_opt(nx+1:nu+(d+1)*nx:n_w);
    u2_opt = w_opt(nx+2:nu+(d+1)*nx:n_w);
    T_opt = w_opt([20,nx+nu+20:nu+(d+1)*nx:n_w]);
        
    Fk = F('x0',x_0,'p',[u_in;dist]);  % this is the plant, change disturbance here.
    dist0 = dist;
    x_plant = full(Fk.xf);             % closed loop solution
    u_vec(i,:) = u_in';
    x_vec(i,:) = x_plant';             %closed loop solution
    x_0 = x_plant;
        
    figure(456)
    clf
    subplot(211)
    plot(u1_opt)
    hold all
    plot(u2_opt)
    subplot(212)
    plot(T_opt)    
    
end
close(progbar);

figure();
subplot(211)
plot(x_vec(:,20)*Tmax)
subplot(212)
stairs(u_vec)

return
tgrid = 0:h:t_f;
figure;
hold on
% plot(tgrid', x_vec(:,1), 'b')   % cold side temperature
plot(tgrid', x_vec(:,20), 'r')   % hot side tempeature
%legend('Tc','Th')
legend('Th')

hold off
figure
stairs(tgrid', [u_vec(:,1);nan], 'b-.')  % cold valve
hold on
stairs(tgrid', [u_vec(:,2);nan], 'r-.')  % hot valve
xlabel('t')
legend('zc','zh')

%save('outputMPC-Std.mat','tgrid','x_vec','u_vec','u0','Q','S_w')  % results from SRC sim
