%% Load open loop data
% gm_ol = load('../data/ground_motion_001.mat');
%%
clear; close all; clc;

%%
sim Assemblage;

%%
Dsample.Data = Dsample.Data - Dsample.Data(1, :);

%% Time domain X-Y-Z
figure;
hold on;
plot(Dsample.Time, Dsample.Data(:, 1));
plot(Dsample.Time, Dsample.Data(:, 2));
plot(Dsample.Time, Dsample.Data(:, 3));
legend({'x', 'y', 'z'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('tomo_time_translations', 'normal-normal')

%% Time domain angles
figure;
hold on;
plot(Dsample.Time, Dsample.Data(:, 4));
plot(Dsample.Time, Dsample.Data(:, 5));
plot(Dsample.Time, Dsample.Data(:, 6));
legend({'$\theta_x$', '$\theta_y$', '$\theta_z$'})
hold off;
xlabel('Time [s]'); ylabel('Angle [rad]');

exportFig('tomo_time_rotations', 'normal-normal')

%% PSD X-Y-Z
han_windows = hanning(ceil(length(Dsample.Time)/10));

[psd_x, freqs_x] = pwelch(Dsample.Data(:, 1), han_windows, 0, [], 1/Ts);
[psd_y, freqs_y] = pwelch(Dsample.Data(:, 2), han_windows, 0, [], 1/Ts);
[psd_z, freqs_z] = pwelch(Dsample.Data(:, 3), han_windows, 0, [], 1/Ts);

figure;
hold on;
plot(freqs_x, sqrt(psd_x));
plot(freqs_y, sqrt(psd_y));
plot(freqs_z, sqrt(psd_z));
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
legend({'x', 'y', 'z'})
hold off;

exportFig('tomo_psd_translations', 'normal-normal')

%% PSD
han_windows = hanning(ceil(length(Dsample.Time)/10));

[psd_x, freqs_x] = pwelch(Dsample.Data(:, 4), han_windows, 0, [], 1/Ts);
[psd_y, freqs_y] = pwelch(Dsample.Data(:, 5), han_windows, 0, [], 1/Ts);
[psd_z, freqs_z] = pwelch(Dsample.Data(:, 6), han_windows, 0, [], 1/Ts);

figure;
hold on;
plot(freqs_x, sqrt(psd_x));
plot(freqs_y, sqrt(psd_y));
plot(freqs_z, sqrt(psd_z));
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$rad/s/\sqrt{Hz}$]');
legend({'$\theta_x$', '$\theta_y$', '$\theta_z$'})
hold off;

exportFig('tomo_psd_rotations', 'normal-normal')

%%
save('./data/ground_motion.mat', 'Dsample')