clear; clc

% Define the ODe
m  =          5.97/2;    % Slugs (accounts for both wheels)
I  =              15;    % THIS NEEDS TO BE CHANGED
k  =          325*12;    % Spring rate constant lbf/ft
c  =       sqrt(k*m);    % Spring damper constant lbf/ft
D  =      29/12;% Diameter of wheel inches/12 (ft)
d  =             1.5;    % shock distance from com
g  =            32.2;    % ft/s^2
SLxo =             0;    % Initial length of shock in x-direction
Slyo =             1;    % Initial length of shock in y-direction feet
track  = @(x) .15*sin(x);    % Bike track
theta = deg2rad(0);
%   u]/I;


% Domain Size (How long we want this to run for)
Lt=20;  % feet
h=0.1;

% Number of steps
N=round(Lt/h);

% Preallocate
t=zeros(1,N);
x=zeros(1,N); % position of COm on x direction
v_x=zeros(1,N);  % shock length accel
y=zeros(1,N); % position of com in y dierction
u_y=zeros(1,N);  % shock length accel
th=zeros(1,N);  % angular position
v_th=zeros(1,N); % angular accel
slf=zeros(1,N);
slr=zeros(1,N);
xf=zeros(1,N);
yf=zeros(1,N);
xr=zeros(1,N);
yr=zeros(1,N);
% Initial Conditions
t(1)=0;
slx(1)=0;
sly(1)=1;


xf(1)=3; % initial positions of top of shocks in ft
xr(1)=0;
yr(1)=3;
yf(1)=3;
th(1)=0;

slfo=1; % initial shock length front
slro=1; % Initial shock length rear

slf(1)=1;
slr(1)=1;

x(1)=0;
v_x(1)=0;
y(1)=0;
u_y(1)=0;
th(1)=0;
v_th(1)=0;

%Loop Over Time
for i=1:N
    t(i+1)=t(i)+h;

    % update shock length
    %     slxf(i+1)= min(SLxo,sin(th(i))* shock_length(xf(i),yf(i),th(i),track,D));
    %     slyf(i+1)= min(SLyo,cos(th(i))* shock_length(xf(i),yf(i),th(i),track,D));
    %
    %     slxr(i+1)=  min(SLxo, sin(th(i))* shock_length(xr(i),yr(i),th(i),track,D)); % shock_length(xo,yo,th,T,D)
    %     slyr(i+1)=  min(SLyo, cos(th(i))* shock_length(xr(i),yr(i),th(i),track,D));
    %
    slf(i+1)= shock_length(xf(i),yf(i),th(i),track,D);
    slr(i+1)= shock_length(xr(i),yr(i),th(i),track,D);


    % fron shock constraints
    if slf(i+1) > Slyo
        slf(i+1)= Slyo;
    end

    if slf(i+1) < 0
        slf(i+1) = yf(i);
    end
    % rear shock constraints
    if slr(i+1) > Slyo
        slr(i+1)= Slyo;
    end

    if slr(i+1) < 0
        slr(i+1) = yr(i);
    end





    % change in shock length (v) in eq

    vf=(slf(i+1)-slf(i))/h;
    vr=(slr(i+1)-slr(i))/h;

    %update xdirection
    x(i+1)=x(i)+h*v_x(i);
    v_x(i+1)=v_x(i)+h*(-k/m*(slr(i)*sin(th(i))-slro*sin(th(i)))-c/m*(vr*sin(th(i)))-k/m*(slf(i)*sin(th(i))-slfo*sin(th(i)))-c/m*vf*sin(th(i)));

    %update y direction
    y(i+1)=y(i)+h*u_y(i);
    u_y(i+1)=u_y(i)+h*(-k/m*(slr(i)*cos(th(i))-slro*cos(th(i)))-c/m*(vr*cos(th(i)))-k/m*(slf(i)*cos(th(i))-slfo*cos(th(i)))-c/m*vf*cos(th(i)));

    %update anguar
    th(i+1)=th(i)+h*v_th(i);
    v_th(i+1)=v_th(i)+h*(((-k/m*(slr(i)*sin(th(i))-slro*sin(th(i)))-c/m*(vr*sin(th(i)))-k/m*(slr(i)*cos(th(i))-slro*cos(th(i)))-c/m*(vr*cos(th(i)))...
        +k/m*(slf(i)*sin(th(i))-slfo*sin(th(i)))+c/m*vf*sin(th(i)))+k/m*(slf(i)*cos(th(i))-slfo*cos(th(i)))+c/m*vf*cos(th(i))))/I;



    % Locations of top of shocks
    xf(i+1)= x(i)+d*sin(th(i));
    yf(i+1)= y(i)+d*cos(th(i));
    xr(i+1)= x(i)-d*sin(th(i));
    yr(i+1)= y(i)-d*cos(th(i));






    %     %plot
    hold on
    plot([xr(i),yr(i)],[xf(i),yf(i)])   % bike frame line
    %
    %
    %      % rear wheel
    %      plot(xb(i),yb(i)-shock lenght(i),'ko')
    drawnow
end