clear all
close all
clc

%% Load the acceleration data (in m/s^2 not g-normalized)

% load("Acc_x.csv")
% load("Acc_y.csv")
% load("Acc_z.csv")

x_acc_1 = Acc_x;
y_acc_1 = Acc_y;
z_acc_1 = Acc_z;

m = min([length(x_acc_1),length(y_acc_1),length(z_acc_1)]); %equalize the length

x_acc = x_acc_1(1:m,1);
y_acc = y_acc_1(1:m,1);
z_acc = z_acc_1(1:m,1);

%% Define the filter (here a 4th order Butterworth band pass)

fs = 50; % sampling frequency of acceleration data by IMU (in Hz)
butter_ord = 4;

fc = 10; % cutting frequency for low pass (in Hz)
[b, a] = butter(butter_ord, fc/(fs/2), 'low'); % Butterworth low pass filter of 4th order

fc_high = 0.5; % cutting frequency for high pass filter (in Hz)
[d, c] = butter(butter_ord, fc_high/(fs/2), 'high'); % Butterworth filter high-pass of 4th order

%% Y-axis position reconstruction

samples = (0:0.02:(length(y_acc)-1)*0.02)'; %discrete samples of acquisition

y_filtered = filtfilt(b, a, y_acc); % low pass filtering

y_acc_filt = filtfilt(d, c, y_filtered); % high-pass filtering

figure %plot acceleration data original vs filtered
plot(samples,y_acc)
hold on
plot(samples,y_acc_filt)
xlabel("Time [s]")
ylabel("Acc [m/s^2]")
title("Acceleration along Y axis")
grid on
legend("original","yddot band pass")

%numerical cumulative integration to get the speed from acceleration
y_speed = cumtrapz(samples,y_acc_filt);
figure
plot(samples,y_speed,"b")
xlabel("Time [s]")
ylabel("Speed [m/s]")
grid on
title("Speed along Y axis")

%filter speed data to clean residual noise
y_speed_filt = filtfilt(d, c, y_speed);

%numerical cumulative integration of the speed to get the position
y_pos = cumtrapz(samples,y_speed_filt);

%Plot of the position
figure
plot(samples,y_pos)
grid on
xlabel("Time [s]")
ylabel("Position [m]")
title("Position along Y axis")

%% X-axis position reconstruction (same as Y)

x_filtered = filtfilt(b, a, x_acc); 
x_acc_filt = filtfilt(d, c, x_filtered);

figure
plot(samples,x_acc)
hold on
plot(samples,x_acc_filt)
title("Acceleration along X axis")
grid on
xlabel("Time [s]")
ylabel("Acc [m/s^2]")
legend("original","xddot band pass")

x_speed = cumtrapz(samples,x_acc_filt);
figure
plot(samples,x_speed,"b")
grid on
xlabel("Time [s]")
ylabel("Speed [m/s]")
title("Speed along X axis")

x_speed_filt = filtfilt(d, c, x_speed);

x_pos = cumtrapz(samples,x_speed_filt);
figure
plot(samples,x_pos)
grid on
xlabel("Time [s]")
ylabel("Position [m]")
title("Position along X axis")

%% Z-Axis position reconstruction (Same as Y and X)

z_filtered = filtfilt(b, a, z_acc); 
z_acc_filt = filtfilt(d, c, z_filtered);

figure
plot(samples,z_acc)
hold on
plot(samples,z_acc_filt)
title("Acceleration along Z axis")
xlabel("Time [s]")
ylabel("Acceleration [m/s^2]")
grid on
legend("Original z acc","zddot band pass")

z_speed = cumtrapz(samples,z_acc_filt);
figure
plot(samples,z_speed,"b")
xlabel("Time [s]")
ylabel("Speed [m/s]")
grid on
title("Speed along Z-axis")

z_speed_filt = filtfilt(d, c, z_speed);

z_pos = cumtrapz(samples,z_speed_filt);

figure
plot(samples,z_pos)
grid on
xlabel("Time [s]")
ylabel("Position [m]")
title("Position along Z axis")

%% Reconstruct the trajectory in 3D
figure
h = plot3(x_pos(1), y_pos(1),z_pos(1),'-o'); %define the 3d plot
xlabel('X axis position [m]')
ylabel('Y axis position [m]')
zlabel('Z axis position[m]')
title('Oscillating Trajectory Over Time')
grid on
maxdist = max([x_pos;y_pos;z_pos]);
mindist = min([x_pos;y_pos;z_pos]);

axis([min(x_pos) max(x_pos) min(y_pos) max(y_pos) min(z_pos) max(z_pos)])
hold on
pause(1.3)
% Animation loop
tailLength = 100;  % Number of points to display in the tail
for k = 1:m
    startIdx = max(1, k-tailLength);
    set(h, 'XData', x_pos(startIdx:k), 'YData', y_pos(startIdx:k), 'ZData', z_pos(startIdx:k));
    drawnow
    pause(0.01);
end