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

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

addpath('active_damping/src/');

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

prepareLinearizeIdentification();

%% 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; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm');  io_i = io_i + 1; % Relative Motion Outputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm');  io_i = io_i + 1; % Force Sensors
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');   io_i = io_i + 1; % Absolute Velocity Outputs

%% Run the linearization
G = linearize(mdl, io, 0.5, 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');

load('./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_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['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_dvf(['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_dvf(['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_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{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_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');

prepareLinearizeIdentification();

%% 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; % Actuator Inputs
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1; % Metrology Outputs

masses = [1, 10, 50]; % [kg]

G_cart = {zeros(length(masses))};

load('mat/stages.mat', 'nano_hexapod');

for i = 1:length(masses)
  initializeSample('mass', masses(i));

  %% Run the linearization
  G = linearize(mdl, io, 0.3, options);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};

  G_cart_i = G*inv(nano_hexapod.J');
  G_cart_i.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};

  G_cart(i) = {G_cart_i};
end

save('./active_damping/mat/cart_plants.mat', 'G_cart', 'masses');

load('./active_damping/mat/cart_plants.mat', 'G_cart', 'masses');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(masses)
  set(gca,'ColorOrderIndex',i);
  p1 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Dnx', 'Fnx'), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',i);
  p2 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Dny', 'Fny'), freqs, 'Hz'))), '--');
  set(gca,'ColorOrderIndex',i);
  p3 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Dnz', 'Fnz'), freqs, 'Hz'))), ':');
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(masses)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Dnx', 'Fnx'), freqs, 'Hz')))), ...
       'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Dny', 'Fny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Dnz', 'Fnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
yticks([-540:180:540]);
legend('location', 'northeast');

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

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(masses)
  set(gca,'ColorOrderIndex',i);
  p1 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Rnx', 'Mnx'), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',i);
  p2 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Rny', 'Mny'), freqs, 'Hz'))), '--');
  set(gca,'ColorOrderIndex',i);
  p3 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Rnz', 'Mnz'), freqs, 'Hz'))), ':');
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend([p1,p2,p3], {'Rx/Mx', 'Ry/Mx', 'Rz/Mz'});

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(masses)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Rnx', 'Mnx'), freqs, 'Hz')))), ...
       'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Rny', 'Mny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Rnz', 'Mnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
yticks([-540:180:540]);
legend('location', 'northeast');

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

prepareTomographyExperiment();

load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '4.5');

sim('sim_nass_active_damping');

save('./active_damping/mat/tomo_exp.mat', 'En', 'Eg', '-append');

load('./active_damping/mat/tomo_exp.mat', 'En');
Fs = 1e3; % Sampling Frequency of the Data
t = (1/Fs)*[0:length(En(:,1))-1];

figure;
ax1 = subplot(3, 1, 1);
hold on;
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
legend('location', 'southwest');

ax2 = subplot(3, 1, 2);
hold on;
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
legend('location', 'southwest');
ylabel('Position Error [m]');

ax3 = subplot(3, 1, 3);
hold on;
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northwest');
xlabel('Time [s]');

linkaxes([ax1,ax2,ax3],'x');
xlim([0.5,inf]);

figure;
ax1 = subplot(3, 1, 1);
hold on;
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
legend('location', 'northwest');

ax2 = subplot(3, 1, 2);
hold on;
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
legend('location', 'southwest');
ylabel('Position Error [rad]');

ax3 = subplot(3, 1, 3);
hold on;
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
legend();
xlabel('Time [s]');

linkaxes([ax1,ax2,ax3],'x');
xlim([0.5,inf]);