%%
clear; close all; clc;

%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');

%% Load Simulation Results
sim_light_vc_ol     = load('./mat/sim_light_vc_ol_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_ol     = load('./mat/sim_light_pz_ol_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');

sim_light_vc_cl     = load('./mat/sim_light_vc_cl_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_cl     = load('./mat/sim_light_pz_cl_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');

sim_light_vc_ol_iff = load('./mat/sim_light_vc_ol_iff.mat',  'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_ol_iff = load('./mat/sim_light_pz_ol_iff.mat',  'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');

sim_light_vc_cl_iff = load('./mat/sim_light_vc_cl_iff.mat',  'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_cl_iff = load('./mat/sim_light_pz_cl_iff.mat',  'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');

%%
figure;
hold on;
plot(sim_light_vc_ol.Dx, sim_light_vc_ol.Dy);
plot(sim_light_vc_cl.Dx, sim_light_vc_cl.Dy);
plot(sim_light_vc_ol_iff.Dx, sim_light_vc_ol_iff.Dy);
plot(sim_light_vc_cl_iff.Dx, sim_light_vc_cl_iff.Dy);
hold off;

%%
rms(sqrt(sim_light_vc_ol.Dx.^2+sim_light_vc_ol.Dy.^2))
rms(sqrt(sim_light_vc_cl.Dx.^2+sim_light_vc_cl.Dy.^2))
rms(sqrt(sim_light_vc_ol_iff.Dx.^2+sim_light_vc_ol_iff.Dy.^2))
rms(sqrt(sim_light_vc_cl_iff.Dx.^2+sim_light_vc_cl_iff.Dy.^2))

%%
rms(sqrt(sim_light_pz_ol.Dx.^2+sim_light_pz_ol.Dy.^2))
rms(sqrt(sim_light_pz_cl.Dx.^2+sim_light_pz_cl.Dy.^2))
rms(sqrt(sim_light_pz_ol_iff.Dx.^2+sim_light_pz_ol_iff.Dy.^2))
rms(sqrt(sim_light_pz_cl_iff.Dx.^2+sim_light_pz_cl_iff.Dy.^2))