% Run analysis on data


clear;clc;close all;
load('drivehome_data.mat');
data_coefficients;

time_calib = time+.2/60;
x_accel_calib = x_accel + 1.3;
y_accel_calib = y_accel - 1.3;
z_accel_calib = z_accel + .65;

x_jerk = diff(x_accel_calib);
y_jerk = diff(y_accel_calib);
z_jerk = diff(z_accel_calib);
y_jerk(359)=y_jerk(358);

phi=0;
phi_rad = deg2rad(phi);
for (i=1:1:3)
    ra(i) = .5*(atl(i) - dtl(i));
    rj(i) = jtl(i);
    ca(i) = (atl(i)-ra(i));
    cj(i) = (jtl(i)-rj(i));
    for (theta=0:1:360)
        theta_rad = deg2rad(theta);
        a(i, theta+1) = ra(i)*cos(phi_rad)*cos(theta_rad)-rj(i)*sin(phi_rad)*sin(theta_rad)+ca(i);
        j(i, theta+1) = rj(i)*cos(phi_rad)*sin(theta_rad)+ra(i)*sin(phi_rad)*cos(theta_rad)+cj(i);
    end
end

hold on
plot(a(1,:),j(1,:))
plot(a(2,:),j(2,:))
plot(a(3,:),j(3,:))
plot(y_accel_calib/9.81, y_jerk, '*');
xlabel('y accceleration (g)');
ylabel('y jerk (m/s^3)');