%%
clear; close all; clc;

%% Used to get sim_conf.Ts
load('./mat/sim_conf.mat', 'sim_conf');

%% Load simulation results
gm_ol = load('./data/ground_motion_ol.mat', 'Dsample');
gm_cl = load('./data/ground_motion.mat', 'Dsample');

%%
figure;
hold on;
plot(gm_ol.Dsample.Data(:, 1),gm_ol.Dsample.Data(:, 3))
plot(gm_cl.Dsample.Data(:, 1),gm_cl.Dsample.Data(:, 3))
legend({'OL', 'CL'})
hold off;
xlabel('Displacement - $x$ [s]'); ylabel('Displacement - $z$ [m]');

exportFig('gm_control_xz', 'half-short')

%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 1));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 1));
legend({'x - OL', 'x - CL'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('gm_control_time_x', 'normal-normal')

%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 2));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 2));
legend({'y - OL', 'y - CL'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('gm_control_time_y', 'normal-normal')

%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 3));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 3));
legend({'z - OL', 'z - CL'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('gm_control_time_z', 'normal-normal')

%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 4));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 4));
legend({'$\theta_x$ - OL', '$\theta_x$ - CL'})
hold off;
xlabel('Time [s]'); ylabel('Angle [rad]');

exportFig('gm_control_time_rx', 'normal-normal')

%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 5));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 5));
legend({'$\theta_y$ - OL', '$\theta_y$ - CL'})
hold off;
xlabel('Time [s]'); ylabel('Angle [rad]');

exportFig('gm_control_time_ry', 'normal-normal')

%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 6));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 6));
legend({'$\theta_z$ - OL', '$\theta_z$ - CL'})
hold off;
xlabel('Time [s]'); ylabel('Angle [rad]');

exportFig('gm_control_time_rz', 'normal-normal')

%% Compare OL and CL - PSD
han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
[psd_x_ol, freqs_x_ol] = pwelch(gm_ol.Dsample.Data(:, 1), han_windows_ol, 0, [], 1/sim_conf.Ts);

han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
[psd_x, freqs_x] = pwelch(gm_cl.Dsample.Data(:, 1), han_windows, 0, [], 1/sim_conf.Ts);

figure;
hold on;
plot(freqs_x_ol, sqrt(psd_x_ol));
plot(freqs_x, sqrt(psd_x));
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
legend({'x - OL', 'x - CL'})
hold off;

exportFig('gm_control_psd_x', 'normal-normal')

%% Compare OL and CL - PSD
han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
[psd_y_ol, freqs_y_ol] = pwelch(gm_ol.Dsample.Data(:, 2), han_windows_ol, 0, [], 1/sim_conf.Ts);

han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
[psd_y, freqs_y] = pwelch(gm_cl.Dsample.Data(:, 2), han_windows, 0, [], 1/sim_conf.Ts);

figure;
hold on;
plot(freqs_y_ol, sqrt(psd_y_ol));
plot(freqs_y, sqrt(psd_y));
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
legend({'y - OL', 'y - CL'})
hold off;

exportFig('gm_control_psd_y', 'normal-normal')

%% Compare OL and CL - PSD
load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
load('./mat/weight_Wxg.mat', 'Wxg');
load('./mat/T_S.mat', 'S', 'T');

freqs = logspace(-1, 3, 1000);
dz_ol = squeeze(abs(freqresp(Wxg*G_xg_to_d(3, 3), freqs, 'Hz')));
dz_cl = squeeze(abs(freqresp(Wxg*G_xg_to_d(3, 3)*S(3, 3), freqs, 'Hz')));

han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
[psd_z_ol, freqs_z_ol] = pwelch(gm_ol.Dsample.Data(:, 3), han_windows_ol, 0, [], 1/sim_conf.Ts);

han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
[psd_z, freqs_z] = pwelch(gm_cl.Dsample.Data(:, 3), han_windows, 0, [], 1/sim_conf.Ts);

figure;
hold on;
plot(freqs_z_ol, sqrt(psd_z_ol), '-', 'Color', [0 0.4470 0.7410], 'DisplayName', '$Dg  \to D_x$ - OL (sim)');
plot(freqs, dz_ol, '--', 'Color', [0 0.4470 0.7410], 'DisplayName', '$Dg  \to D_x$ - OL (th)');
plot(freqs_z, sqrt(psd_z), '-', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$Dg  \to D_x$ - CL (sim)');
plot(freqs, dz_cl, '--', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$Dg  \to D_x$ - CL (th)');
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
legend('location', 'southwest');
hold off;

exportFig('gm_control_psd_z', 'normal-normal')