%% 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

%% Colors for the figures
colors = colororder;

%% Load measured data - hysteresis
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'u', 'de');

% Initial time set to zero
apa_hyst.t = apa_hyst.t - apa_hyst.t(1);

ampls = [0.1, 0.2, 0.4, 1, 2, 4]; % Excitation voltage amplitudes

%% Measured displacement as a function of the output voltage
figure;

hold on;
for i = [6,5,4,2]
    i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
    plot(20*apa_hyst.u(i_lim), 1e6*detrend(apa_hyst.de(i_lim), 0), ...
         'DisplayName', sprintf('$V_a = 65 + %.0f \\sin (\\omega t) \\ [V]$', 20*ampls(i)))
end
hold off;
xlabel('Stack Voltage $V_a$ [V]'); ylabel('Displacement $d_e$ [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-20, 150]);
ylim([-120, 120]);

%% Load data for stiffness measurement
apa_nums = [1 2 4 5 6 8];
apa_mass = {};
for i = 1:length(apa_nums)
    apa_mass(i) = {load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', apa_nums(i)), 't', 'de')};
    % The initial displacement is set to zero
    apa_mass{i}.de = apa_mass{i}.de - mean(apa_mass{i}.de(apa_mass{i}.t<11));
end

added_mass = 6.4; % Added mass [kg]

%% Plot the deflection at a function of time
figure;
hold on;
plot(apa_mass{2}.t(1:100:end)-apa_mass{2}.t(1), 1e6*apa_mass{2}.de(1:100:end), 'k-');
plot([0,20], [-0.4, -0.4], 'k--', 'LineWidth', 0.5)
plot([0,20], [-4.5, -4.5], 'k--', 'LineWidth', 0.5)
plot([0,20], [-37.4, -37.4], 'k--', 'LineWidth', 0.5)
% first stroke for stiffness measurements
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [2, -0.4, 0, -37];
text(2.5, -20, sprintf('$d_1$'), 'horizontalalignment', 'left');

% second stroke for stiffness measurements
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [18, -37.4, 0, 32.9];
text(18.5, -20, sprintf('$d_2$'), 'horizontalalignment', 'left');

% annotation('textarrow',[],y,'String',' Growth ','FontSize',13,'Linewidth',2)
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [$\mu$m]');

%% Load Data
add_mass_oc = load('frf_data_1_add_mass_open_circuit.mat',   't', 'de');
add_mass_cc = load('frf_data_1_add_mass_closed_circuit.mat', 't', 'de');

%% Zero displacement at initial time
add_mass_oc.de = add_mass_oc.de - mean(add_mass_oc.de(add_mass_oc.t<11));
add_mass_cc.de = add_mass_cc.de - mean(add_mass_cc.de(add_mass_cc.t<11));

%% Estimation of the stiffness in Open Circuit and Closed-Circuit
apa_k_oc = 9.8 * added_mass / (mean(add_mass_oc.de(add_mass_oc.t > 12 & add_mass_oc.t < 12.5)) - mean(add_mass_oc.de(add_mass_oc.t > 20 & add_mass_oc.t < 20.5)));
apa_k_sc = 9.8 * added_mass / (mean(add_mass_cc.de(add_mass_cc.t > 12 & add_mass_cc.t < 12.5)) - mean(add_mass_cc.de(add_mass_cc.t > 20 & add_mass_cc.t < 20.5)));

%% Identification using sweep sine (low frequency)
load('frf_data_sweep.mat');
load('frf_data_noise_hf.mat');

%% Sampling Frequency
Ts = 1e-4; % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]

%% "Hanning" windows used for the spectral analysis:
Nfft = floor(2/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Separation of frequencies: low freqs using sweep sine, and high freq using noise
% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep{1}.u, apa_sweep{1}.de, win, Noverlap, Nfft, 1/Ts);
i_lf = f <= 350;
i_hf = f >  350;

%% FRF estimation of the transfer function from u to de
enc_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.u,    apa_sweep{i}.de,    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.de, win, Noverlap, Nfft, 1/Ts);
    enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end

