clc
clear all
close all

 s=tf('s');

 %using a 2states empirical model
 aux=open('G_exp.mat');
 sys_ss=ss(aux.Ge);

%Open loop plant dynamics
Ap=sys_ss.a;
Bp=sys_ss.b(:,1);
Cm=sys_ss.c(1,:);
Cr=sys_ss.c(1,:);
Dp=sys_ss.d(1:1);

I=eye(size(Ap));
% G=minreal(sys_ss.c(1,:)*inv(s*I-sys_ss.a)*sys_ss.b);



nx=length(Ap);
ny=size(Cm,1);
nu=size(Bp,2);

%augmenting the system to get integral action
Aaug=[Ap zeros(nx,1); -Cr  zeros()];
Baug=[Bp;0];
Bcmd=[zeros(1,nx) 1]';
%Assuming that P_bottom and P_top are measured or estimated elsewhere
Caug=[[Cm zeros(ny,1)]; [zeros(1,nx) 1]];
Daug=[Dp;0];



%% step 1: design a LQR controller to obtain desired robustness properties

    q_vec= linspace(15,45,30);
%   q_vec=0.1; %z=5%
% q_vec=1; % z=10%
%   q_vec=12;
for i=1:length(q_vec)
    
    q_i=q_vec(i);
    
      Q=diag([0 0  1]);
      Q(end)=q_i*Q(end);
      R=1*eye(1,1);
    
    

    Klqr=lqr(Aaug,Baug,Q,R);
    I=eye(size(Aaug));
    

    Llqr=minreal(Klqr*inv(s*I-Aaug)*Baug);
    
    %reference system
    Aref=Aaug-Baug*Klqr;
    Bref=[zeros(1,nx) 1]';
    Cref=Caug;
    
  
    Ccl=[Cref; %ym
        -Klqr   %ucontrol
        -Klqr*Aref %udot
        ];
    
    Mcl=ss(Aref,Bref,Ccl,0);
    [Gm,Pm,Wgm,Wpm] = margin(Llqr);
    auxmargin=allmargin(Llqr);
    [yall,t]=step(Mcl);
   
    y=yall(:,1);
    resp   = stepinfo(y,t,'RiseTimeLimits',[0 0.63],'SettlingTimeThreshold',0.05);% pump pressure the controlled variable
    taur   = resp.RiseTime;
    
    
    
    u=yall(:,end-1);
    udot=yall(:,end);
        
    ISE(i)=trapz(t,(1-y).^2);
    Gmv(i) =Gm;
    Pmv(i)=Pm;
    Dmv(i)=Pm*pi/180/Wpm;
    Mtv(i)=getPeakGain( minreal(Llqr/(1+Llqr)));
    Msv(i)=getPeakGain( minreal(1/(1+Llqr)),1e-16);
    udotvec(i)=max(abs(udot));
    umaxvec(i)=max(abs(u));
    RiseTime(i)=taur;
    
    
    
end
% 
% load Cimc
% load Kloop.mat


% Lvec={Llqr,Limc,Loop};
% name_vec={'Llqr','Limc','Loop'};
%  compareControllers( Lvec,name_vec ,0)
% 

% [y,t]=step(Limc/(1+Limc));
% ISE_imc=trapz(t,(1-y).^2);
% Msimc=getPeakGain(minreal(1/(1+Limc)));
% Mtimc=getPeakGain(minreal(Limc/(1+Limc)));
% [Gmimc,Pmimc,Wgm,Wpm] = margin(Limc);
% DMimc=Pmimc*pi/180/Wpm;

% figure(10)
% step(Mcl(1,1),Limc/(1+Limc),Loop/(1+Loop))

figure(3)
subplot(211)
plot(q_vec,udotvec)
ylabel('Udot Max')
subplot(212)
plot(q_vec,umaxvec)
ylabel('U Max')
xlabel('q_vec')
% 
% 
figure(1)
%  plot(q_vec,ISE)
 plot(q_vec,RiseTime)
% % hold on
% % plot([q_vec(1) q_vec(end)],'k--')
 title('Performance: lower is better')
 xlabel('q_vec')
 ylabel('RiseTime')
