%% 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_super_element'; % Name of the Simulink File
open(mdl); % Open Simscape Model

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

%% Estimate "Sensor Constant" - (THP5H)
d33 = 680e-12;        % Strain constant [m/V]
n   = 160;            % Number of layers per stack
eT  = 4500*8.854e-12; % Permittivity under constant stress [F/m]
sD  = 21e-12;         % Compliance under constant electric displacement [m2/N]

gs = d33/(eT*sD*n);   % Sensor Constant [V/m]

%% Estimate "Actuator Constant" - (THP5H)
d33 = 680e-12;   % Strain constant [m/V]
n   = 320;       % Number of layers

cE  = 1/sD;      % Youngs modulus [N/m^2]
A   = (10e-3)^2; % Area of the stacks [m^2]
L   = 40e-3;     % Length of the two stacks [m]
ka  = cE*A/L;    % Stiffness of the two stacks [N/m]

ga = d33*n*ka;   % Actuator Constant [N/V]

%% Load reduced order model
K = readmatrix('APA95ML_K.CSV'); % order: 48
M = readmatrix('APA95ML_M.CSV');
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');

%% Stiffness estimation
m = 0.0001; % block-free condition, no payload
k_support = 1e9;
c_support = 1e3;

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

G = linearize(mdl, io);

% The inverse of the DC gain of the transfer function
% from vertical force to vertical displacement is the axial stiffness of the APA
k_est = 1/dcgain(G); % [N/m]

%% Estimated compliance of the APA95ML
freqs = logspace(2, log10(5000), 1000);

% Get first resonance indice
i_max = find(abs(squeeze(freqresp(G, freqs(2:end), 'Hz'))) - abs(squeeze(freqresp(G, freqs(1:end-1), 'Hz'))) < 0, 1);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))), 'DisplayName', 'Compliance');
plot([freqs(1), freqs(end)], [1/k_est, 1/k_est], 'k--', 'DisplayName', sprintf('$1/k$ ($k = %.0f N/\\mu m$)', 1e-6*k_est))
xline(freqs(i_max), '--', 'linewidth', 1, 'color', [0,0,0], 'DisplayName', sprintf('$f_0 = %.0f$ Hz', freqs(i_max)))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([100, 5000]);

%% Estimation of the amplification factor and Stroke
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/d'],  1, 'openoutput'); io_i = io_i + 1;

G = linearize(mdl, io);

% Estimated amplification factor
ampl_factor = abs(dcgain(G(1,1))./dcgain(G(2,1)));

% Estimated stroke
apa_stroke = ampl_factor * 3 * 20e-6; % [m]

%% Experimental plant identification
% with PD200 amplifier (gain of 20) - 2 stacks as an actuator, 1 as a sensor
load('apa95ml_5kg_2a_1s.mat')

Va = 20*u; % Voltage amplifier gain: 20

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

% Identification of the transfer function from Va to di
[G_y,  f]  = tfestimate(detrend(Va, 0), detrend(y, 0), win, Noverlap, Nfft, 1/Ts);
[G_Vs, ~]  = tfestimate(detrend(Va, 0), detrend(v, 0), win, Noverlap, Nfft, 1/Ts);

%% Plant Identification from Multi-Body model
% Load Reduced Order Matrices
K = readmatrix('APA95ML_K.CSV'); % order: 48
M = readmatrix('APA95ML_M.CSV');
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');

m = 5.5; % Mass of the suspended granite [kg]
k_support = 4e7;
c_support = 3e2;

% Compute transfer functions
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Voltage accros piezo stacks [V]
io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor stack voltage [V]

Gm = linearize(mdl, io);
Gm.InputName  = {'Va'};
Gm.OutputName = {'y', 'Vs'};

