%% We reset all variables and clean the mess
clc
clear
% close all

%% We define a few relevant parameters for the simulation
% Characteristics of the vehicle:
d=0.3; %distance between the wheels

% Controller settings:
gain=0.15;
% The control law (as described in the document) is:
control_V=@(VF,VS) [VF-gain*VS*sign(VF);VF+gain*VS*sign(VF)];

% Speed command: we assume that the operator wants to move in a certain 
% direction (e.g. to the North) and therefore they will keep aiming the 
% joystick in that direction as the vehicle moves
VrefX=0.1;
VrefY=0.1;
control_Vref=@(theta1,theta2) [VrefX*cos(theta1+theta2)+VrefY*sin(theta1+theta2);VrefX*sin(theta1+theta2)-VrefY*cos(theta1+theta2)];

% Simulation parametres:
Tsim=1e-3; % what is the time step required to perform the simulation (1 ms)
Tcontrol=1e-2; % what is the sampling period of the speed controller (10 ms)
Ncontrol=Tcontrol/Tsim;

tmax_sim=50; % what is the time horizon of the simulation (100 s)
SimSteps=tmax_sim/Tsim;
ControlSteps=floor(tmax_sim/Tcontrol);

% Two functions that are used to calculate the speed of the vehicle once
% the speed of the two wheels is known and the orientation of the vehicle
% is known:
% speed
calc_VG=@(VR,VL,theta1,theta2) (VR+VL)/2*[cos(theta1+theta2);sin(theta1+theta2)];
% rotational speed
calc_Omega=@(VR,VL) (VR-VL)/(2*d);

% The following variables are used during the simulation
m=1; % this counts the number of simulation steps 
n=1; % this counts the number of control cycles
VR=0;
VL=0;
theta1=0;
theta2=0;
theta12=0;
x=5; % this is the initial position of the vehicle
y=5;
vgx=0;
vgy=0;

% The following tables will store the value of different variables during
% the simulation:
tab_t=zeros(1,SimSteps);
tab_x=zeros(1,SimSteps);
tab_y=zeros(1,SimSteps);
tab_vgx=zeros(1,SimSteps);
tab_vgy=zeros(1,SimSteps);
tab_VF=zeros(1,SimSteps);
tab_VS=zeros(1,SimSteps);
tab_VR=zeros(1,SimSteps);
tab_VL=zeros(1,SimSteps);
tab_theta1=zeros(1,SimSteps);
tab_theta2=zeros(1,SimSteps);

% The following loop is the main loop of the simulation that runs from t=0
% until the end of the simulation and updates the control signal of the
% speed controller and calculates the position of the vehicle
for t=0:Tsim:tmax_sim
    % the following code is the code of the controller, which updates the
    % speed of the wheels in order to make the vehicle follow the commands
    % of the operator:
    if(mod(t,Tcontrol)==0)
        temp1=control_Vref(theta1,theta2);      
        temp2=control_V(temp1(1),temp1(2));
        VR=temp2(1);
        VL=temp2(2);
        n=n+1;
    end
    % the following lines calculate the speeds and the new position of the
    % vehicle
    temp3=calc_VG(VR,VL,theta1,theta2);
    vgx=temp3(1);
    vgy=temp3(2);
    x=x+vgx*Tsim;
    y=y+vgy*Tsim;
    temp4=calc_Omega(VR,VL);
    theta12=theta12+temp4*Tsim;
    theta1=atan2(y,x);
    theta2=mod(theta12-theta1,2*pi);

    % the following code stores the data point in the tables
    tab_t(m)=t;
    tab_x(m)=x;
    tab_y(m)=y;
    tab_vgx(m)=vgx;
    tab_vgy(m)=vgy;
    tab_VF(m)=temp1(1);
    tab_VS(m)=temp1(2);
    tab_VR(m)=VR;
    tab_VL(m)=VL;
    tab_theta1(m)=theta1;
    tab_theta2(m)=theta2;
    m=m+1;
end

%% Once the simulation is over, we generate some plots to show the results:

% first we produce an animation of the position of the vehicle
Texport=1;
Nexport=tmax_sim/Texport;
p=figure('Position',[100,100,600,600],'Name',['Vrefx= ', num2str(VrefX),...
    ' m/s, Vrefy= ', num2str(VrefY), 'm/s']);
for l=1:Nexport    
    plot(tab_x(1:(l*Texport/Tsim)),tab_y(1:(l*Texport/Tsim)),'k--');
    hold on; grid on
    plot_vehicle(tab_x(l*Texport/Tsim),tab_y(l*Texport/Tsim),...
        tab_theta1(l*Texport/Tsim),tab_theta2(l*Texport/Tsim),...
        tab_vgx(l*Texport/Tsim),tab_vgy(l*Texport/Tsim),d);
    p.CurrentAxes.XTick=0:1:10;   
    p.CurrentAxes.YTick=0:1:10;
    title(['t= ' num2str(tab_t(l*Texport/Tsim)) ' s']);
    xlabel('x [m]');
    ylabel('y [m]');
%     print(['./frames/f' num2str(l,'%04d') '.png'],'-dpng');
    pause(0.01);    
end
%run 'ffmpeg -i ./frames/f%04d.png ./anim.mp4' in order to generate the
%movie

%%
p=figure('Position',[100,100,400,700],'Name',['Vrefx= ', num2str(VrefX),...
    ' m/s, Vrefy= ', num2str(VrefY), 'm/s']);
subplot(4,1,1);
plot(tab_t,tab_vgx,'r');
grid on
hold on
plot(tab_t,VrefX*ones(1,length(tab_t)),'r--');
plot(tab_t,tab_vgy,'b');
plot(tab_t,VrefY*ones(1,length(tab_t)),'b--');
axis([0 tmax_sim -0.15 0.15]);
legend('v_{Gx}','v_{Gx}^*','v_{Gy}','v_{Gy}^*');
ylabel('v [m/s]');

subplot(4,1,2);
plot(tab_t,tab_VF,'r');
grid on
hold on
plot(tab_t,tab_VS,'b');
axis([0 tmax_sim -0.15 0.15]);
legend('V_F','V_S');
ylabel('v [m/s]');

subplot(4,1,3);
plot(tab_t,tab_VR,'r');
grid on
hold on
plot(tab_t,tab_VL,'b');
axis([0 tmax_sim -0.15 0.15]);
legend('V_R','V_L');
ylabel('v [m/s]');

subplot(4,1,4);
plot([-Tcontrol, tab_t(1:Ncontrol:(SimSteps-Ncontrol))],[tab_vgx(1)/Tsim (tab_vgx(Ncontrol:Ncontrol:(SimSteps-1))-tab_vgx(1:Ncontrol:(SimSteps-Ncontrol)))/Tsim],'r');
grid on
hold on
plot([-Tcontrol, tab_t(1:Ncontrol:(SimSteps-Ncontrol))],[tab_vgy(1)/Tsim (tab_vgy(Ncontrol:Ncontrol:(SimSteps-1))-tab_vgy(1:Ncontrol:(SimSteps-Ncontrol)))/Tsim],'b');
axis([0 tmax_sim -0.15 0.15]);
legend('A_{Gx}','A_{Gy}');
ylabel('a [m/s^2]');
xlabel('Time [s]');

%print(['plot_' num2str(VrefX) '_' num2str(VrefY) '.png'],'-dpng');

