%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                           The start-up file.                            %
%                                                                         %
%  Survey of influence of iterations number on regulation quality.        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all
warning off
% Model parameters
global uLmin uLmax uRmin uRmax aLV aRV sLV sRV suL suR auL auR
global alfaLH alfaRH alfaLL alfaRL sLH sRH sLL sRL SL SR 
global betaH betaL gamaH gamaL s0H s0L V0 H0
global pA rok g m R cp T



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                  SETTING                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ts=5;                               %Sampling time
hor=20;                             %Horizon length
tsim=4200;                          %Simulation time
pA=101325;
%-------------------------------------------------------------------------%
pav=3;                              %Number of manipulated variables
prv=2;                              %Number of controlled outputs
Cc=eye(4,6);                        %Filter matrix (for states occuring in constraints)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
parametry;                          %HPS parameters loading 
options=odeset('RelTol',1e-4);      %Simulation setting
crw2;                                %Creating of a vector of desired system outputs
%-------------------------------------------------------------------------%
%                                 Linearization                           %
%-------------------------------------------------------------------------%
%Liquid mass flow in lin. point
pplin=30000;
oLlin=0.2892;
oRlin=0.3165;
%-------------------------------------------------------------------------%
%                                Initialization                           %
%-------------------------------------------------------------------------%
pp0=30000;
hLL0=0.3;
hRL0=0.05;
QL0=alfaLL*sLL*rok*sqrt(2*g*hLL0);
QR0=alfaRL*sRL*rok*sqrt(2*g*hRL0);
oL0=(QL0/sqrt(2*rok*pp0)-auL*suL)/(aLV*sLV);
oR0=(QR0/sqrt(2*rok*pp0)-auR*suR)/(aRV*sRV);
hLH0=(QL0/(alfaLH*sLH*rok*sqrt(2*g)))^2;
hRH0=(QR0/(alfaRH*sRH*rok*sqrt(2*g)))^2;

state;                              %Creating of linear discrete state-space model


%-------------------------------------------------------------------------%
%                             Weighting Matrices                          %
%-------------------------------------------------------------------------%

vahc=0.5;
vahQ=0.5;

rvh=30;
qoLh=1e-5;
qoRh=1e-5;
qpph=1e-8;

rv=rvh*(1-vahc);
qppv=qpph*vahQ*vahc;
qoLv=qoLh*(1-vahQ)*vahc;
qoRv=qoRh*(1-vahQ)*vahc;

Rv=eye(prv*hor)*rv;                %Output error penalization
Qv=zeros(pav*hor);                  %Input changes penalization
Q2=Qv;

for i=1:hor
    for j=1:hor
        Qv(1+(i-1)*pav:i*pav,1+(i-1)*pav:i*pav)=[qoLv 0 0;0 qoRv 0;0 0 qppv];
    end 
end
Q2(1+(i-1)*pav:i*pav,1+(i-1)*pav:i*pav)=[0 0 0;0 0 0;0 0 1e-10];

for i=1:hor
    for j=1:hor
        if i==j
            Ev(1+(i-1)*pav:i*pav,1+(j-1)*pav:j*pav)=eye(pav);
        elseif (i==(j+1))
            Ev(1+(i-1)*pav:i*pav,1+(j-1)*pav:j*pav)=-eye(pav);
        end
    end
end
Ev2=Ev;
Ev2(1:pav,1:pav)=zeros(pav);

%--------------------------Controllers synthesis--------------------------
Hi=zeros(prv*hor,pav*hor);
for k=1:hor
    pHk=C*Ad^(k-1)*Bd;
    for j=1:hor-k+1        
        Hi((k+j-2)*prv+1:(k+j-1)*prv,(j-1)*pav+1:j*pav)=pHk;
    end
end

tint=0:Ts:(hor-1)*Ts;
M=(Hi'*Rv*Hi+Ev'*Qv*Ev+Q2);

%------------------------ Constraints definition --------------------------
nsc=size(Cc,1);     % Number of states occuring in constraints
S2x=zeros(nsc*hor,pav*hor);

for k=1:hor
    for j=1:hor-k+1        
        S2x((k+j-2)*nsc+1:(k+j-1)*nsc,(j-1)*pav+1:j*pav)=Cc*Ad^(k-1)*Bd; % Const. component
    end
end
umax=[1;1;6e4];
umin=[0;0;0];
xmax=[0.3;0.3;0.3;0.3];
xmin=[0.05;0.05;0.05;0.05];

ilin=[oLlin;oRlin;pplin];       % Point of linearization on system inputs

S=[-S2x;S2x;-eye(size(S2x,2));eye(size(S2x,2))]';
s=zeros(2*hor*nsc+2*hor*pav,1);

for i=1:hor
s((i-1)*nsc+1:nsc*i,1)=-xmax;
end
for i=1:hor
s((i-1)*nsc+1+nsc*hor:nsc*i+nsc*hor,1)=xmin;
end
for i=1:hor
s((i-1)*pav+1+2*nsc*hor:pav*i+2*nsc*hor,1)=-umax;
end
for i=1:hor
s((i-1)*pav+1+2*nsc*hor+pav*hor:pav*i+2*nsc*hor+pav*hor,1)=umin;
end

xlin=[hLHlin;hRHlin;hLLlin;hRLlin;pA;pA];
xc=eye(4,6);

u=zeros(pav*hor,1);                % Vector for constraints corection
uoptv=u;

for i=1:hor
    u(1+(i-1)*pav:i*pav)=[oL0;oR0;pp0];        % Working vector of u0
    uoptv(1+(i-1)*pav:i*pav)=[0;0;0];
end