%% FRF estimation of the transfer function from u to Vs
iff_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.u,    apa_sweep{i}.Vs,    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.Vs, win, Noverlap, Nfft, 1/Ts);
    iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end

%% Save the identified dynamics for further analysis
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');

%% Plot the FRF from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

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

%% Plot the FRF from u to Vs
figure;
tiledlayout(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)));
end
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, 2e3]);

%% Long measurement
long_noise = load('frf_struts_align_3_noise_long.mat', 't', 'u', 'Vs');

% Long window for fine frequency axis
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(10/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

% Transfer function estimation
[frf_noise, f] = tfestimate(long_noise.u, long_noise.Vs, win, Noverlap, Nfft, 1/Ts);
[coh_noise, ~] = mscohere(long_noise.u, long_noise.Vs, win, Noverlap, Nfft, 1/Ts);

%% Bode plot of the FRF from u to de
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

nexttile();
hold on;
plot(f, coh_noise, '.-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
hold off;
xlim([38, 45]);
ylim([0, 1]);

%% Bode plot of the FRF from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_noise), '.-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(frf_noise), '.-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 0]);

linkaxes([ax1,ax2],'x');
xlim([38, 45]);

%% Load the data
wi_k = load('frf_data_1_sweep_lf_with_R.mat', 't', 'Vs', 'u'); % With the resistor
wo_k = load('frf_data_1_sweep_lf.mat',        't', 'Vs', 'u'); % Without the resistor

