clear; clc
% Bike Project Model

% Define Constants
m  =            5.97;    % Slugs
I  =               1;    % THIS NEEDS TO BE CHANGED
k  =          325*12;    % Spring rate constant lbf/ft
c  =       sqrt(k*m);    % Spring damper constant lbf/ft
wheelD  =           29/12;    % Diameter of wheel inches/12 (ft)
g  =            32.2;    % ft/s^2
SLxo =             0;    % Initial length of shock in x-direction
SLyo =             1;    % Initial length of shock in y-direction
track  = @(t) sin(t);    % Bike track
theta = deg2rad(0);

% Define the ODE

% ODE in x-direction
fx = @(t,SLx,Vx) Vx; % Velocity of shock in x-direction (dx/dt)
fVx = @(t,SLx,Vx) -k/m*(SLx-SLxo)-c/m*Vx; % Acceleration of shock in x-direction (d^2x/dt^2)

% ODE in y-direction
fy=@(t,SLy,Vy) Vy; % Velocity of shock in y-direction (dy/dt)
fVy=@(t,SLy,Vy) -k/m*(SLy-SLyo)-c/m*Vy; % Acceleration of shock in y-direction (d^2y/dt^2)

% ODE in angular direction
% ##########
% ##########

% Domain size
trackLength = 5*pi; % Track length
h           = .01; % Step-size

% Number of steps
N = round(trackLength/h); % Calculates number of steps

% Preallocate
t  = zeros(1,N); % Time array
x  = zeros(1,N); % Velocity in x-direction array
Vx = zeros(1,N); % Acceleration in x-direction array
y  = zeros(1,N); % Velocity in y-direction array
Vy = zeros(1,N); % Acceleration in y-direction array

% Initial Conditions
t(1)   = 0; % Starts at time 0
SLx(1) = 0; % Initial shock length in the x-direction
Vx(1)  = 0; % Initial velocity in the x-direction
SLy(1) = 1; % Initial shock length in the y-direction
Vy(1)  = 0; % Initial velocity in the y-direction

% Loop over time
for i=1:N-1
    t(i+1)  = t(i)+h;
    Ls(i+1)  = shock_length(t(i),SLyo,theta,track,wheelD);  % shock length at each step
    Lsx(i+1) = Ls(i)*sin(theta);                            % Length of shock in x-direction
    Lsy(i+1) = Ls(i)*cos(theta);                            % Length of shock in y-direction
    x(i+1)  = x(i)+h*fx(t(i),SLx(i),Vx(i));
    Vx(i+1) = Vx(i)+h*fVx(t(i),SLx(i),Vx(i));
    y(i+1)  = y(i)+h*fy(t(i),SLy(i),Vy(i));
    Vy(i+1) = Vy(i)+h*fVy(t(i),SLy(i),Vy(i));
end

% Plot the solution
figure (1); clf(1)
plot(t,x)
hold on
plot(t,Vx)
plot(t,y)
plot(t,Vy)