% Bike Suspension Model - Project 1
% Coded by: Kevin Abrahamson, Calvin Delbrueck, Trent Pearson

clear; clc

m      =                   90; % Slugs (accounts for both wheels)
I      =                10000; % THIS NEEDS TO BE CHANGED
k      =                  352; % Spring rate constant lbf/ft
c      =            sqrt(k*m); % Spring damper constant lbf/ft
D      =                29/12; % Diameter of wheel inches/12 (ft)
d      =                 0.50; % shock distance from com (m)
g      =                  9.8; % m/s^2
track  = @(x)     0.15*sin(x); % Bike track

% Domain Size
Lt=100; % Length of track
h=0.01; % Step-size

% Number of steps
N=round(Lt/h); % Number of times to go through loop

% Preallocate arrays
t=zeros(1,N);
x=zeros(1,N); % position of COm on x direction
Vx=zeros(1,N);  % shock length accel
y=zeros(1,N); % position of com in y dierction
Vy=zeros(1,N);  % shock length accel
th=zeros(1,N);  % angular position
Vth=zeros(1,N); % angular accel

% Initial Conditions
t(1)   = 0; % Start at time 0
SLx(1) = 0; % Shock length in x-direction
SLy(1) = 1; % Shock length in y-direction
xF(1)  = 1;
xR(1)  = 0;
yR(1)  = 0;
yF(1)  = 0;
th(1)  = 0;
x(1)   = 0;
Vx(1)  = 2;
y(1)   = 0;
Vy(1)  = 0;
th(1)  = 0;
Vth(1) = 0;

%Loop over track
for i=1:N
    % Update time
    t(i+1)=t(i)+h;

    % Update shock length

    % Shock length of front shock in the x-direction, min function keeps SL within bounds
    slxf(i+1)= min(SLx(1),sin(th(i))* shock_length(xF(i),yF(i),th(i),track,D));

    % Shock length of front shock in the y-direction, min function keeps SL within bounds
    slyf(i+1)= min(SLy(1),cos(th(i))* shock_length(xF(i),yF(i),th(i),track,D));

    % Shock length of rear shock in the y-direction, min function keeps SL within bounds
    slxr(i+1)=  min(SLx(1), sin(th(i))* shock_length(xR(i),yR(i),th(i),track,D)); % shock_length(xo,yo,th,T,D)

    % Shock length of rear shock in the y-direction, min function keeps SL within bounds
    slyr(i+1)=  min(SLy(1), cos(th(i))* shock_length(xR(i),yR(i),th(i),track,D));

    % dL/dt for front and rear shocks in x and y-directions
    vxf=(slxf(i+1)-slxf(i))/h;
    vxr=(slxr(i+1)-slxr(i))/h;
    vyf=(slyf(i+1)-slyf(i))/h;
    vyr=(slyr(i+1)-slyr(i))/h;

    % Update x-direction equations
    x(i+1)=x(i)+h*Vx(i);
    Vx(i+1)=Vx(i)+h*[-k/m*slxf(i)-c/m*vxf + -k/m*slxr(i)-c/m*vxr];

    % Update y-direction equations
    y(i+1)=y(i)+h*Vy(i);
    Vy(i+1)=Vy(i)+h*[-k/m*slyr(i)-c/m*vyf + -k/m*slyr(i)-c/m*vyr];

    %update anguar
    th(i+1)=th(i)+h*Vth(i);
    Vth(i+1)=Vth(i)+h*([-k*slxf(i)-c*vxf + -k*slyf(i)-c*vyf]/I - [-k*slxr(i)-c*vxr + -k*slyr(i)-c*vyr]/I);

    % Locations

    % Front shock locations
    xF(i+1)= x(i)+d*sin(th(i));
    yF(i+1)= y(i)+d*cos(th(i));

    % Rear shock locations
    xR(i+1)= x(i)-d*sin(th(i));
    yR(i+1)= y(i)-d*cos(th(i));

    xF1(i+1)=x(i)+2;
    yR1(i+1)=yR(i)+0.25;
    xR1(i+1)=x(i)-2;

    % Plot the solution
if (rem(i,10)==0)
    figure(1); clf(1)
    plot(xR1(i),yR1(i),'o')
    hold on
    plot(xF1(i),yF(i),'o')
    hold on
    plot(x(i),y(i),'x')
    xtrack=linspace(-1,Lt,1001);
    plot(xtrack,track(xtrack))
    ylim([-3,3])
    xlim([-1,25])
    drawnow

end % End if statement

end % End for loop