clc

%% Driver %%
Fd = 120;
Fd_max = 150;
Vi = 120;
Vi = Vi / 3.6; % Convert from km/h to m/s

%% Road %%
grad = 5; % Road gradient
mu_max = 0.8;
mu_s = 0.65;
va = 0;
va = va / 3.6; % Convert from km/h to m/s

%% Pedal %%
i = 4; % Pedal ratio

%% Servo %%
Pa = 1; % Atmospheric pressure
Pm = 0.45; % Master cylinder pressure
Do = 238; % Servo diameter
di = 75; % Servo piston diameter
factor = Fd / Fd_max;

%% Master %%
Dm = 18; % Master cylinder diameter

%% Front disk %%
Db_f = 55; % Front disk piston diameter
Db_f_o = 200; % Outer diameter of front disk
Db_f_i = 160; % Inner diameter of front disk
mu_f = 0.3; % Friction coefficient for front disk
N_f = 2; % Number of front disks

%% Rear disk %%
Db_r = 50; % Rear disk piston diameter
Db_r_o = 220; % Outer diameter of rear disk
Db_r_i = 160; % Inner diameter of rear disk
mu_r = 0.2813; % Friction coefficient for rear disk

%% Tire %%
W_N = 195; % Tire width
As_Ratio = 65; % Tire aspect ratio
rim_diameter = 15; % Rim diameter in inches

%% Vehicle characteristics %%
W = 2000; % Vehicle weight (Note: In the first code, W is in kN)
CG = 45; % Center of gravity height
CG_H = 600; % CG height
CG_L = 50; % CG lateral position
Jw = 0.8;
track = 1425; % Track width
WheelB = 2540; % Wheelbase
nf = 2; % Number of front wheels
nr = 2; % Number of rear wheels
Afr = 2; % Frontal area

%% Resistance %%
Fr = 0.007; % Rolling resistance coefficient
Cd = 0.4; % Drag coefficient

%% Calculations %%
A_servo = (pi/4) * ((Do)^2 - (di)^2);
A_master = (pi/4) * (Dm)^2;
Af = (pi/4) * (Db_f)^2;
Ar = (pi/4) * (Db_r)^2;
Rmean_f = (1e-3) * (2/3) * ((Db_f_o/2)^3 - (Db_f_i/2)^3)/((Db_f_o/2)^2 - (Db_f_i/2)^2);
Rmean_r = (1e-3) * (2/3) * ((Db_r_o/2)^3 - (Db_r_i/2)^3)/((Db_r_o/2)^2 - (Db_r_i/2)^2);
Rtire = ((W_N * As_Ratio / 100) + (rim_diameter * 25.4 / 2)) * 1e-3;

%% Force and Pressure %%
Fp = Fd * i;
F_servo = factor*(Pa - Pm) * A_servo / 10;
Pl = (Fp + F_servo) / A_master;
RR = Fr * W * 9.81 * cos(atan(grad/100)); % Rolling resistance
GR = sin(atan(grad/100)) * W * 9.81; % Gradient resistance

%% Weight distribution %%
nF_L = (2/nf) * (1 - CG_L) * W * 9.81 * (1 - CG); % Normal force on front left tire
nF_R = (2/nf) * (CG_L) * W * 9.81 * (1 - CG);   % Normal force on front right tire
nR_L = (2/nr) * (1 - CG_L) * W * 9.81 * (CG); % Normal force on rear left
nR_R = (2/nr) * (CG_L) * W * 9.81 * (CG);   % Normal force on rear right

%% Initial conditions %%
V = Vi; % Initial vehicle velocity
h = 0.0001; % Time step for simulation
N = 0; % Counter for iterations
X = 0; % Initial distance covered
T = 20; % Total simulation time
t = 0; % Initial time
omega = Vi / Rtire; % Initial angular velocity of tires
OMEGA = [omega omega omega omega]; % Angular velocity for each wheel
I_a = [0 0 0 0]; % Adjusted for 4 wheels
Pl_0 = 0;
Pl_pre = 0;
F_fl = 0; F_fr = 0; R_rl = 0; R_rr = 0; % Initial braking forces
a1 = -110;
a2 = -125;
a3 = 20;
a4 = 200;
Alpha = 1.5;
ACC_w = Alpha * Rtire;
control = 1;

