%%
clear all;
close all;

%% Initial State Values
initV = 14.4546;
initBeta = 0;
initR = 0;

%% Car Parameters to set-up
forceForward = 0.59;
forceRare = 0.41;
car_length = 2.28;
%car_length = 1.8;
car.m = 840;
car.lf = forceRare*car_length; % there was a mistake
car.lr = forceForward*car_length;
car.I = 2600;%1283; % taken from the LFS, but was almost ok: car.m*car.lf*car.lr;
car.p = 0.2765; % could be changed Rim radius is 0.2
car.Iw = .5; %from LFS (not used there)
car.BrakeBalance = 0.85;
car.BrakeStrength  = 780;
car.MaximumDriveTorque = 260;
car.Gear.ReverseRatio = -6;
car.Gear.FirstRatio = 3.3;
car.Gear.SecondRatio = 2.4;
car.Gear.ThirdRatio = 1.9;
car.Gear.FourthRatio = 1.5;
car.Gear.FifthRatio = 1.22;
car.Gear.SixthRatio = 1;
car.Gear.FinalDriveRatio = 3.2;
car.Gear.Efficiency = 0.8500000238418579;
%% Resistances
car.UseRollingResistance = 0; %dont used there
car.k = 0.015;
car.UseAerodynamicsResistance = 1;
car.Air.Ca = 0.4; %changed to LFS set-up
car.Air.rho = 1.2;
car.Air.A = 3;
%% Car Par-ms Subcalculation
car.Fzf = 9.81 * car.m * car.lr /(car.lr + car.lf);
car.Fzr = 9.81 * car.m * car.lf /(car.lr + car.lf);
%% Tire Parameters
% car.Tire.Cdy = 1;
% car.Tire.Cby = 6.9;
% car.Tire.Ccy = 1.8;
% car.Tire.Cey = 0.1;
Pacejka_y=load('Offline_Telemetry/Pacejka_y.mat');
Pacejka_x=load('Offline_Telemetry/Pacejka_x.mat');
Pacejka_y=Pacejka_y.Pacejka;
Pacejka_x=Pacejka_x.Pacejka;

car.Tire.Cdy =  Pacejka_y(1);
car.Tire.Cby = Pacejka_y(3);
car.Tire.Ccy = Pacejka_y(2);
car.Tire.Cey = Pacejka_y(4);
% car.Tire.Cdy =   2.3344;
% car.Tire.Cby = 6.0976;
% car.Tire.Ccy = 2.4457 ;
% car.Tire.Cey = 1.1707;
car.Tire.Cdx = Pacejka_x(1);
car.Tire.Cbx = Pacejka_x(3);
car.Tire.Ccx = Pacejka_x(2);
car.Tire.Cex = Pacejka_x(4);

% car.Tire.Cdx = 2.2535;
% car.Tire.Cbx = 30;
% car.Tire.Ccx = 1.3921;
% car.Tire.Cex = -3;

% car.Tire.Cdx = 1;
% car.Tire.Cbx = 15;
% car.Tire.Ccx = 1.7;
% car.Tire.Cex = -0.5;

%% Input Boundaries
df_max = 0.42; % from LFS
dr_max = 0;
omegaR_max = 300;
omegaF_max = 300;

%% Select Pacejka Config
%LoadPacejkaConfigurations; %change parameters in this file
%car.Pacejka = Classic; % (Aggressive, Classic, AlmostSaturation)
%clear Aggressive Classic AlmostSaturation
%% SlipRatio envelope parameters
Pacejka = [car.Tire.Cdx, car.Tire.Ccx, car.Tire.Cbx, car.Tire.Cex];
car.SlipRatio_max = 0.0511;
car.SlipRatio_min = -0.0511;
%% SlipAngle envelope parameters
Pacejka = [car.Tire.Cdy, car.Tire.Ccy, car.Tire.Cby, car.Tire.Cey];
car.SlipAngle_max = 0.1787;
car.SlipAngle_min = -0.1787;
%% Lateral Control Parameters
v = [5 10 15 20];
LateralPID = cell(1, length(v));
car.LateralPID.Kp = zeros(1, length(v));
car.LateralPID.Ki = zeros(1, length(v));
car.LateralPID.Kd = zeros(1, length(v));
counter = 1;
for i = v
    LateralPID{counter} = load(['Offline_Telemetry/LateralPID/regulator_' num2str(i) '.mat'], ['regulator_' num2str(i)]);
    eval(['LateralPID{counter} = LateralPID{counter}.regulator_' num2str(i) ';']);
    car.LateralPID.Kp(counter) = LateralPID{counter}.Kp;
    car.LateralPID.Ki(counter) = LateralPID{counter}.Ki;
    car.LateralPID.Kd(counter) = LateralPID{counter}.Kd;
    counter = counter + 1;
end
%% Bus creation for Simulink
busInfo = Simulink.Bus.createObject(car);
tmp = Simulink.Bus.objectToCell({busInfo.busName});
tmp2 = tmp{1};
tmp2{1} = 'car_bus';
tmp{1} = tmp2;
Simulink.Bus.cellToObject(tmp);


%% Remove junk variables
% clearvars -except car car_bus LateralPID