%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

%%This simulation implement the acceleration in the model
%% an RungeKutta4 solver is used to solve the differential equation
%% due to the term t^2 this method is not reliable for non straight
%% trajectories
clc
clear all
close all
%% vl,vr expressed in meters/seconds
%%the left wheel velocity
vl=0.03;
%%the right wheel velocity
vr=0.03;
%% the acceleration time
ar=0.6;
al=0.5;

%%the initial orientation of the robot
%%the angle is counter clockwise from the x axis
theta0=0
%%b is the axis length of the robot that connect the 2 wheels expressed in
%%meters
b=0.5;
%%the simulation time
tstart=0;
tend=5;
timestep=0.01;


%%now define the number of time steps
nsteps=(tend-tstart)/timestep;
%%this is the starting position for the robot
start_pos=[0,0,theta0];
position_pts=[start_pos];
old_pos=[start_pos];
lwheel_trace=LeftWheelLoc(start_pos,b);
rwheel_trace=RightWheelLoc(start_pos,b);

figure(1);
title(['Odometry simulation using acceleration...','ar =',num2str(ar),'al =',num2str(al)]);
xlabel('X coordinate');
ylabel('Y coordinate');
DrawRobotPosition(old_pos,b);
%%Take only 4 samples
tend4=tstart+3*timestep;

[Tinit,Xinit,Theta]=RungeKutta4(tstart,tend4,4,ar,al,vr,vl,b,theta0,'x');
[Tinit,Yinit,Theta]=RungeKutta4(tstart,tend4,4,ar,al,vr,vl,b,theta0,'y');

Theta=[];

[T,X,Theta]=AdamsbmAcc(tstart,tend,nsteps,Xinit(1:4),ar,al,vr,vl,b,theta0,'x');
[T,Y,Theta]=AdamsbmAcc(tstart,tend,nsteps,Yinit(1:4),ar,al,vr,vl,b,theta0,'y');

figure(1);
DrawRobotPosition(old_pos,b);
for t=1:length(X)
    position=[X(t,1) Y(t,1) Theta(t+1)];
    DrawRobotPosition(position,b);
    pause(0.3);
end