%% Simulate the model and plot the simulation results
% Simulate the model
sim('ssc_hydraulic_actuator_digital_control')

% Extract simulation results from the Simscape logging nodes
simlog1 = simlog_ssc_hydraulic_actuator_digital_control;
pCylA1 = simlog1.Hydraulic_Actuator.Hydraulic_Cylinder.Chamber_A.A.p.series;
pCylB1 = simlog1.Hydraulic_Actuator.Hydraulic_Cylinder.Chamber_B.A.p.series;
aValve1 = simlog1.Hydraulic_Actuator.Custom_2_Way_Valve.Calculate_Orifice_Area.PS_Saturation.O.series;

% Plot simulation results 
h3_ssc_hydraulic_actuator_digital_control=figure('Name',...
    'ssc_hydraulic_actuator_digital_control',...
    'Outerposition', [1244 673 576 513]);

% Plot pressures
ah(1) = subplot(2,1,1);
plot(pCylA1.time,pCylA1.values,'LineWidth',6,'Color','b');
hold on
plot(pCylB1.time,pCylB1.values,'LineWidth',3,'Color','b');
hold off
grid on
ylabel('Pressure (Pa)');
title('Cylinder Pressures');
legend({'Side A','Side B'},'Location','Best');

% Plot valve activity
ah(2) = subplot(2,1,2);
plot(aValve1.time,aValve1.values,'LineWidth',3,'Color','b');
grid on
title('Valve Opening vs. Time');
ylabel('Opening (m^2)');
xlabel('Time (s)');
linkaxes(ah,'x');

% Create legend
legend('Original','Location','southeast');

%% Trim the model
assignin('base','ClosedLoop',1); % Close the feedback loop
[t,x,y] = sim('ssc_hydraulic_actuator_digital_control');
idx = find(t>2.5,1);                                            
X = x(idx,:); 
U = y(idx);

%% Linearize the model
assignin('base','ClosedLoop',0); % Break the feedback loop
[a,b,c,d]=linmod('ssc_hydraulic_actuator_digital_control',X,U);
assignin('base','ClosedLoop',1); % Close the feedback loop

%% Calculate and plot the frequency response
% Generate data
npts = 100;
w = logspace(-3,5,npts); 
G = zeros(1,npts);
for i=1:npts % Use negative feedback convention ( c:=-c, d:=-d)
    G(i) = (-c)*(2*pi*1i*w(i)*eye(size(a))-a)^-1*b +(-d);
end

% Create Bode plot figure
h4_ssc_hydraulic_actuator_digital_control=figure...
    ('Name','hydraulic_actuator_digital_control');

% Plot magnitude
ah(1) = subplot(2,1,1);
magline_h=semilogx(w,20*log10(abs(G)));
xlim(ah(1),[w(1),w(end)]);
grid on
ylabel('Magnitude (dB)');
title('Frequency Response (Hydraulic Actuator)');

% Plot phase
ah(2) = subplot(2,1,2);
phsline_h=semilogx(w,180/pi*unwrap(angle(G)));
set([magline_h,phsline_h],'LineWidth',1);
ylabel('Phase (deg)'); 
xlabel('Frequency (Hz)');
grid on
linkaxes(ah,'x');

% Create legend
legend('Original','Location','southwest');

%% Calculate and plot the poles
% Extract the poles
poles = eig(a);

% Extract the real and imaginary coordinates
X1 = real(poles); 
Y1 = imag(poles);

% Create complex plane plot figure
h5_ssc_hydraulic_actuator_digital_control = figure('Name',...
    'hydraulic_actuator_digital_control',...
    'OuterPosition',[668 150 1137 512]);

% Create axes
axes1 = axes('Parent',h5_ssc_hydraulic_actuator_digital_control,...
    'Position',[0.0315 0.112 0.932 0.815]);
hold(axes1,'on');

% Create title
title('Complex Plane Plot');