%% Comparison of the identified transfer function from Va to di to the multi-body model
freqs = logspace(1, 3, 500);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_y), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'Measured FRF');
plot(freqs, abs(squeeze(freqresp(Gm('y', 'Va'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '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, 1e-5]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_y), '-', 'color' , [colors(2,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm('y', 'Va'), 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([-45, 180]);

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

%% Comparison of the identified transfer function from Va to Vs to the multi-body model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_Vs), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'Measured FRF');
plot(freqs, abs(squeeze(freqresp(Gm('Vs', 'Va'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '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([1e-3, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_Vs), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm('Vs', 'Va'), 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:90:360); ylim([-180, 180]);

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

%% Integral Force Feedback Controller
K_iff = (1/(s + 2*2*pi))*(s/(s + 0.5*2*pi));
K_iff.inputname = {'Vs'};
K_iff.outputname = {'u_iff'};

% New damped plant input
S1 = sumblk("u = u_iff + u_damp");

% Voltage amplifier with gain of 20
voltage_amplifier = tf(20);
voltage_amplifier.inputname = {'u'};
voltage_amplifier.outputname = {'Va'};

%% Load experimental data with IFF implemented with different gains
load('apa95ml_iff_test.mat', 'results');

% Tested gains
g_iff = [0, 10, 50, 100, 500, 1000];

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

%% Computed the identified FRF of the damped plants
tf_iff = {zeros(1, length(g_iff))};
for i=1:length(g_iff)
    [tf_est, f] = tfestimate(results{i}.u, results{i}.y, win, Noverlap, Nfft, 1/Ts);
    tf_iff(i) = {tf_est};
end

%% Estimate the damped plants from the multi-body model
Gm_iff = {zeros(1, length(g_iff))};

for i=1:length(g_iff)
    K_iff_g = -K_iff*g_iff(i); K_iff_g.inputname = {'Vs'}; K_iff_g.outputname = {'u_iff'};
    Gm_iff(i) = {connect(Gm, K_iff_g, S1, voltage_amplifier, {'u_damp'}, {'y'})};
end

%% Identify second order plants from the experimental data
% This is mandatory to estimate the experimental "poles"
% an place them in the root-locus plot
G_id = {zeros(1,length(results))};

f_start = 70; % [Hz]
f_end = 500; % [Hz]

for i = 1:length(results)
    tf_id = tf_iff{i}(sum(f<f_start):length(f)-sum(f>f_end));
    f_id = f(sum(f<f_start):length(f)-sum(f>f_end));

    gfr = idfrd(tf_id, 2*pi*f_id, Ts);
    G_id(i) = {procest(gfr,'P2UDZ')};
end

%% Comparison of the Root-Locus computed from the multi-body model and the identified closed-loop poles
gains = logspace(0, 5, 1000);
figure;
hold on;
plot(real( pole(Gm('Vs', 'Va'))), imag( pole(Gm('Vs', 'Va'))), 'kx', 'HandleVisibility', 'off');
plot(real(tzero(Gm('Vs', 'Va'))), imag(tzero(Gm('Vs', 'Va'))), 'ko', 'HandleVisibility', 'off');
for i = 1:length(gains)
    cl_poles = pole(feedback(Gm('Vs', 'Va'), gains(i)*K_iff));
    plot(real(cl_poles(imag(cl_poles)>100)), imag(cl_poles(imag(cl_poles)>100)), 'k.', 'HandleVisibility', 'off');
end
for i = 1:length(g_iff)
    cl_poles = pole(Gm_iff{i});
    plot(real(cl_poles(imag(cl_poles)>100)), imag(cl_poles(imag(cl_poles)>100)), '.', 'MarkerSize', 20, 'color', colors(i,:), 'HandleVisibility', 'off');
    plot(real(pole(G_id{i})), imag(pole(G_id{i})), 'x', 'color', colors(i,:), 'DisplayName', sprintf('g = %0.f', g_iff(i)), 'DisplayName', sprintf('$g = %.0f$', g_iff(i)));
end
xlabel('Real Part');
ylabel('Imaginary Part');
axis equal;
ylim([-100, 2100]);
xlim([-2100,100]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Experimental damped plant for several IFF gains and estimated damped plants from the model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2, 1]);
hold on;
plot(f, abs(tf_iff{1}), '-', 'DisplayName', '$g = 0$', 'color', [0,0,0, 0.5], 'linewidth', 2.5)
plot(f, abs(squeeze(freqresp(Gm_iff{1}, f, 'Hz'))), 'k--', 'HandleVisibility', 'off')
for i = 2:length(results)
    plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)), 'color', [colors(i-1,:), 0.5], 'linewidth', 2.5)
end
for i = 2:length(results)
    plot(f, abs(squeeze(freqresp(Gm_iff{i}, f, 'Hz'))), '--', 'color', colors(i-1,:), 'HandleVisibility', 'off')
end
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude $y/V_a$ [m/N]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-6, 2e-4]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_iff{1}./squeeze(freqresp(exp(-s*2e-4), f, 'Hz'))), '-', 'color', [0,0,0, 0.5], 'linewidth', 2.5)
plot(f, 180/pi*angle(squeeze(freqresp(Gm_iff{1}, f, 'Hz'))), 'k--')
for i = 2:length(results)
    plot(f, 180/pi*angle(tf_iff{i}./squeeze(freqresp(exp(-s*2e-4), f, 'Hz'))), '-', 'color', [colors(i-1,:), 0.5], 'linewidth', 2.5)
    plot(f, 180/pi*angle(squeeze(freqresp(Gm_iff{i}, f, 'Hz'))), '--', 'color', colors(i-1,:))
end
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
hold off;
yticks(-360:45:360);
ylim([-10, 190]);

linkaxes([ax1,ax2], 'x');
xlim([150, 500]);