%% Simulation loop %%
while t < T
    N = N + 1;
    [Pl_ramp] = Ramp(Pl, Pl_0, t, Pl_pre, control);
    Pl_pre = Pl_ramp;
    [T_f, T_r] = Braking_torque(Pl, Af, Ar, mu_f, mu_r, N_f, Rmean_f, Rmean_r);
    Pl_pre_FL = Pl_ramp;  Pl_pre_FR = Pl_ramp;
    Pl_pre_RL = Pl_ramp; Pl_pre_RR = Pl_ramp;
    Pline(N, 1) = Pl_ramp;
    vr = V - va; % Relative velocity for air resistance calculation
    Ar = 0.5 * 1.225 * Cd * Afr * vr^2; % Calculate air resistance

    % Calculate total resistance
    R_t = ((sign(V)) * (Ar + RR)) + GR;

    % Calculate dynamics for front left wheel
    ddt_FL = -(T_f - (F_fl * Rtire)) / Jw;
    dt_FL = (ddt_FL * h) + OMEGA(N, 1);
    vfl = dt_FL * Rtire;
    ifl = (vfl / V) - 1;
    ifl = max(min(ifl, 0), -1);
    if ifl == -1, dt_FL = 0; end

    aw_FL = ddt_FL * Rtire; % Calculate angular acceleration

    if N == 1
        aw_pre_FL = 0;
    else
        aw_pre_FL = aw_FL; % Update aw_pre_FL with the current value
    end

    control_FL = ABScycle(a1, a2, a3, a4, aw_FL, aw_pre_FL);
    [Pl_ramp_FL] = Ramp(Pl, Pl_0, t, Pl_pre_FL, control_FL);
    Pl_pre_FL = Pl_ramp_FL;
    [T_f, T_dum] = Braking_torque(Pl_ramp_FL, Af, Ar, mu_f, mu_r, N_f, Rmean_f, Rmean_r);

    % Same structure for the other wheels (front right, rear left, rear right)

    % Calculate braking force
    FB = F_fl + F_fr + R_rl + R_rr;

    % Vehicle dynamics
    ddx = -(R_t + ((sign(V)) * FB)) / W;
    dx = (ddx * h) + V;
    x = dx * h;
    X = X + x;
    V = dx;

    % Update time, acceleration, velocity, and distance
    time(N, 1) = t;
    acc(N, 1) = ddx;
    vel(N, 1) = V;
    dist(N, 1) = X;

    % Calculate tire forces
    F_rr = tiremodel(irr, nR_R, mu_max);
    F_rl = tiremodel(irl, nR_L, mu_max);
    F_fr = tiremodel(ifr, nF_R, mu_max);
    F_fl = tiremodel(ifl, nF_L, mu_max);

    % Break the loop if the vehicle almost stops
    if V <= 0.1
        break;
    end

    % Increment time
    t = t + h;

    % Update angular velocities for next iteration
    OMEGA(N + 1, :) = [dt_FL dt_FR dt_RL dt_RR];
    % Update slip ratio for each wheel
    I_a(N, :) = [ifl ifr irl irr];
end


% Trim vectors to the length of completed iterations
finalLength = N;
time = time(1:finalLength);
acc = acc(1:finalLength);
vel = vel(1:finalLength);
dist = dist(1:finalLength);
I_a = I_a(1:finalLength, :);
OMEGA = OMEGA(1:finalLength, :);

% Plotting the results
figure; plot(time, acc); xlabel('Time (s)'); ylabel('Acceleration (m/s^2)'); title('Acceleration vs Time');
figure; plot(time, vel); xlabel('Time (s)'); ylabel('Velocity (m/s)'); title('Velocity vs Time');
figure; plot(time, dist); xlabel('Time (s)'); ylabel('Distance (m)'); title('Distance vs Time');
figure; plot(time, I_a); xlabel('Time (s)'); ylabel('Slip Ratio (I)'); title('Slip Ratio vs Time');
figure; plot(time, OMEGA); xlabel('Time (s)'); ylabel('Wheel Angular Velocity (OMEGA)'); title('Angular Velocity vs Time');

% Final values
t_end = time(end);
vel_end = vel(end);
acc_end = acc(end);
stopping_distance = dist(end);


this code gives me irr,irl,ifr,ifl variable unknown