%% Large Hanning window for good low frequency estimate
Nfft = floor(50/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Compute the transfer functions from u to Vs
[frf_wo_k, f] = tfestimate(wo_k.u, wo_k.Vs, win, Noverlap, Nfft, 1/Ts);
[frf_wi_k, ~] = tfestimate(wi_k.u, wi_k.Vs, win, Noverlap, Nfft, 1/Ts);

%% Model for the high pass filter
C = 5.1e-6; % Sensor Stack capacitance [F]
R = 80.6e3; % Parallel Resistor [Ohm]

f0 = 1/(2*pi*R*C); % Crossover frequency of RC HPF [Hz]

G_hpf = 0.6*(s/(2*pi*f0))/(1 + s/(2*pi*f0));

%% Compare the HPF model and the measured FRF
figure;
tiledlayout(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $R$');
plot(f, abs(frf_wi_k), 'DisplayName', 'With $R$');
plot(f, abs(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--', 'DisplayName', 'RC model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([2e-1, 1e0]);
yticks([0.2, 0.5, 1]);
legend('location', 'southeast', 'FontSize', 8);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(frf_wo_k));
plot(f, 180/pi*angle(frf_wi_k));
plot(f, 180/pi*angle(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--', 'DisplayName', 'RC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360); ylim([-5, 60]);
yticks([0, 15, 30, 45, 60]);

linkaxes([ax1,ax2],'x');
xlim([0.2, 8]);
xticks([0.2, 0.5, 1, 2, 5]);

%% Load identification Data
data = load("2023-03-17_11-28_iff_plant.mat");

%% Spectral Analysis setup
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(5/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Compute the transfer function from applied force to measured rotation
[G_iff, f]  = tfestimate(data.id_plant, data.Vs, win, Noverlap, Nfft, 1/Ts);

%% Basic manually tuned model
w0z = 2*pi*42.7; % Zero frequency
xiz = 0.004; % Zero damping

w0p = 2*pi*95.2; % Pole frequency
xip = 0.02; % Pole damping

G_iff_model = exp(-2*s*Ts)*0.64*(1 + 2*xiz/w0z*s + s^2/w0z^2)/(1 + 2*xip/w0p*s + s^2/w0p^2)*(s/(s+2*pi*0.4));

%% Identified IFF plant and manually tuned model of the plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff), 'color', colors(2,:), 'DisplayName', 'Identified plant')
plot(f, abs(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--', 'DisplayName', 'Manual fit')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_iff), 'color', colors(2,:));
plot(f, 180/pi*angle(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-90, 180])

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

%% Integral Force Feedback Controller
K_iff = -10*(1/(s + 2*pi*20)) * ... % LPF: provides integral action above 20Hz
            (s/(s + 2*pi*2))  * ... % HPF: limit low frequency gain
            (1/(1 + s/2/pi/2e3));   % LPF: more robust to high frequency resonances

%% Load Data
data = load("2023-03-17_14-10_damped_plants_new.mat");

%% Spectral Analysis setup
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Get the frequency vector
[~, f]  = tfestimate(data.data(1).id_plant(1:end), data.data(1).dL(1:end), win, Noverlap, Nfft, 1/Ts);

%% Gains used for analysis are between 1 and 50
i_kept = [5:10];

%% Identify the damped plant from u' to de for different IFF gains
G_dL_frf = {zeros(1,length(i_kept))};

for i = 1:length(i_kept)
    [G_dL, ~]  = tfestimate(data.data(i_kept(i)).id_plant(1:end), data.data(i_kept(i)).dL(1:end), win, Noverlap, Nfft, 1/Ts);
    G_dL_frf(i) = {G_dL};
end

%% Fit the data with 2nd order transfer function using vectfit3
opts = struct();

opts.stable = 1;    % Enforce stable poles
opts.asymp = 1;     % Force D matrix to be null
opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
opts.skip_pole = 0; % Do NOT skip pole identification
opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
opts.cmplx_ss = 0;  % Create real state space model with block diagonal A

opts.spy1 = 0;      % No plotting for first stage of vector fitting
opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)

Niter = 100; % Number of iteration.
N = 2; % Order of approximation
poles = [-25 - 1i*60, -25 + 1i*60]; % First get for the pole location

G_dL_id  = {zeros(1,length(i_kept))};

% Identification just between two frequencies
f_keep = (f>5 & f<200);

for i = 1:length(i_kept)
    %% Estimate resonance frequency and damping
    for iter = 1:Niter
        [G_est, poles, ~, frf_est] = vectfit3(G_dL_frf{i}(f_keep).', 1i*2*pi*f(f_keep)', poles, ones(size(f(f_keep)))', opts);
    end
    G_dL_id(i) = {ss(G_est.A, G_est.B, G_est.C, G_est.D)};
end

%% Identified dynamics from u' to de for different IFF gains
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
for i = 1:length(i_kept)
    plot(f, abs(G_dL_frf{i}), 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %.0f', data.gains(i_kept(i))))
    plot(f, abs(squeeze(freqresp(G_dL_id{i}, f, 'Hz'))), '--', 'color', [colors(i,:), 1], 'HandleVisibility', 'off')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_e/u^\prime$ [m/V]');
xlim([10, 1e3]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

%% Root Locus of the APA300ML with Integral Force Feedback
% Comparison between the computed root locus from the plant model and the root locus estimated from the damped plant pole identification
gains = logspace(-1, 3, 1000);

figure;
hold on;
G_iff_poles = pole(pade(G_iff_model));
i = imag(G_iff_poles) > 100; % Only keep relevant poles
plot(real(G_iff_poles(i)),  imag(G_iff_poles(i)),  'kx', ...
    'DisplayName', '$g = 0$');
G_iff_zeros = tzero(G_iff_model);
i = imag(G_iff_zeros) > 100; % Only keep relevant zeros
plot(real(G_iff_zeros(i)), imag(G_iff_zeros(i)), 'ko', ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(pade(G_iff_model), g*K_iff, 1));
    i = imag(clpoles) > 100; % Only keep relevant poles
    plot(real(clpoles(i)), imag(clpoles(i)), 'k.', ...
        'HandleVisibility', 'off');
end

for i = 1:length(i_kept)
    plot(real(pole(G_dL_id{i})), imag(pole(G_dL_id{i})), 'x', 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %1.f', data.gains(i_kept(i))));
end
xlabel('Real Part')
ylabel('Imaginary Part')
axis equal
ylim([0, 610]);
xlim([-300,0]);
legend('location', 'southwest', 'FontSize', 8);