clear; clc;
Tref = 0.0008;
T_end = 1;
sd=0.01;
noiseON = 1;

% 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 = 1*[157;0];
% yplant=0; uplant=0; 
ydata=[]; udata=[]; wdata=[i]; tdata=[];
yplant=[0;-0.0001]; uplant=[0;0]; wdata1=[];
xplant=[0;0;-0.0001;0;0;0];set=[];xdata = [];
ymdata=[];noises=[];

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)];

% A = [Ap zeros(4,2);Cp*Ap eye(2,2)];
% B = [Bp;Cp*Bp];
% C = [zeros(2,4) eye(2,2)];
%%%%%%% 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.5;
% k=0;
% unit1 = zeros(Nu*m1,1);unit1(1:2,1) = 1;
% unit2 = zeros()
for t=N1:N2
    wdata = [wdata; w(:,1)];
    wdata1 = [wdata1; 2*w(:,1)];
end
  yk = yplant(:,1);
  xk = xplant(:,1);

for t=0:Tref:T_end
     %k = K+1;
   if noiseON
       noisevalue = randn;
   else
       noisevalue = 0;
   end
   noise = sd*noisevalue;   
   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
    uk = K1*wdata1 + K2*xk;
     set = [set, 2*w(:,1)];
else
    uk = K1*wdata + K2*xk;
    set = [set, w(:,1)];
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,:)];
    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('Speed'), 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('Iq'), xlabel('Time')