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

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

%% Path for functions, data and scripts
addpath('./src/'); % Path for scripts
addpath('./mat/'); % Path for data
addpath('./STEPS/'); % Path for Simscape Model
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;

%% Open Simscape Model
mdl = 'detail_fem_APA300ML'; % Name of the Simulink File
open(mdl); % Open Simscape Model

% Piezoelectric parameters
ga = -25.9; % [N/V]
gs = -5.08e6; % [V/m]

%% Colors for the figures
colors = colororder;
freqs = logspace(1,3,500); % Frequency vector [Hz]

%% Identify dynamics with "Reduced Order Flexible Body"
K = readmatrix('APA300ML_mat_K.CSV');
M = readmatrix('APA300ML_mat_M.CSV');
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA300ML_out_nodes_3D.txt');

m = 5; % [kg]
ga = 25.9; % [N/V]
gs = 5.08e6; % [V/m]

clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1;

G_fem = linearize(mdl, io);
G_fem_z = G_fem('z','Va');
G_fem_Vs = G_fem('Vs', 'Va');
G_fem_comp = G_fem('z', 'Fd');

%% Determine c1 and k1 from the zero
G_zeros = zero(minreal(G_fem_Vs));
G_zeros = G_zeros(imag(G_zeros)>0);
[~, i_sort] = sort(imag(G_zeros));
G_zeros = G_zeros(i_sort);
G_zero = G_zeros(1);

% Solving 2nd order equations
c1 = -2*m*real(G_zero);
k1 = m*(imag(G_zero)^2 + real(G_zero)^2);

%% Determine ka, ke, ca, ce from the first pole
G_poles = pole(minreal(G_fem_z));
G_poles = G_poles(imag(G_poles)>0);
[~, i_sort] = sort(imag(G_poles));
G_poles = G_poles(i_sort);
G_pole = G_poles(1);

% Solving 2nd order equations
ce = 3*(-2*m*real(G_pole(1)) - c1);
ca = 1/2*ce;

ke = 3*(m*(imag(G_pole)^2 + real(G_pole)^2) - k1);
ka = 1/2*ke;

%% Matching sensor/actuator constants
% ga = dcgain(G_fem_z) / (1/(ka + k1*ke/(k1 + ke)));
clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Fa'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/z'], 1, 'openoutput'); io_i = io_i + 1;
ga = dcgain(G_fem_z)/dcgain(linearize([mdl, '_2dof'], io));

clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Va'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/dL'], 1, 'openoutput'); io_i = io_i + 1;
gs = dcgain(G_fem_Vs)/dcgain(linearize([mdl, '_2dof'], io));

%% Identify dynamics with tuned 2DoF model
clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Va'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/Fd'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/z'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/Vs'], 1, 'openoutput'); io_i = io_i + 1;

G_2dof = linearize([mdl, '_2dof'], io);
G_2dof_z = G_2dof('z','Va');
G_2dof_Vs = G_2dof('Vs', 'Va');
G_2dof_comp = G_2dof('z', 'Fd');

%% Comparison of the transfer functions from Va to vertical motion - FEM vs 2DoF
freqs = logspace(1, 3, 500);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_fem_z, freqs, 'Hz'))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'FEM')
plot(freqs, abs(squeeze(freqresp(G_2dof_z, freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $y/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 2e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_fem_z, freqs, 'Hz')))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_2dof_z, freqs, 'Hz')))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360); ylim([-20, 200]);

linkaxes([ax1,ax2],'x');
xlim([10, 1e3]);

%% Comparison of the transfer functions from Va to Vs - FEM vs 2DoF
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_fem_Vs, freqs, 'Hz'))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_2dof_Vs, freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '2DoF Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([6e-4, 3e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_fem_Vs, freqs, 'Hz')))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_2dof_Vs, freqs, 'Hz')))), '--', 'color', colors(1,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360); ylim([-20, 200]);

linkaxes([ax1,ax2],'x');
xlim([10, 1e3]);

%% Effect of electrical boundaries on the
oc = load('detail_fem_apa95ml_open_circuit.mat',  't', 'encoder', 'u');
sc = load('detail_fem_apa95ml_short_circuit.mat', 't', 'encoder', 'u');

% Spectral Analysis parameters
Ts = sc.t(end)/(length(sc.t)-1);
Nfft = floor(2/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

% Identification of the transfer function from Va to di
[G_oc,  f]  = tfestimate(detrend(oc.u, 0), detrend(oc.encoder, 0), win, Noverlap, Nfft, 1/Ts);
[G_sc,  f]  = tfestimate(detrend(sc.u, 0), detrend(sc.encoder, 0), win, Noverlap, Nfft, 1/Ts);

% Find resonance frequencies
[~, i_oc] = max(abs(G_oc(f<300)));
[~, i_sc] = max(abs(G_sc(f<300)));

%% Effect of the electrical bondaries of the force sensor stack on the APA95ML resonance frequency
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_oc), '-', 'DisplayName', sprintf('Open-Circuit - $f_0 = %.1f Hz$', f(i_oc)))
plot(f, abs(G_sc), '-', 'DisplayName', sprintf('Short-Circuit - $f_0 = %.1f Hz$', f(i_sc)))
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-6, 1e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_oc), '-')
plot(f, 180/pi*angle(G_sc), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
yticks(-360:45:360);
ylim([-20, 200]);
axis padded 'auto x'

linkaxes([ax1,ax2], 'x');
xlim([100, 300]);

%% Compare Dynamics between "Reduced Order" flexible joints and "2-dof and 3-dof" joints
% Let's initialize all the stages with default parameters.
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSample('m', 50);

initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

mdl = 'detail_fem_nass';

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');              io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Errors in the frame of the struts
io(io_i) = linio([mdl, '/NASS'],       3, 'openoutput', [], 'fn');  io_i = io_i + 1; % Force Sensors

% Flexible actuators
initializeSimplifiedNanoHexapod('actuator_type', 'flexible', ...
            'flex_type_F', '2dof', ...
            'flex_type_M', '3dof');

G_flex = linearize(mdl, io);
G_flex.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_flex.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};

% Actuators modeled as 2DoF system
initializeSimplifiedNanoHexapod('actuator_type', 'apa300ml', ...
            'flex_type_F', '2dof', ...
            'flex_type_M', '3dof');

G_ideal = linearize(mdl, io);
G_ideal.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_ideal.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};

%% Comparison of the dynamics for actuators modeled using "reduced order flexible body" and using 2DoF system - HAC plant
freqs = logspace(1, 4, 1000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for j = 1:5
    for k = j+1:6
        plot(freqs, abs(squeeze(freqresp(G_flex("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
            'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_ideal("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
            'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_flex("l1","f1"), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_ideal("l1","f1"), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '2-DoF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 1e-4]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex("l1","f1"), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ideal("l1","f1"), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);

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

%% Comparison of the dynamics for actuators modeled using "reduced order flexible body" and using 2DoF system - IFF plant
freqs = logspace(0, 3, 1000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for j = 1:5
    for k = j+1:6
        plot(freqs, abs(squeeze(freqresp(G_flex("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
            'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_ideal("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
            'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_flex("fm1","f1"), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_ideal("fm1","f1"), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '2-DoF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-5, 1e1]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex("fm1","f1"), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ideal("fm1","f1"), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);

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