% Main script to simulate static example from stability section in the
% transformation paper
% 12.07.2021
clc;close all; clear;
set(0,'DefaultTextFontName','Times','DefaultTextFontSize',18,...
    'DefaultAxesFontName','Times','DefaultAxesFontSize',18,...
    'DefaultLineLineWidth',4)

indigo   = [0.2930 0 0.5078];
cerulean = [0.1094    0.6641    0.8359];
greencg = [83,188,102]/256;
bluecg  = [44,144,195]/256;
movcg   = [198,102,177]/256;
cherry   = [222, 58, 58]/256;


black = [0 0 0];
pos   = [100 100 400 300]; % figure position on screen
yl    = 3.2; % limits for y axis

% v = g*u with g(s) ~ -exp(-3*s)/(4*s+1), from step response
Kc    = -1*4/(3+3);      % proportional gain for cascade of v implementation
tauI  = 4*(3+3);                                             % integral time
%KI    = -1/16;
KI = Kc/tauI;
%% Step changes
tsim = 50;
td = 1; tv = 1;                                     % time for step changes
df = 0;                                                 % final value for d
vf = 1;                                                 % final value for v


%% Simulate
sim('simExamplez0')                                  % exact implementation
sim('simExamplez0_cascade')                   % cascade of v implementation

%% Plot
figure();%subplot(2,3,1)
plot(time,y,'Color',cherry);hold on;plot(timec,yc,'Color',cerulean);plot(time,vs,'k--')
ylabel('y');xlabel('Time');
legend('Model-based','Feedback-based','Setpoint','Location','best'); 
legend boxoff
ylim([-yl yl]); xlim([0 tsim])
set(gcf,'Position',pos); figname = strcat('z0_y_vf_', num2str(vf));
print('-depsc2','-r600',figname)

figure();%subplot(2,3,3)
plot(time,w,'Color',cherry);hold on;plot(timec,wc,'Color',cerulean)
ylabel('w'); xlabel('Time'); legend('Model-based','Feedback-based','Location','best');
legend boxoff
ylim([-yl yl]); xlim([0 tsim])
set(gcf,'Position',pos); figname = strcat('z0_w_vf_', num2str(vf));
print('-depsc2','-r600',figname)

figure();%subplot(2,3,2)
plot(time,u,'Color',cherry);hold on;plot(timec,uc,'Color',cerulean)
ylabel('u'); xlabel('Time');legend('Model-based','Feedback-based','Location','best'); 
legend boxoff
ylim([-yl yl]); xlim([0 tsim])
set(gcf,'Position',pos); figname = strcat('z0_u_vf_', num2str(vf));
print('-depsc2','-r600',figname)

figure();%subplot(2,3,4)
plot(time,vs,'Color',cherry);hold on;plot(timec,vc,'Color',cerulean)
ylabel('v'); xlabel('Time'); legend('Model-based','Feedback-based','Location','best');
legend boxoff
ylim([-yl yl]); xlim([0 tsim])
set(gcf,'Position',pos); figname = strcat('z0_v_vf_', num2str(vf));
print('-depsc2','-r600',figname)

figure();%subplot(2,3,5)
plot(time,d,'k');ylabel('d') ; xlabel('Time'); ylim([-yl yl]); xlim([0 tsim])
set(gcf,'Position',pos); figname = strcat('z0_d_vf_', num2str(vf));
print('-depsc2','-r600',figname)