% 
figure(2)
plot(q_vec,Gmv)
title('Robustness: higher is better')
xlabel('q_vec')
ylabel('GM')
% 
% % figure(3)
% % semilogx(q_vec,Pmv)
% % title('Robustness: higher is better')
% % xlabel('q_vec')
% % ylabel('PM')
% 
figure(4)
plot(q_vec,Dmv)
% hold on
% plot([q_vec(1) q_vec(end)],'k--')
title('Robustness: higher is better')
xlabel('q_vec')
ylabel('DM')
% 
% figure(5)
% semilogx(q_vec,Mtv)
% hold on
% plot([q_vec(1) q_vec(end)],[Mtimc Mtimc],'k--')
% title('Robustness: lower is better')
% xlabel('q_vec')
% ylabel('Mt')
% 
% % figure(6)
% % semilogx(q_vec,Msv)
% % title('Robustness: lower is better')
% % xlabel('q_vec')
% % ylabel('Ms')
% 
tilefigs

%%


%reference system
Aref=Aaug-Baug*Klqr;
Bref=[zeros(1,nx) 1]';
Cref=Caug;

%reference model
Mcl=ss(Aref,Bref,Cref,0);



%% observer design -squaring up stage


%  zeroplace=  [-1]; %only used for pole placement place() 
%  Bsqr=getAugSquareUp(Aaug,Baug,[Caug ;1 1 -1],zeroplace,'lqr');
%   Bsqr=10+20*rand(3,2);
 Bsqr=1*[ 25.0253   23.9815
   15.1019   27.8181
   20.1191   29.1858];

sys_aug=ss(Aaug,Bsqr,Caug,0);
display('-----zeros original coordinates----')
zervec = tzero(minreal(sys_aug))
if zervec>0
    error('transmission zeros must be stable')
end

%%Compute Kalman gain


Q0=Q(1:end,1:end);
R0=2*eye(size(Caug,1));

 vvec=logspace(-1,1,10);
 vvec=linspace(0.2,.45,10);
  vvec=0.2833;
for v=vvec
%  v=5e0;
v
 Qv=Q0+(v+1)/v*Bsqr*(Bsqr');

Rv=v/(v+1)*R0;

%========using ricatti equation======
[Lv,Pv] = lqe(Aaug,eye(size(Aaug)),Caug,Qv,Rv);
% 
% % KF solution check%
% % important to check this when using LTR
% kf_eq = Aaug*Pv + Pv*Aaug' + Qv - Pv*Caug'*inv(Rv)*Caug*Pv;
% norm_kf_eq = norm(kf_eq);
% disp(['KF FARE Solution Tolerance is: ', num2str(norm_kf_eq)]);

%========using place======
% poles_ob=10*[  -0.1371 + 0.2078i
%   -0.1371 - 0.2078i
%   -0.1820 + 0.0000i
%    -1
%    -2];
% Lv = place(Aaug',Caug',poles_ob);
% Lv=Lv';
% eig(Aref-Lv*Cref)


% LTR Controller
Ac = Aref-Lv*Caug;
Bc1 = Lv;
Bc2=Bref;
Cc = -Klqr;
Dc1 = [0. 0. ];
Dc2 = 0.;


%SS model of loop gain at the plant input
Ain = [ Aaug 0.*Baug*Cc;  Bc1*Caug Ac];
Bin = [ Baug; Bc1*Daug];
Cin = -[ Dc1*Caug Cc];%change sign for loop gain
Din = -[ Dc1*Daug];

Lltr = minreal(ss(Ain,Bin,Cin,Din));
wrange=logspace(-5,2,200);
figure
bode(Llqr,Lltr,wrange)
drawnow
grid on
hold on
legend('LQR','LTR')
hold on
end


% write closed loop observer LTI model
Aob=Aref-Lv*Caug;
Bob=[Bref Lv];
%outputs estimation error: ey=Cxest-ym
Cob=[Caug];
Dob=[zeros(ny+1,1)   -eye(ny+1)];
sysobserver=balred(ss(Aob,Bob,Cob,Dob),3);
sysobserver.InputName={'ref','P1','Inte'};
sysobserver.OutputName={'e1','e3'};
display('--------Observer Eigenvalues------')
eig(Aob)
display('--------Ref model Eigenvalues------')
eig(Aref)

 %%                                                                  



%constants for the adaptive law
R0=R0;
S=[1 0];
auxM=Baug'*Caug'*R0^(-0.5);
[U,Sig,V]=svd(auxM);
W=(U*V)';
F=R0^(-0.5)*W*S';
% F=[-1 0];
% F=inv(Pv)*Baug

% save('adaptive_systemNewZ10_1MEAS', 'Klqr', 'sysobserver','F','thetamax','Gammau','Gamma','gammavec');
%%
%  callLinearSim
%%
AdaptOn=1;
callNonLinearSim