%------------------- Parameters of mathematical model ---------------------
const=[aLV;aRV;sLV;sRV;suL;suR;auL;auR;alfaLH;alfaRH;alfaLL;alfaRL;sLH;sRH;sLL;sRL;SL;SR;
betaH;betaL;gamaH;gamaL;s0H;s0L;V0;H0;pA;rok;g;m;R;cp;T];

% Nonlinear MPC controller parameters
parn.M = M;                         % matrix of controller                                
parn.H = Hi;                        % forced output prediction matrix                               
parn.Rv= Rv;                        % weighting matrix (prediction error)
parn.Q2= Q2;                        % weighting matrix (steady state input deviation)
parn.Q = Qv;                        % weighting matrix (time difference)
parn.Ev= Ev;                        % weighting matrix (time difference)
parn.Ev2= Ev2;                      % weighting matrix (time difference)
parn.Ts= Ts;                        % samplingtime  
parn.nam=@ hpsnr;                   % nonlinear math. model file name 
parn.C = C;                         % state matrix
parn.uopt=uoptv;                    % vector of optimal inputs (nu x 1) 
parn.ulin=ilin;                     % inputs linearization points (vector)
parn.xlin=xlin;                     % states linearization points (vector)
parn.xc= xc;                        % States which occure in constraints
parn.S = S;                         % Constraints - left side
parn.s = s;                         %               right side

%------------------------------ Nonlinear ---------------------------------
tt=floor(tsim/Ts);
Cjx=eye(4,6);

for ni=1:5                          % maximum number of iterations
    parn.mni=ni;                    
    x=[hLH0;hRH0;hLL0;hRL0;pA;pA];
    Jep=0;
    Jcp=0;
    uhist=u(1:pav);
    X=zeros(6,tt+1);
    for i=1:tt
        X(:,i)=x;    
        u=NMPC_with_cons(x,u,w(1+(i-1)*prv:hor*prv+(i-1)*prv),parn,const);
        uhist(1+i*pav:(i+1)*pav)=u(1:pav);
        ua=[(uLmax-uLmin);(uRmax-uRmin);1].*u(1:pav)+[uLmin;uRmin;0];
        y0=[x;ua];    
        [time,Y]=ode45('hps',[0 Ts],y0,options);
        x=Y(end,1:length(x))';
        Jep=Jep+[1 1]*abs(w(1+(i-1)*2:i*2)-C*x);
        Jcp=Jcp+(xmin>Cjx*x)'*abs(xmin-Cjx*x);
        Jcp=Jcp+(xmax<Cjx*x)'*abs(xmax-Cjx*x);
    end
    
    Je(ni)=Jep;
    Jc(ni)=Jcp;
    X(:,i+1)=x;

    figure(1)
    xres=[0.01 0.01 0.01 0.01 1000 1000];
    clf
    t=0:Ts:floor(tsim/Ts)*Ts;
    for s=1:6
        subplot(3,2,s)
        hold on
        plot(t,X(s,:)/xres(s),'r')
        if s<5
            plot ([0 tsim],[xmax(s) xmax(s)]/xres(s),'--')
            plot ([0 tsim],[xmin(s) xmin(s)]/xres(s),'--')
            minpl=min([min(X(s,:)) xmin(s)]/xres(s));
            maxpl=max([max(X(s,:)) xmax(s)]/xres(s));
        else
            minpl=min(X(s,:))/xres(s);
            maxpl=max(X(s,:))/xres(s);
        end
        xlim([0 tsim])
        bord=(maxpl-minpl)*0.05;
        ylim([minpl-bord maxpl+bord])
        xlabel('{\itt} [s]')
        switch s
            case 1
                ylabel('{\ith_{LH}}({\itt})  [cm]')
            case 2
                ylabel('{\ith_{RH}}({\itt})  [cm]')
            case 3
                ylabel('{\ith_{LL}}({\itt})  [cm]')
                plot (whLL(1,:),whLL(2,:)/xres(s),'g')
            case 4
                ylabel('{\ith_{RL}}({\itt})  [cm]')
                plot (whRL(1,:),whRL(2,:)/xres(s),'g')
            case 5
                ylabel('{\itp_{H}}({\itt})  [kPa]')
            case 6
                ylabel('{\itp_{L}}({\itt})  [kPa]')
        end
        hold off
    end

    uplot=zeros(floor(tsim/Ts)+1,pav);
    uplot(1,:)=[oL0;oR0;pp0];
    for i=1:floor(tsim/Ts)
        uplot(i+1,:)=uhist(1+(i-1)*pav:i*pav);
    end

    figure(2)
    clf
    ures=[1 1 1000];
    for s=1:3
        subplot(3,1,s)
        hold on
        plot (t,uplot(:,s)/ures(s),'r')
        plot ([0 tsim],[umax(s) umax(s)]/ures(s),'--')
        plot ([0 tsim],[umin(s) umin(s)]/ures(s),'--')
        minpl=min([min(uplot(:,s)) umin(s)])/ures(s);
        maxpl=max([max(uplot(:,s)) umax(s)])/ures(s);
        xlim([0 tsim])
        bord=(maxpl-minpl)*0.05;
        ylim([minpl-bord maxpl+bord])
        xlabel('{\itt} [s]')
        switch s
            case 1
                ylabel('{\ito_{L}}({\itt})  [-]')
            case 2
                ylabel('{\ito_{R}}({\itt})  [-]')
            case 3
                ylabel('{\itw_{p_p}}({\itt})  [kPa]')
        end    
        hold off
    end
end

figure (3)
plot(1:ni,Je)
xlabel('{\itni} [-]')
xlim([1 ni])
ylabel('{\it\eta_{r}} [m]')

figure (4)
plot(1:ni,Jc)
ylabel('{\it\eta_{c}} [m]')
xlabel('{\itni} [-]')
xlim([1 ni])