%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

%% Add the getAsynchronousError to path
addpath('./src/');

% Load Measurement Data

spindle_1rpm_table  = readtable('./mat/10turns_1rpm_icepap.txt');
spindle_60rpm_table = readtable('./mat/10turns_60rpm_IcepapFIR.txt');

spindle_1rpm_table(1, :)

spindle_1rpm  = table2array(spindle_1rpm_table);
spindle_60rpm = table2array(spindle_60rpm_table);

% Convert Signals from [deg] to [sec]

speed_1rpm = 360/60; % [deg/sec]
spindle_1rpm(:, 1) = spindle_1rpm(:, 2)/speed_1rpm;  % From position [deg] to time [s]

speed_60rpm = 360/1; % [deg/sec]
spindle_60rpm(:, 1) = spindle_60rpm(:, 2)/speed_60rpm; % From position [deg] to time [s]

% Convert Signals

% scaling = 1/80000; % 80 mV/um
scaling = 1e-6; % [um] to [m]

spindle_1rpm(:, 3:end) = scaling*spindle_1rpm(:, 3:end); % [V] to [m]
spindle_1rpm(:, 3:end) = spindle_1rpm(:, 3:end)-mean(spindle_1rpm(:, 3:end)); % Remove mean

spindle_60rpm(:, 3:end) = scaling*spindle_60rpm(:, 3:end); % [V] to [m]
spindle_60rpm(:, 3:end) = spindle_60rpm(:, 3:end)-mean(spindle_60rpm(:, 3:end)); % Remove mean

% Ts and Fs for both measurements

Ts_1rpm = spindle_1rpm(end, 1)/(length(spindle_1rpm(:, 1))-1);
Fs_1rpm = 1/Ts_1rpm;

Ts_60rpm = spindle_60rpm(end, 1)/(length(spindle_60rpm(:, 1))-1);
Fs_60rpm = 1/Ts_60rpm;

% Find Noise of the ADC [$\frac{m}{\sqrt{Hz}}$]

data = spindle_1rpm(:, 5);
dV_1rpm = min(abs(data(1) - data(data ~= data(1))));
noise_1rpm = dV_1rpm/sqrt(12*Fs_1rpm/2);

data = spindle_60rpm(:, 5);
dV_60rpm = min(abs(data(50) - data(data ~= data(50))));
noise_60rpm = dV_60rpm/sqrt(12*Fs_60rpm/2);

% Save all the data under spindle struct

spindle.rpm1.time = spindle_1rpm(:, 1);
spindle.rpm1.deg  = spindle_1rpm(:, 2);
spindle.rpm1.Ts   = Ts_1rpm;
spindle.rpm1.Fs   = 1/Ts_1rpm;
spindle.rpm1.x    = spindle_1rpm(:, 3);
spindle.rpm1.y    = spindle_1rpm(:, 4);
spindle.rpm1.z    = spindle_1rpm(:, 5);
spindle.rpm1.adcn = noise_1rpm;

spindle.rpm60.time = spindle_60rpm(:, 1);
spindle.rpm60.deg  = spindle_60rpm(:, 2);
spindle.rpm60.Ts   = Ts_60rpm;
spindle.rpm60.Fs   = 1/Ts_60rpm;
spindle.rpm60.x    = spindle_60rpm(:, 3);
spindle.rpm60.y    = spindle_60rpm(:, 4);
spindle.rpm60.z    = spindle_60rpm(:, 5);
spindle.rpm60.adcn = noise_60rpm;

% Compute Asynchronous data

for direction = {'x', 'y', 'z'}
    spindle.rpm1.([direction{1}, 'async']) = getAsynchronousError(spindle.rpm1.(direction{1}), 10);
    spindle.rpm60.([direction{1}, 'async']) = getAsynchronousError(spindle.rpm60.(direction{1}), 10);
end

% Save data

save('./mat/spindle_data.mat', 'spindle');