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

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

addpath('./mat/');

freqs = logspace(1, 3, 1000);

% Load Plant

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

% Integral Force Feedback

bode(sys({'Vch', 'Vcv'}, {'Uch', 'Ucv'}));

Kppf = blkdiag(-10000/s, tf(0));

Kppf.InputName  = {'Vch', 'Vcv'};
Kppf.OutputName = {'Uch', 'Ucv'};

figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [dB]');
hold off;

% Phase
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G, freqs, 'Hz'))), 'k-');
set(gca,'xscale','log');
yticks(-360:90:180);
ylim([-360 0]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;

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

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

sys_cl = connect(sys, Kppf, inputs, outputs);

figure; bode(sys_cl({'Vph', 'Vpv'}, {'Uch', 'Ucv'}), sys({'Vph', 'Vpv'}, {'Uch', 'Ucv'}))