% Plot poles
plot(X1,Y1,'DisplayName','Original Poles','Marker','*',...
    'MarkerSize',3,'LineStyle','none');

% Limit and label axes
xlim(axes1,[-3000 500]);
xlabel({'Real','Axis'});
ylim(axes1,[-2000 2000]);
ylabel({'Imaginary',' Axis'});
set(axes1,'XAxisLocation','origin','YAxisLocation','origin');
box(axes1,'on');

% Create legend
legend(axes1,'show','Original','Location','southwest');

%% Print fast-pole values
eigs(a,6)

%% Eliminate Transport Delay block from the network
set_param('ssc_hydraulic_actuator_digital_control/Transport Delay',...
    'Commented','through')

%% Trim, Linearize, and update the Bode plot.
% Trim
assignin('base','ClosedLoop',1); % Close the feedback loop
[t,x,y] = sim('ssc_hydraulic_actuator_digital_control');
idx = find(t>2.5,1);                                            
X = x(idx,:); 
U = y(idx);

% Linearize
assignin('base','ClosedLoop',0); % Break the feedback loop
[a,b,c,d]=linmod('ssc_hydraulic_actuator_digital_control',X,U);
assignin('base','ClosedLoop',1); % Close the feedback loop

% Generate data 
npts = 100; w = logspace(-3,5,npts); G = zeros(1,npts);
for i=1:npts % Use negative feedback convention ( c:=-c, d:=-d)
    G(i) = (-c)*(2*pi*1i*w(i)*eye(size(a))-a)^-1*b +(-d);
end

% Update the Bode plot figure
figure(h4_ssc_hydraulic_actuator_digital_control);
hold on
ah(1) = subplot(2,1,1);
hold on
magline_h=semilogx(w,20*log10(abs(G)),'--');
xlim(ah(1),[w(1),w(end)]);
ah(2) = subplot(2,1,2);
hold on
phsline_h=semilogx(w,180/pi*unwrap(angle(G)),'--');
set([magline_h,phsline_h],'LineWidth',1);
linkaxes(ah,'x');
legend('Original','No Transport Delay','Location','southwest');

%% Calculate and plot the poles
% Extract the poles
poles = eig(a);

% Extract the real and imaginary coordinates
X1 = real(poles); 
Y1 = imag(poles);

% Update the complex plane plot figure
figure(h5_ssc_hydraulic_actuator_digital_control)
hold on
plot(X1,Y1,'DisplayName','No Transport Delay','Marker','d',...
    'MarkerSize',5,'LineStyle','none');

%% Plot the simulation results
% Get simulation results
simlog2 = simlog_ssc_hydraulic_actuator_digital_control;
pCylA = simlog2.Hydraulic_Actuator.Hydraulic_Cylinder.Chamber_A.A.p.series;
pCylB = simlog2.Hydraulic_Actuator.Hydraulic_Cylinder.Chamber_B.A.p.series;
aValve = simlog2.Hydraulic_Actuator.Custom_2_Way_Valve.Calculate_Orifice_Area.PS_Saturation.O.series;

% Update figure
figure(h3_ssc_hydraulic_actuator_digital_control)
hold on
ah(1) = subplot(2,1,1);
hold on
plot(pCylA.time,pCylA.values,'LineWidth',3,'Color','r','LineStyle','-');
hold on
plot(pCylB.time,pCylB.values,'LineWidth',1,'Color','r','LineStyle','-');
legend({'Side A','Side B'},'Location','Best');
hold off
ah(2)= subplot(2,1,2);
hold on
plot(aValve.time,aValve.values,'LineWidth',1,'Color','r','LineStyle','-');
hold off
legend({'Original','No Transport Delay'},'Location','southeast');

%% Zoom the figure
% Define the x-axis limits
xStart = 0;
xEnd = 10;
xZoomStart1 = 1.78;
xZoomEnd1 = 1.88;

