function safeDistance ()
% safeDistance()
% 
% Calculate the safe distance between two cars travelling behind each
% other, when the front car suddenly brakes, and the rear car is first
% accerlerating (during reaction time) and finally also brakes.
%
% Author: Alexander Braun
% Version: v1
% Date: 2018-03-02

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Init: variables and stuff
%
% index F is always the Front car (cF)
% index R is always the Rear car (cR)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% base distances
LR = 5; % m
LF = 5; % m
L = (LR + LF)./2; % m
dmin = 30; % m  - this will be calculated later on, just to have a value

% base velocities
vR0 = 12; % m/s - starting velocity of cF
vF0 = 18; % m/s - starting velocity of cR

% base acceleration 
aA = 4; % m/s^2
aB = -6; % m/s^2

% base times
Treact = 2; % s - rho in the paper
tRes = 0.01; % s - time resolution of this simulation

% dependent variables
vRmax = vR0 + Treact.*aA; % m/s - maximum velocity of cR after reaction time
TR = Treact + abs(vRmax./aB); % s - time until cR stops
TF = abs(vF0 ./ aB); % s - time until cF stops



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% simulation time
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t = [0:tRes:TR]; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Calculation
%
% The whole process in the paper is divided into three parts:
% 1. reaction time
% 2. time until cF stops (TF)
% 3. time until cR stops (TR)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate minimum distance as per Mobileye paper
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dThusitha = Treact.^2.*aA.*0.5;
%dThusitha = 0;
dminPaper = L + TF.*( (vRmax - vF0) - Treact.*aB) + Treact.^2.*aB.*0.5 + 0.5.*(TR - TF).*(vRmax + (TF - Treact).*aB);
dminThusitha = dminPaper - dThusitha;
%dmin = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% now calculate using our own derivation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% velocities
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% init
vF = zeros(size(t)); 
vR = zeros(size(t)); 

% front car
vF = vF0 + aB.*t; 
vF(vF < 0) = 0; % don'r reverse, stand still

% rear car
% cR is separated into two parts: acceleration and deceleration
% we separate into two zones: before and after reaction time
idxT1 = (t <= Treact);
idxT2 = (t > Treact);
vR(idxT1) = vR0 + aA.*t(idxT1);
vRmaxIdx = find(vR, 1, 'last');
vRmax2 = vR(vRmaxIdx);
vRaccuracy = vRmax2 - vRmax; % difference due to the time resolution
disp(['v_{R, max} error: ' num2str(vRaccuracy) ' m/s']);
vR(idxT2) = vRmax + aB.*(t(idxT2) - Treact);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% distances
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We calculate three minimum distances:
% dMinInt: the final distance between xf(tr) and xr(tr) after integration
% dMinPaper: as per paper
% dMinThusitha: as per Thusita Parakrama formula, with the extra (negative)
% term
% Note that dminInt does not include the length L (from the car lengths),
% which explains the difference between dMinInt and dMinThusitha. 

% init
xF = zeros(size(t)); 
xR = zeros(size(t));

% cF is easy: simply brake to complete standstill
% we separate into two zones: before and after stand still
idxTF = (t <= TF);
xF(idxTF) = 0.5.*aB.*t(idxTF).^2 + vF0.*t(idxTF);
xF(t>TF) = xF(find(xF, 1, 'last'));

% we separate into two zones: before and after reaction time
xR(idxT1) = 0.5.*aA.*t(idxT1).^2 + vR0.*t(idxT1);
xRReact = xR(find(xR, 1, 'last'));
xR(idxT2) = xR(vRmaxIdx) + 0.5.*aB.*(t(idxT2) - Treact).^2 + vRmax.*(t(idxT2) - Treact);

% minimum distance
dminInt = xR(end) - xF(end);
disp(['Car length L: ' num2str(L) ' m']);
disp(['Min safe distance (integrated): ' num2str(dminInt) ' m']);
disp(['Min safe distance (paper): ' num2str(dminPaper) ' m']);
disp(['Min safe distance (Thusitha): ' num2str(dminThusitha) ' m']);
disp(['Just the Thusitha-difference: ' num2str(dThusitha) ' m']);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% draw output
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% distance
xFig = 1;
figure(xFig);
clf; hold on;
plot(t, xF + dminInt, 'b-', 'LineWidth', 2);
%plot(t, xF + dminInt + L, 'b-', 'LineWidth', 2);
%plot(t, xF, 'b-', 'LineWidth', 2);
plot(t, xR, 'g-', 'LineWidth', 2);
xlabel('t (s)');
ylabel('Distance (m)');
legend('x_F', 'x_R', 'Location', 'best');
strFormat = '%5.2f';
text(3, 6, ['v_{R,max} = ' num2str(vRmax, strFormat) ' m/s']);
text(3, 9, ['T_R = ' num2str(TR, strFormat) ' s']);
text(3, 12, ['T_F = ' num2str(TF, strFormat) ' s']);
text(3, 15, ['x_F(T_F) = ' num2str(xF(end), strFormat) ' m']);
text(3, 18, ['x_R(T_R) = ' num2str(xR(end), strFormat) ' m']);
text(3, 21, ['x_R(\rho) = ' num2str(xRReact, strFormat) ' m']);
text(3, 24, ['Real d_{min} = ' num2str(dminInt, strFormat) ' m']);

% velocity
vFig = 2;
figure(vFig);
clf; hold on;
plot(t, vF, 'b-', 'LineWidth', 2);
plot(t, vR, 'g-', 'LineWidth', 2);
xlabel('t (s)');
ylabel('Velocity (m/s)');
legend('v_F', 'v_R', 'Location', 'best');


end