clc;
close all;
clear;
load ltiexamples
%%
t = 0:0.01:20 ;
ygcl1 = step(Gcl1 , t) ;
ygcl2 = step(Gcl2 , t) ;
ygcl3 = step(Gcl3 , t) ;
plot(t , ygcl1  , 'r*', t , ygcl2 , 'b-' , t , ygcl3  , 'ko', 'LineWidth' , 3) ;
grid on
xlabel('Time (Second)') ; ylabel('Amp') ;
title('Step Response of Gcl') ;
legend('Gcl1','Gcl2','Gcl3')
%% step information
StepInfo = stepinfo(Gcl1) ;
PO = StepInfo.Overshoot  ;
ST = StepInfo.SettlingTime  ;
RT = StepInfo.RiseTime ;
%% impulse
t = 0:0.01:20 ;
hgcl1 = impulse(Gcl1 , t) ;
hgcl2 = impulse(Gcl2 , t) ;
hgcl3 = impulse(Gcl3 , t) ;
plot(t , hgcl1  , 'r*', t , hgcl2 , 'b-' , t , hgcl3  , 'ko', 'LineWidth' , 3) ;
grid on
xlabel('Time (Second)') ; ylabel('Amp') ;
title('Impulse Response of Gcl') ;
legend('Gcl1','Gcl2','Gcl3');
%%
N = 50000;
w = logspace(-2 , 2 , N) ;
bode(Gcl1 , Gcl2 , Gcl3 , w) ;
Margin = allmargin(Gcl1) ;
GM = Margin.GainMargin
PM = Margin.PhaseMargin
PMFrequency = Margin.PMFrequency
GMFrequency = Margin.GMFrequency

%% nyquist
N = 5000 ;
w = logspace(-2 , 2 , N) ;
nyquist(Gcl1 , w) ;
%% initial reponse
system = ss(Gcl1) ;
% X0 = randn(4 , 1) ;
X0 = [1 2 3 5]' ;
t = 0:0.01:20 ;
InitialY = initial(system , X0 , t) ;
plot(t , InitialY  , 'r-', 'LineWidth' , 3) ;
grid on
xlabel('Time (Second)') ; ylabel('Amp') ;
title('initial Response of Gcl1') ;
legend('Gcl1');
%% lsim
t = 0:0.01:30 ;
% u = sin(t) ;
X0 = 0*[1 0 1 0]' ;
% u = 1.*(t >= 5).*(t <= 7)-1.*(t>7).*(t < 8) ;
u = sin(t).*(t >= 2*pi).*(t < 4*pi) ;
% u = 3+2*gensig('square' , 10 , 30 , 0.01) ;
y = lsim(system  , u , t , X0) ;
plot(t , u , t , y  , 'r-', 'LineWidth' , 3) ;
grid on
xlabel('Time (Second)') ; ylabel('Amp') ;
title('square wave Response of Gcl1') ;
legend('Input signal' , 'Output');

lsiminfo(y , t) ;
%% RLOCUS
system = Gcl1 ;
rlocus(system) ;
grid on
[K , Poles] =  rlocfind(system);
%%
tic
system = ss(Gcl1) ;
A = system.a ;
B = system.b ;
C = system.c ;
D = system.d ;
X0 = 0.01*randn(4,1) ; %% initial condition
Ts = 0.001  ;
t = 0:Ts:20 ;
u = 1.*(t >= 5).*(t <= 7)-1.*(t>7).*(t < 8) ;
[Yreal,t,Xreal] = lsim(system,u , t , X0) ;
N = numel(t) ;
Xapp = zeros(4 , N) ; %% X(1 , 10)
Xapp(: , 1) = X0 ;
Yapp = zeros(1 , N) ;
for i = 1:N
    Xapp(: , i+1) = Xapp(: , i) + Ts*(A * Xapp(: , i) + B*u(i));
    Yapp(i) = C*Xapp(: , i) ;
end
figure(1) ;
for j = 1:4
    subplot(2,2,j) ;
    plot(t , Xreal(: , j) , t , Xapp(j , 1:N) , 'LineWidth' , 3) ;
    xlabel('Time'); ylabel('Amp') ;
    title('Compare initial responses') ;
    legend('Xreal','Xapp') ;
end
figure(2) ;
subplot(211) ;
plot(t , Yreal , t , Yapp , 'LineWidth' , 3) ;
subplot(2,1,2) ;
plot(t , Yreal'-Yapp, 'LineWidth' , 3) ;
toc
%%
tic
system = ss(Gcl1) ;
A = system.a ;
B = system.b ;
C = system.c ;
D = system.d ;
X0 = 0.01*randn(4,1) ; %% initial condition
Ts = 0.01  ;
t = 0:Ts:20 ;
u = 1.*(t >= 5).*(t <= 7)-1.*(t>7).*(t < 8) ;
[Yreal,t,Xreal] = lsim(system,u , t , X0) ;
N = numel(t) ;
Xapp = zeros(4 , N) ; %% X(1 , 10)
Xapp(: , 1) = X0 ;
Yapp = zeros(1 , N) ;
f = @(x ,u) A*x + B*u ;
for i = 1:N
    k1 = Ts * f(Xapp(: , i) , u(i)) ;
    k2 = Ts * f(Xapp(: , i)+k1/2 , u(i)) ;
    k3 = Ts * f(Xapp(: , i)+k2/2 , u(i)) ;
    k4 = Ts * f(Xapp(: , i)+k3 , u(i)) ;
    Xapp(: , i+1) = Xapp(: , i) + (k1+2*k2+2*k3+k4)/6;
    Yapp(i) = C*Xapp(: , i) ;
end
figure(1) ;
for j = 1:4
    subplot(2,2,j) ;
    plot(t , Xreal(: , j) , t , Xapp(j , 1:N) , 'LineWidth' , 3) ;
    xlabel('Time'); ylabel('Amp') ;
    title('Compare initial responses') ;
    legend('Xreal','Xapp') ;
end
figure(2) ;
subplot(211) ;
plot(t , Yreal , t , Yapp , 'LineWidth' , 3) ;
subplot(2,1,2) ;
plot(t , Yreal'-Yapp, 'LineWidth' , 3) ;

toc

%% nonlinear simulation
sigma = 10 ;
rho = 28 ;
beta = 8/3 ;
f = @(X) [sigma*(X(2) - X(1)) ; X(1)*(rho-X(3))-X(2) ; X(1)*X(2)-beta*X(3)] ;
Ts = 0.01 ;
t = 0:Ts:200 ;
N = numel(t) ;
X0 = [0;1.1;0] ;
X = zeros(3, N) ; X(: , 1) = X0 ; 
for i = 1:N
    k1 = Ts * f(X(: , i) ) ;
    k2 = Ts * f(X(: , i)+k1/2 ) ;
    k3 = Ts * f(X(: , i)+k2/2 ) ;
    k4 = Ts * f(X(: , i)+k3 ) ;
    X(: , i+1) = X(: , i) + (k1+2*k2+2*k3+k4)/6;
end
plot(t ,X(: , 1:N) , 'LineWidth' , 3) ;
xlabel('Time'); ylabel('X') ;
title('intial reponse') ;
legend('X1' , 'X2' , 'X3') ;

figure(2) ;
plot3(X(1 , :) , X(2 , :) , X(3 , :)  , 'LineWidth' , 3 ) ;
grid on
xlabel('X1'); ylabel('X2') ; zlabel('X3') ;
title('X1-X2-X3  curve') ; 
