%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 4 Corners Model With Suspension Simulation
% MAIN - by Dr. Efstathios Velenis & Bernat Carreras
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%re%%%%%%%%

clear all
close all
clc

% DEFINICIĶ VARIABLES
global m lf lr tf tr Jp Jx Jy W B C D Jwf Rwf Jwr Rwr h Kf Kr Cf Cr maxsteer toef toer
global BrkConstF BrkConstR AccConstF AccConstR

% Importaciķ variables
type exp.txt;
fileID = fopen('exp.txt','r');
formatSpec = '%f';
sizeA = [Inf 1];
var_entrada = fscanf(fileID,formatSpec)
fclose(fileID);


% GEOMETRIA
m = var_entrada(1);%mass in kg
lf = var_entrada(2); 
lr = var_entrada(3); 
tf = 1.4; tr = 1.4; 
Jp = 2741.9; Jx = 1000; Jy = 1000; W = m*9.81;
Jwf = 1.8; Jwr = 1.8;
Rwf = var_entrada(4); 
Rwr = var_entrada(4);
B = 7; C = 1.6; D = 0.9; %tyre parameters
h = 0.35; %CoG height
toef = 0* (2*pi/360); %toe-in, units in rad
toer = 0* (2*pi/360); 

% PARĀMETRES DE SUSPENSIĶ
Kf = var_entrada(7); 
Kr = var_entrada(8); 
Cf = var_entrada(9); 
Cr = var_entrada(10); 

% PARĀMETRES DEL MOTOR / FRENS
%driving & engine constants
maxsteer = 30; %deg
BrkConstF = 500; %mN
BrkConstR = 400;
AccConstF = var_entrada(5);
AccConstR = var_entrada(6);

% DADES INICIALITZACIĶ
% POSICIĶ INICIAL DEL CDG
x0 = 0;
y0 = 0;
z0 = h;

% VELOCITAT INICIAL
xdot0 = var_entrada(33)/3.6;
ydot0 = 0;
zdot0 = 0;

%ANGLES INICIALS DEL COTXE
psi0 = 0;
psidot0 = 0;
theta0 = 0;
thetadot0 = 0;
phi0 = 0;
phidot0 = 0;
omegaFL0 = sqrt(xdot0^2 + ydot0^2)/Rwf;
omegaFR0 = sqrt(xdot0^2 + ydot0^2)/Rwf;
omegaRL0 = sqrt(xdot0^2 + ydot0^2)/Rwr;
omegaRR0 = sqrt(xdot0^2 + ydot0^2)/Rwr;

% SIMULACIĶ
e0 = [x0 xdot0 y0 ydot0 z0 zdot0 psi0 psidot0 theta0 thetadot0 phi0 phidot0 omegaFL0 omegaFR0 omegaRL0 omegaRR0]';
tspan = [0:0.01:10];
options = [];
[t,e] = ode45('FWeom',tspan,e0,options);

%============================================================
%============================================================
%============================================================

x = e(:,1);
xdot = e(:,2);
y = e(:,3);
ydot = e(:,4);
z = e(:,5);
zdot = e(:,6);
psi = e(:,7);
psidot = e(:,8);
theta = e(:,9);
thetadot = e(:,10);
phi = e(:,11);
phidot = e(:,12);
omegaFL = e(:,13);
omegaFR = e(:,14);
omegaRL = e(:,15);
omegaRR = e(:,16);


for i = 1:length(t)
    delta = steer_fun(t(i));
    deltaFL(i) = delta(1);
    deltaFR(i) = delta(2);
    deltaRL(i) = delta(3);
    deltaRR(i) = delta(4);
    clear delta
    torque = accbrk_fun(t(i),omegaFL(i),omegaFR(i), omegaRL(i), omegaRR(i));
    TFL(i) = torque(1);
    TFR(i) = torque(2);
    TRL(i) = torque(3);
    TRR(i) = torque(4);
    clear torque
    
    V(i) = sqrt(xdot(i)^2 + ydot(i)^2);
    
    rho(i) = atan2(ydot(i),xdot(i));
    beta(i) = rho(i)-psi(i);


%============================================================
%   Vij
%============================================================
VFLx(i) = V(i)*cos(beta(i) - deltaFL(i)) + psidot(i)*lf*sin(deltaFL(i)) - psidot(i)*(tf/2)*cos(deltaFL(i));
VFLy(i) = V(i)*sin(beta(i) - deltaFL(i)) + psidot(i)*lf*cos(deltaFL(i)) - psidot(i)*(tf/2)*sin(deltaFL(i));

VFRx(i) = V(i)*cos(beta(i) - deltaFR(i)) + psidot(i)*lf*sin(deltaFR(i)) - psidot(i)*(tf/2)*sin(deltaFR(i));
VFRy(i) = V(i)*sin(beta(i) - deltaFR(i)) + psidot(i)*lf*cos(deltaFR(i)) + psidot(i)*(tf/2)*sin(deltaFR(i));

VRLx(i) = V(i)*cos(beta(i) + deltaRL(i)) + psidot(i)*lr*sin(deltaRL(i)) - psidot(i)*(tr/2)*cos(deltaRL(i));
VRLy(i) = V(i)*sin(beta(i) + deltaRL(i)) - psidot(i)*lr*cos(deltaRL(i)) - psidot(i)*(tr/2)*sin(deltaRL(i));

VRRx(i) = V(i)*cos(beta(i) - deltaRR(i)) - psidot(i)*lr*sin(deltaRR(i)) + psidot(i)*(tr/2)*cos(deltaRR(i));
VRRy(i) = V(i)*sin(beta(i) - deltaRR(i)) - psidot(i)*lr*cos(deltaRR(i)) - psidot(i)*(tr/2)*sin(deltaRR(i));


%============================================================
%   Slip
%============================================================
slip = slip_fun(VFLx(i), VFLy(i), VFRx(i), VFLy(i), VRLx(i),VRLy(i), VRRx(i), VRRy(i), omegaFL(i), omegaFR(i), omegaRL(i), omegaRR(i));

sFLx(i) = slip(1);
sFLy(i) = slip(2);
sFL(i)  = slip(3);

sFRx(i) = slip(4);
sFRy(i) = slip(5);
sFR(i)  = slip(6);

sRLx(i) = slip(7);
sRLy(i) = slip(8);
sRL(i)  = slip(9);

sRRx(i) = slip(10);
sRRy(i) = slip(11);
sRR(i)  = slip(12);

clear slip
%============================================================
%   Friction
%============================================================

friction = friction_fun(sFLx(i),sFLy(i),sFL(i), sFRx(i), sFRy(i), sFR(i), sRLx(i), sRLy(i), sRL(i), sRRx(i), sRRy(i), sRR(i), z(i), zdot(i), theta(i), thetadot(i), phi(i), phidot(i));

fFLx(i) = friction(1);
fFLy(i) = friction(2);
fFL(i)  = friction(3);

fFRx(i) = friction(4);
fFRy(i) = friction(5);
fFR(i)  = friction(6);

fRLx(i) = friction(7);
fRLy(i) = friction(8);
fRL(i)  = friction(9);

fRRx(i) = friction(10);
fRRy(i) = friction(11);
fRR(i)  = friction(12);

fFLz(i) = friction(13);
fFRz(i) = friction(14);
fRLz(i) = friction(15);
fRRz(i) = friction(16);

clear friction

end
snapshots
plots
%plots_B
%plots_C
