%Her er hovedprogrammet til MPC-regulatoren. Her initialiseres alle verdiene, 
%og MPC-algoritmene blir kalt opp.
clear all

%.........parametere........

global data
data.A=0.15^2*pi;%m^2
data.htot=0.6;%m
data.T=288;%K
 data.Cv2=0.00025/(0.5*sqrt(1.7e5+1000*9.8*0.3-1e5));%1.851344e-6;
 data.Cv4=0.04/(0.5*sqrt(1.7e5^2-1e5^2));%5.81914e-7;
 data.q1=0.00025;%m^3/s
 data.n3=0.04;%*8.3145*288/7.5e5;%0.0001277m^3/s 0.04 mol/s
 data.rho=1000;%kg/m*s^2
 data.g=9.8;%m/s^2
 data.P0=1e5;%pascal
 data.R=8.3145;%J/K*mol
 data.U20=0.5;
 data.U40=0.5;
 data.Vtot=data.htot*data.A;%m^3

%........Initialtilstander........
data.U20=0.5;
data.U40=0.5;
data.hl0=0.3;%m
data.Pg0=1.7e5;%pascal


x0 = [data.hl0;data.Pg0];%data.U20;data.U40];
y0 = [x0(1);x0(2)];

u0 = [data.U20;data.U40];


%Her lineariseres de dynamiske ligningene
[A,B,C,D,E,F,yp]=linearize(@trykktank_modell,0,x0,[],u0,0);

%De kontinuerlige matrisene gjøres om til diskret tilstands matriser
dt=0.2;
A=eye(size(A,1))+A*dt;
B=B*dt;


%...dimensjoner...
dimu=length(B(1,:));
dimx=length(A(1,:));
dimy=length(C(:,1));

u=[zeros(dimu,1)];
x=[zeros(dimx,1)];
y=[zeros(dimy,1)];

du=[zeros(dimu,1)];
dx=[zeros(dimx,1)];

%......Tuningsparametre....
%Siden de lineariserte matrisene er avviksmatriser, med steady-state
%verdier lik 0, innføres ur og yre for å vise de virkelige verdiene i 
%plottet. ur = u + 0.5 og yre = 0 + 0.3/1.7e5. Siden regulatoren ikke
%starter før i 2. tidsskritt, er de første verdiene satt, for å få
%finere grafer.

u=[0;0];
ur0=[0.5;0.5];
yre0=[0.3;1.7e5];
ur(:,1)=ur0;
xr0=[0.3;1.7e5];
yre(:,1)=yre0;
yre(:,2)=yre0;

%Reguleringshorisonten Z, pådragshorisonten N og reguleringsstarten kstart
Z=97;
kStart=2;
N=5;

x(:,kStart)=[0;0];
y(:,kStart)=C*x(:,kStart);

%Siden den lineariserte modellen benytter avviksvarible, blir referanse
%verdien til y lik 0. Dvs. [0;0] tilsvarer [0.3;1.7e5].
yr=[0;0];

Q=1*eye(dimy);
%Q=[1 0;0 1];
S=0*eye(dimu);
R=eye(dimu);

%Begrensninger, igjen referansen y=[0;0] tilsvarer [0.3;1.7e5] og u=[0;0] tilsvarer [0.5;0.5]
ymax=[0.3;1e6]; ymin=[-0.3;-1e5];
j1=1; j2=4;
umax=[0.5;0.5]; umin=[-0.5;-0.5];
Dumax=[0.1;0.1]; Dumin=[-0.1;-0.1];

%.....Algoritmen.....
options=optimset('LargeScale','off','display','off');
cputid=cputime;
%Her kalles selve algoritmen som løser qp problemet
MPCalg
cputid=cputime-cputid

%....Plotting av resultater....
figure(1)
subplot(2,1,1);
plot(0:k,yre(1,:),'g')
title('MPC-regulering');
xlabel('tid[s]');
ylabel('høyde[m]');

subplot(2,1,2);
plot(0:k,yre(2,:))
xlabel('tid[s]');
ylabel('trykk[Pa]');
figure(2)

subplot(2,1,1);
plot(1:k,ur(1,:))
title('Pådrag MPC-regulering');
xlabel('tid[s]')
 ylabel('Væskeventilåpning')
subplot(2,1,2)
plot(1:k,ur(2,:))
xlabel('tid[s]')
 ylabel('Gassventilåpning')