velocity = 1;
if velocity < 21.5
    gear = 1;
elseif velocity < 30
    gear = 2;
elseif velocity < 37
    gear = 3;
elseif velocity < 47
    gear = 4;
else
    gear = 5;
end

MaxTorqueVel = 652.335; %(radians per second)
TorqueSpreadHalf = 717.568;
MaxTorque = 307.040;
FirstRatio = car.Gear.FirstRatio;
SecondRatio = car.Gear.SecondRatio;
ThirdRatio = car.Gear.ThirdRatio;
FourthRatio = car.Gear.FourthRatio;
FifthRatio = car.Gear.FifthRatio;
SixthRatio = car.Gear.SixthRatio;
FinalDriveRatio = car.Gear.FinalDriveRatio;
Efficiency = car.Gear.Efficiency;
Iw = car.Iw;
p = car.p;
k = 0.01;
Pacejka = [car.Tire.Cdx, car.Tire.Ccx, car.Tire.Cbx, car.Tire.Cex];
F_zf = car.Fzf;
Cf = prod(Pacejka) * F_zf;

switch gear
    case 1
        gearGain = FirstRatio;
    case 2
        gearGain = SecondRatio;
    case 3
        gearGain = ThirdRatio;
    case 4
        gearGain = FourthRatio;
    case 5
        gearGain = FifthRatio;
    case 6
        gearGain = SixthRatio;
    otherwise
        gearGain = 0;
end

A = -(p*Cf)/(Iw*velocity);
B = ((gearGain * FinalDriveRatio * Efficiency)/Iw) * (MaxTorque/(1 + ((gearGain * FinalDriveRatio * velocity/p - MaxTorqueVel)/TorqueSpreadHalf)^2));
d = -Cf/Iw - k*F_zf;
