% This script both models the acceleration profiles and processes real data

%% Modelling the IMU system
r = 0.5*.0254; %m

%freq = 100; %Hz
freq = 200;
t_step = 1/freq;

max_rpm = 5; %RPM
max_omega = max_rpm*pi/30;

alpha_rpm = 5/60;
alpha = alpha_rpm*pi/30;

%t = 0:t_step:360; %Seconds
t = 0:t_step:((76/3)*60);
t = t(1:length(t)-2);

% Omega for slow acceleration, pos velocity, deceleration, then negative v
% To use this, uncomment the appropriate frequency and t vector
% omega(1:60/t_step) = alpha*t(1:60/t_step);
% omega(60/t_step:120/t_step) = max_omega;
% omega(120/t_step:240/t_step-1) = max_omega - alpha*t(1:120/t_step);
% omega(240/t_step:300/t_step) = -max_omega;
% omega(300/t_step:360/t_step-1) = -max_omega + alpha*t(1:60/t_step);

% Omega for five different velocities with 1 min of no motion in between
% (assuming instantaneous acceleration - actaully 250rpm/s)
% To use this, uncomment the appropriate frequency and t vector
omega(1:60/t_step) = 0;
omega(60/t_step:180/t_step) = max_omega;
omega(180/t_step:240/t_step) = 0;
omega(240/t_step:440/t_step) = max_omega * 3/5;
omega(440/t_step:500/t_step) = 0;
omega(500/t_step:800/t_step) = max_omega * 2/5;
omega(800/t_step:860/t_step) = 0;
omega(860/t_step:1460/t_step) = max_omega / 5;
omega(1460/t_step:1520/t_step - 1) = 0;

% figure;
% plot(t,omega);
% title('Omega vs. Time');
% xlabel('Time (s)');
% ylabel('Omega (rad/s)');

% Computing centripetal acceleration
acc = (omega.^2)*r;
%acc(180/t_step:length(acc)) = -acc(180/t_step:length(acc));
figure;
plot(t,acc);
title('Modeled Centripetal Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');

% Computing tangential velocity
% vel_tan = omega*r;
% figure;
% plot(t,vel_tan);
% title('Tangential velocity vs. time');
% xlabel('Time (s)');
% ylabel('Velocity (m/s)');

% Numerically integrating centripetal accel
integrated_acc = cumtrapz(t, acc);
% figure;
% plot(t, integrated_acc);
% title('Integrated centripetal acceleration');
% xlabel('Time (min)');
% ylabel('Velocity? (m/s)');

%% Load and analyze data
tenTurns = loadData('ambient_5_3_2_1RPM');
plotXYZ(tenTurns);

t_mins = 0:(1/6000):(length(tenTurns.accel)-1)/6000.0;
t_secs = 0:.005:(length(tenTurns.accel)-1)/200.0;

%xAccF1 = filter(filt1.tf.num, filt1.tf.den, tenTurns.accel(:,1));

vel = cumtrapz(t_secs,(tenTurns.accel(:,1)));
%velF1 = cumtrapz(t_secs,xAccF1);

% figure;
% subplot(2,1,1);
% plot(t_secs, vel);
% xlabel('Time (s)');
% ylabel('X velocity (m/s)');
% title('Integrated X acceleration');
% 
% vel = cumtrapz(t_secs,tenTurns.accel(:,2));
% subplot(2,1,2);
% plot(t_mins, vel);
% xlabel('Time (s)');
% ylabel('Y velocity (m/s)');
% title('Integrated Y acceleration');

% Filter signal then block average sections
x_acc = tenTurns.accel(:,1);
load('filt1.mat');
x_acc_f1 = filter(filt1.tf.num, filt1.tf.den, x_acc);

x_acc_f1_b = x_acc_f1;
x_acc_f1_b = x_acc_f1_b(~isnan(x_acc_f1_b));
x_acc_f1_b(1:15839) = mean(x_acc_f1(1:13120)); %zero section
x_acc_f1_b(15840:38780) = mean(x_acc_f1(15840:38780)); %high section
x_acc_f1_b(38781:52499) = mean(x_acc_f1(41580:49380)); %zero
x_acc_f1_b(52000:90350) = mean(x_acc_f1(52000:90350)); % high
x_acc_f1_b(90351:106899) = mean(x_acc_f1(93300:103400)); % zero
x_acc_f1_b(106900:164000) = mean(x_acc_f1(106900:164000)); %high
x_acc_f1_b(163999:177899) = mean(x_acc_f1(167000:175400)); % zero
x_acc_f1_b(177900:291900) = mean(x_acc_f1(177900:291900)); %high
x_acc_f1_b(291900:306329) = mean(x_acc_f1(297400:306329)); % zero

% figure;
% plot(t_secs, x_acc_f1_b, t, acc);

figure;
subplot(1,2,1);
plot(t_secs, x_acc);
title('Raw X Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');
set(gca, 'fontsize', 14);
subplot(1,2,2);
plot(t_secs, x_acc_f1);
title('Low Pass Filtered X Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');
set(gca, 'fontsize', 14);

figure;
plot(t_secs, x_acc);
hold on;
plot(t_secs, x_acc_f1, 'LineWidth', 3);
plot(t_secs, x_acc_f1_b, 'LineWidth', 4);
title('Three Stages of Data Processing');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');
set(gca, 'fontsize', 14);

figure;
plot(t_secs, x_acc_f1_b);
title('Filtered and Block Averaged Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');
set(gca, 'fontsize', 14);

% Integrate the new smoothed/block averaged accelerations
v_filt = cumtrapz(t_secs,x_acc_f1_b);
% figure;
% plot(t_secs, v_filt);
% xlabel('Time (s)');
% ylabel('Velocity (m/s)');
% title('Velocity calculated from filtered and block averaged accel');

figure;
yyaxis left;
plot(t, integrated_acc);
ylabel('Modeled Velocity (m/s)');
yyaxis right;
plot(t_secs, v_filt);
xlabel('Time (s)');
ylabel('Velocity Calculated From Processed Acceleration Data (m/s)');
set(gca, 'fontsize', 14);

figure;
yyaxis left;
plot(t, acc);
ylabel('Modeled Acceleration (m/s^2)');
yyaxis right;
plot(t_secs, x_acc_f1_b);
xlabel('Time (s)');
ylabel('Processed Acceleration Data (m/s^2)');
set(gca, 'fontsize', 14);