%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

addpath('./mat/');

freqs = logspace(0, 3, 1000);

% Load Plant

load('mat/plant.mat', 'sys', 'Gi', 'Zc', 'Ga', 'Gc', 'Gn', 'Gd');

% Diagonal Controller
% Using =SISOTOOL=, a diagonal controller is designed.
% The two SISO loop gains are shown in Fig. [[fig:diag_contr_loop_gain]].

Kh = -0.25598*(s+112)*(s^2 + 15.93*s + 6.686e06)/((s^2*(s+352.5)*(1+s/2/pi/2000)));
Kv = 10207*(s+55.15)*(s^2 + 17.45*s + 2.491e06)/(s^2*(s+491.2)*(s+7695));

K = blkdiag(Kh, Kv);
K.InputName  = {'Rh', 'Rv'};
K.OutputName = {'Uch', 'Ucv'};

figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(Kh*sys('Rh', 'Uch'), freqs, 'Hz'))), 'DisplayName', '$L_h = K_h G_{d,h}^{-1} G_{\frac{V_{p,h}}{\tilde{U}_{c,h}}} G_{i,h} $');
plot(freqs, abs(squeeze(freqresp(Kv*sys('Rv', 'Ucv'), freqs, 'Hz'))), 'DisplayName', '$L_v = K_v G_{d,v}^{-1} G_{\frac{V_{p,v}}{\tilde{U}_{c,v}}} G_{i,v} $');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [dB]');
hold off;
legend('location', 'northeast');

% Phase
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Kh*sys('Rh', 'Uch'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Kv*sys('Rv', 'Ucv'), freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+NAME: fig:diag_contr_loop_gain
% #+CAPTION: Loop Gain using the Decentralized Diagonal Controller ([[./figs/diag_contr_loop_gain.png][png]], [[./figs/diag_contr_loop_gain.pdf][pdf]])
% [[file:figs/diag_contr_loop_gain.png]]

% We then close the loop and we look at the transfer function from the Newport rotation signal to the beam angle (Fig. [[fig:diag_contr_effect_newport]]).

inputs  = {'Uch', 'Ucv', 'Unh', 'Unv'};
outputs = {'Vch', 'Vcv', 'Ich', 'Icv', 'Rh', 'Rv', 'Vph', 'Vpv'};

sys_cl = connect(sys, -K, inputs, outputs);

figure;
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(sys('Rh', 'Unh'), freqs, 'Hz'))), '-', 'DisplayName', 'OL - $R_h/U_{n,h}$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(sys_cl('Rh', 'Unh'), freqs, 'Hz'))), '--', 'DisplayName', 'CL - $R_h/U_{n,h}$');
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(sys('Rv', 'Unv'), freqs, 'Hz'))), '-', 'DisplayName', 'OL - $R_v/U_{n,v}$');
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(sys_cl('Rv', 'Unv'), freqs, 'Hz'))), '--', 'DisplayName', 'CL - $R_v/U_{n,v}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [dB]');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'southeast');

% Save the Controller

Kd = c2d(K, 1e-4, 'tustin');



% The diagonal controller is accessible [[./mat/K_diag.mat][here]].

save('mat/K_diag.mat', 'K', 'Kd');