%%
clear; close all; clc;

%% Load Spindle Data
load('./mat/spindle_data.mat', 'spindle');

%% Compute the PSD
n_av = 4; % Number of average

[pxx_1rpm, f_1rpm] = pwelch(spindle.rpm1.xasync, hanning(ceil(length(spindle.rpm1.xasync)/n_av)), [], [], spindle.rpm1.Fs);
[pyy_1rpm, ~]      = pwelch(spindle.rpm1.yasync, hanning(ceil(length(spindle.rpm1.yasync)/n_av)), [], [], spindle.rpm1.Fs);
[pzz_1rpm, ~]      = pwelch(spindle.rpm1.zasync, hanning(ceil(length(spindle.rpm1.zasync)/n_av)), [], [], spindle.rpm1.Fs);

[pxx_60rpm, f_60rpm] = pwelch(spindle.rpm60.xasync, hanning(ceil(length(spindle.rpm60.xasync)/n_av)), [], [], spindle.rpm60.Fs);
[pyy_60rpm, ~]       = pwelch(spindle.rpm60.yasync, hanning(ceil(length(spindle.rpm60.yasync)/n_av)), [], [], spindle.rpm60.Fs);
[pzz_60rpm, ~]       = pwelch(spindle.rpm60.zasync, hanning(ceil(length(spindle.rpm60.zasync)/n_av)), [], [], spindle.rpm60.Fs);

%%
figure;
hold on;
plot(f_1rpm, (pxx_1rpm).^.5, 'DisplayName', '$P_{xx}$ - 1rpm');
plot(f_1rpm, (pyy_1rpm).^.5, 'DisplayName', '$P_{yy}$ - 1rpm');
plot(f_1rpm, (pzz_1rpm).^.5, 'DisplayName', '$P_{zz}$ - 1rpm');
% plot(f_1rpm, spindle.rpm1.adcn*ones(size(f_1rpm)), '--k', 'DisplayName', 'ADC - 1rpm');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
legend('Location', 'northeast');
xlim([f_1rpm(2), f_1rpm(end)]);

exportFig('spindle_psd_xyz_1rpm', 'normal-normal');

%%
figure;
hold on;
plot(f_60rpm, (pxx_60rpm).^.5, 'DisplayName', '$P_{xx}$ - 60rpm');
plot(f_60rpm, (pyy_60rpm).^.5, 'DisplayName', '$P_{yy}$ - 60rpm');
plot(f_60rpm, (pzz_60rpm).^.5, 'DisplayName', '$P_{zz}$ - 60rpm');
% plot(f_60rpm, spindle.rpm60.adcn*ones(size(f_60rpm)), '--k', 'DisplayName', 'ADC - 60rpm');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
legend('Location', 'northeast');
xlim([f_60rpm(2), f_60rpm(end)]);

exportFig('spindle_psd_xyz_60rpm', 'wide-normal');

%% Load the model of the spindle
load('./mat/spindle_model.mat', 'sys');
Tfd = abs(squeeze(freqresp(sys('d', 'f'), f_60rpm, 'Hz')));

%% Plot the PSD of the Force using the model
figure;
plot(f_60rpm, (pxx_60rpm.^.5)./Tfd, 'DisplayName', '$P_{xx}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$N/\sqrt{Hz}$]');
xlim([f_60rpm(2), f_60rpm(end)]);

exportFig('spindle_psd_f_60rpm', 'wide-normal');

%% Estimated Shape of the PSD of the force
s = tf('s');

Wd_simple = 1e-8/(1+s/2/pi/0.5)/(1+s/2/pi/100);
Wf_simple = Wd_simple/tf(sys('d', 'f'));
TWf_simple = abs(squeeze(freqresp(Wf_simple, f_60rpm, 'Hz')));

% Wf = 0.48902*(s+327.9)*(s^2 + 109.6*s + 1.687e04)/((s^2 + 30.59*s + 8541)*(s^2 + 29.11*s + 3.268e04));
% Wf = 0.15788*(s+418.6)*(s+1697)^2*(s^2 + 124.3*s + 2.529e04)*(s^2 + 681.3*s + 9.018e05)/((s^2 + 23.03*s + 8916)*(s^2 + 33.85*s + 6.559e04)*(s^2 + 71.43*s + 4.283e05)*(s^2 + 40.64*s + 1.789e06));

Wf = (s+1697)^2*(s^2 + 114.5*s + 2.278e04)*(s^2 + 205.1*s + 1.627e05)*(s^2 + 285.8*s + 8.624e05)*(s+100)/((s+0.5)*3012*(s^2 + 23.03*s + 8916)*(s^2 + 17.07*s + 4.798e04)*(s^2 + 41.17*s + 4.347e05)*(s^2 + 78.99*s + 1.789e06));

% sisotool(tf(1), Wf) % SISOTOOL

TWf = abs(squeeze(freqresp(Wf, f_60rpm, 'Hz')));

%% PSD in [N]
figure;
hold on;
plot(f_60rpm, (pxx_60rpm.^.5)./Tfd, 'DisplayName', '$\sqrt{P_{xx}}/|T_{d/f}|$');
plot(f_60rpm, TWf, 'DisplayName', 'Wf');
plot(f_60rpm, TWf_simple, '-k', 'DisplayName', 'Wfs');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$N/\sqrt{Hz}$]');
xlim([f_60rpm(2), f_60rpm(end)]);
legend('Location', 'northeast');

exportFig('spindle_psd_f_comp_60rpm', 'wide-normal');

%% PSD in [m]
figure;
hold on;
plot(f_60rpm, pxx_60rpm.^.5, 'DisplayName', '$\sqrt{P_{xx}}$');
plot(f_60rpm, TWf.*Tfd, 'DisplayName', '$|W_f|*|T_{d/f}|$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
xlim([f_60rpm(2), f_60rpm(end)]);
legend('Location', 'northeast');

exportFig('spindle_psd_d_comp_60rpm', 'normal-normal');

%% Compute the resulting RMS value [m]
figure;
hold on;
plot(f_60rpm, 1e9*cumtrapz(f_60rpm, (pxx_60rpm)).^.5, '--', 'DisplayName', 'Exp. Data');
plot(f_60rpm, 1e9*cumtrapz(f_60rpm, ((TWf.*Tfd).^2)).^.5, '--', 'DisplayName', 'Estimated');
hold off;
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$nm/\sqrt{Hz}$]');
xlim([f_60rpm(2), f_60rpm(end)]);
legend('Location', 'northeast');

exportFig('spindle_cps_d_comp_60rpm', 'normal-normal');

%% Compute the resulting RMS value [m]
figure;
hold on;
plot(f_1rpm, 1e9*cumtrapz(f_1rpm, (pxx_1rpm)), '--', 'DisplayName', 'Exp. Data');
plot(f_1rpm, 1e9*(f_1rpm(end)-f_1rpm(1))/(length(f_1rpm)-1)*cumsum(pxx_1rpm), '--', 'DisplayName', 'Exp. Data');
hold off;
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$nm/\sqrt{Hz}$]');
xlim([f_1rpm(2), f_1rpm(end)]);
legend('Location', 'northeast');

exportFig('spindle_cps_d_comp_1rpm', 'normal-normal');

%% Save
save('./mat/spindle_Wf.mat', 'Wf');