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

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

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

load('./active_damping/mat/undamped_plants.mat', '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_ine(['Vnlm', 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_ine(['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');

K_ine = 1e3/(1+s/(2*pi*100));

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i=1:6
  plot(freqs, abs(squeeze(freqresp(K_ine*G_ine(['Vnlm', 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(K_ine*G_ine(['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');

K_ine = -K_ine*eye(6);

save('./active_damping/mat/K_ine.mat', 'K_ine');

initializeReferences();
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);

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

G_ine = identifyPlant();

save('./active_damping/mat/plants.mat', 'G_ine', '-append');

freqs = logspace(0, 3, 1000);

figure;

subplot(2, 1, 1);
title('$D_g$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');

subplot(2, 1, 2);
title('$F_s$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');

freqs = logspace(0, 3, 1000);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');

ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');

ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');

ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');

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

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

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

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

initDisturbances();

K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_ine = K_ine;
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');

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

sim('sim_nass_active_damping');

t_ine = t;
Ern_ine = Ern;
save('./active_damping/mat/tomo_exp.mat', 'Ern_ine', 't_ine', '-append');

load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_ine', 't', 't_ine');

figure;
hold on;
plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
set(gca,'ColorOrderIndex',1);
plot(t_ine, Ern_ine(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - Inertial')
plot(t_ine, Ern_ine(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - Inertial')
plot(t_ine, Ern_ine(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - Inertial')
hold off;
xlim([1,inf]);
legend();

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}$')
set(gca,'ColorOrderIndex',1);
plot(t_ine, Ern_ine(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - Inertial')
plot(t_ine, Ern_ine(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - Inertial')
plot(t_ine, Ern_ine(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - Inertial')
hold off;
xlim([1,inf]);
legend();