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

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

open('active_damping/matlab/sim_nass_active_damping.slx')

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();

initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);

initializeReferences();

K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_ine = tf(zeros(6));
save('./mat/controllers.mat', 'K_ine', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
K_dvf = tf(zeros(6));
save('./mat/controllers.mat', 'K_dvf', '-append');

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'sim_nass_active_damping';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');  io_i = io_i + 1;

%% Run the linearization
G = linearize(mdl, io, options);
G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};

G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
G_dvf = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
G_ine = minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));

save('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
  plot(freqs, abs(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
  plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

linkaxes([ax1,ax2],'x');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
  plot(freqs, abs(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
  plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

linkaxes([ax1,ax2],'x');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
  plot(freqs, abs(squeeze(freqresp(G(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [(m/s)/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
  plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

linkaxes([ax1,ax2],'x');

initDisturbances();

initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);

load('mat/conf_simscape.mat');
set_param(conf_simscape, 'StopTime', '3');

sim('sim_nass_active_damping');

save('./active_damping/mat/tomo_exp.mat', 'Ern', 't');

figure;
hold on;
plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
hold off;
xlim([1,inf]);
legend();
xlabel('Time [s]'); ylabel('Position Error [m]');

figure;
hold on;
plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
hold off;
xlim([1,inf]);
legend();
xlabel('Time [s]'); ylabel('Position Error [rad]');