% Mpc Controller for Flywheel with constraints
%% By Chen Shauixun
%% Power Group

%%%%%%%%%%      PROBLEM DEFINITION:    %%%%%%%%%%

% Define time-constant of reference trajectory Tref:
clear all; 
clc;
warning off all;
Tref = 0.0008;
T_end = 1;
sd=0.01;

% Define sampling interval Ts (default Tref/10):
if Tref == 0,
  Ts = 1;
else
  Ts = Tref;
end

K1 = 2.90e6; K2 = 455.44;Kt = 3.028;
m = 4.09; J = 0.0158;
w = [157;0];
ymin=[-110*pi;-0.01];
ymax=[110*pi;0.01];
ldu=[-5;-5];
udu=[5;5];

% yplant=0; uplant=0; 
ydata=[]; udata=[]; wdata=[]; tdata=[];
yplant=[0;-0.0001]; uplant=[0;0]; wdata1=[];wdata2=[];
xplant=[0;0;-0.0001;0;0;0];set=[];xdata = [];
ymdata=[];noises=[];wdata=[];

Ap = [0 1 0 0;0 0 0 0;0 0 0 1; 0 0 K1/m 0];
Bp = [0 0;Kt/J 0;0 0;0 K2/m];
Cp = [0 1 0 0;0 0 1 0];

%%%%%% Get the size of A,B,C %%%%%%%
% [n,n] = size(Ap);
% [n,m1] = size(Bp);
% [p,n] = size(Cp);

% % State Space Model 7
% A1 = [Ap Bp;zeros(2,4) eye(2,2)];
% B1 = [Bp;Cp*Bp];
% C1 = [Cp zeros(2,2)];
% % State Space Model 8
A1 = [Ap zeros(4,2);Cp eye(2,2)];
B1 = [Bp;zeros(2,2)];
C1 = [Cp eye(2,2)];
% % State Space Model 9
% A1 = [Ap zeros(4,2);Cp*Ap eye(2,2)];
% B1 = [Bp;Cp*Bp];
% C1 = [zeros(2,4) eye(2,2)];

%%%%%%% model 7 %%%%%%%%%%%%%%%%%%%%
% A1 = [Ap Bp;[0 0 0 0;0 0 0 0] [1 0;0 1]];
% B1 = [Bp;Cp*Bp];
% C1 = [Cp [0 0;0 0]];
% A = [Ap Bp;[0 0 0 0;0 0 0 0] [1 0;0 1]];
% B = [Bp;Cp*Bp];
% C = [Cp [0 0;0 0]];
%%%%%%% Ts=0.0008 %%%%%%%%%
G = ss(A1,B1,C1,0);
G1 =c2d(G,Tref);
[A,B,C,D] = ssdata(G1);

% Define control horizon :
N1 = 1;
N2 = 20; %10;
Nu = 1;
lamda = 0.1;
% k=0;
% unit1 = zeros(Nu*m1,1);unit1(1:2,1) = 1;
% unit2 = zeros()
for t=N1:N2
    wdata1 = [wdata1; w(:,1)];
    wdata2 = [wdata2; 2*w(:,1)];
end

  yk = yplant(:,1);
  xk = xplant(:,1);
        
for t=0:Tref:T_end
     %k = K+1;
   noise = sd*randn;
   ykm = (1+noise)*yk;
   xk(2:3,1)=(1+noise)*xk(2:3,1);
    
    [Phi, G] = predictions(A,B,C,N1,N2,Nu);
    [K1,K2] = MPC2(Phi,G,lamda);
    
    
if t>0.5
    wdata = wdata2;
     set = [set, 2*w(:,1)];
else
    wdata = wdata1;
    set = [set, w(:,1)];
end
    uk = K1*wdata + K2*xk;
if ~((uk(1,1)<2&&uk(1,1)>-2)&&(uk(2,1)<2&&uk(2,1)>-2))
    [H,f,Omega,b]=form_constraints(A,B,C,N1,N2,Nu,xk,wdata,lamda,ymax,ymin);
%     uk=quadprog(H,f,Omega,b,[],[],ldu,udu);
 uk=quadprog(H,f,Omega,b);
end 


%     tdata = [tdata;k-1]; wdata = [wdata; w];
%     ydata = [ydata;yk]; udata = [udata; uk];    
    
    tdata = [tdata,t]; %set = [set, w(:,1)];
    ydata = [ydata,yk]; udata = [udata, uk(1:2,1)];
    xdata = [xdata,xk]; ymdata = [ymdata,ykm];
    noises = [noises,noise];

    
    
    xk = A*xk + B*uk(1:2,1);
    yk = C*xk;
      
end

% subplot(411),plot(tdata, ydata(1,:)), ylabel('Output'), xlabel('Sample')
% subplot(412),plot(tdata, ydata(2,:)), ylabel('Control'), xlabel('Sample')
% subplot(413),plot(tdata, xdata(5,:)),ylabel('input1'), xlabel('Sample')
% subplot(414),plot(tdata, xdata(6,:)),ylabel('input2'), xlabel('Sample')
subplot(311),plot(tdata, ydata(1,:),'-',tdata,ymdata(1,:),':',tdata,set(1,:),'r'), ylabel('sSpeed'), xlabel('Time')
subplot(312),plot(tdata, ydata(2,:),tdata,ymdata(2,:),tdata,set(2,:),'r'), ylabel('Displacement'), xlabel('Time')
subplot(313),plot(tdata, xdata(5,:)),ylabel('Input(Iq)'), xlabel('Sample')