% Zoom in
figure(h3_ssc_hydraulic_actuator_digital_control);
ah(2);
xlim([xZoomStart1, xZoomEnd1]);

%% Remove Compressibility
model = 'ssc_hydraulic_actuator_digital_control';
subsystem = 'Hydraulic Actuator';
composite_block = 'Hydraulic Cylinder';

CylA = [model,'/',subsystem,'/',composite_block,'/Chamber A'];
CylB = [model,'/',subsystem,'/',composite_block,'/Chamber B'];
set_param(CylA, 'Compressibility', '0')
set_param(CylB, 'Compressibility', '0')

%% Trim
assignin('base','ClosedLoop',1); % Close the feedback loop
[t,x,y] = sim('ssc_hydraulic_actuator_digital_control');
idx = find(t>2.5,1);                                            
X = x(idx,:); U = y(idx);

%% Linearize
assignin('base','ClosedLoop',0); % Break the feedback loop
[a,b,c,d]=linmod('ssc_hydraulic_actuator_digital_control',X,U);
assignin('base','ClosedLoop',1); % Close the feedback loop

%% Update Bode plot
% Generate data
npts = 100; 
w = logspace(-3,5,npts); 
G = zeros(1,npts);
for i=1:npts % Use negative feedback convention ( c:=-c, d:=-d)
    G(i) = (-c)*(2*pi*1i*w(i)*eye(size(a))-a)^-1*b +(-d);
end

figure(h4_ssc_hydraulic_actuator_digital_control);
hold on
ah(1) = subplot(2,1,1);
hold on
magline_h=semilogx(w,20*log10(abs(G)));
xlim(ah(1),[w(1),w(end)]);
% figure(h4_ssc_hydraulic_actuator_digital_control);
% hold on

ah(2) = subplot(2,1,2);
hold on
phsline_h=semilogx(w,180/pi*unwrap(angle(G)));
set([magline_h,phsline_h],'LineWidth',1);
linkaxes(ah,'x');
legend('Original','No Transport Delay','No Compressibility',...
    'Location','southwest');

%% Update complex plane plot
poles = eig(a);

figure(h5_ssc_hydraulic_actuator_digital_control)
hold on
X1 = real(poles); 
Y1 = imag(poles);
plot(X1,Y1,'DisplayName','No Compressibility','Marker','d',...
    'MarkerSize',8,'LineStyle','none');

%% Print the pole values
eigs(a,2)

%% Print simulation results
% Get simulation results
simlog3 = simlog_ssc_hydraulic_actuator_digital_control;
pCylA = simlog3.Hydraulic_Actuator.Hydraulic_Cylinder.Chamber_A.A.p.series;
pCylB = simlog3.Hydraulic_Actuator.Hydraulic_Cylinder.Chamber_B.A.p.series;
aValve = simlog3.Hydraulic_Actuator.Custom_2_Way_Valve.Calculate_Orifice_Area.PS_Saturation.O.series;

% Update results
figure(h3_ssc_hydraulic_actuator_digital_control)
hold on
ah(1)= subplot(2,1,1);
hold on
plot(pCylA.time,pCylA.values,'LineWidth',1,'Color','y','LineStyle','--');
hold on
plot(pCylB.time,pCylB.values,'LineWidth',1,'Color','y','LineStyle','--');
legend({'Side A','Side B'},'Location','Best');
hold off
ah(2)= subplot(2,1,2);
hold on
plot(aValve.time,aValve.values,'LineWidth',1,'LineStyle','--');
legend({'Original','No Transport Delay','Compressibility'},'Location','southeast');
hold off

% Zoom out
figure(h3_ssc_hydraulic_actuator_digital_control);
ah(2);
xlim([xStart, xEnd]);

%% Zoom the figure
% Zoom in
figure(h3_ssc_hydraulic_actuator_digital_control);
ah(2);
xlim([xZoomStart1, xZoomEnd1]);