%% Solver parameters
% tstep = 0.001;                               %Time step for the simulation [s]
% solver = 'ode3';                              %Solver used in the simulink model
% ZOOX_config_base = base_config(tstep,solver);   %Load simulink configuration file

%% Vehicle data
g = 9.81;                                     % Gravity [m/s^2]
Atm_Pres = 101325;                            % Atmospheric pressure [Pa]
M_vehicle = 2650;                             % Vehicle mass [kg]
M_bias = 50;                                  % Load dsitribution [%]
mt = 55;                                      % Unsprung mass [kg]
L = 2.78;                                  	% Wheel base [m]
t = 1.59;                                     % Track width [m]
Steer_Ratio = 2.48/1000;                      % Check value! On center steering ratio, rack displacement vs wheel steer angle [m/deg]
Motion_Ratio = 1;                             % Chceck value! Suspension motion ratio (strut displacement/wheel displacement)
Zero_tol = 0.001;                             % Value for when dividing by zero
%% Active Steering
%General paramterers
                      
ASteer_ON = 1;                                % Active steering active
ASteer_variant = 1;                           % Active steering variant [1 = trajectory tracker] [2 = 4ws ffs + CM driver]
ASteer_CL_ON = 0;                             % Closed Loop:1//Open Loop:0
ASteer_Fback_AngDev = -0.1;                   % Proportional gain for the heading error control [m/rad]
ASteer_Fback_PDev_P = -0.1;                   % Proportional gain for the path deviation control [m/m]
ASteer_Fback_PDev_I = -0.02;                  % Integral gain for the path deviation control [1/s]
ASteer_Fback_PDev_D = -0.02;                  % Derivative gain for the path deviation control [s]
%Curvature demand calculation
ASteer_K_Curv_Dem_F = 1;                      % Front rack curvature demand gain
ASteer_K_Curv_Dem_R = 1;                      % Rear rack curvature demand gain
%Feedforward calculation 
ASteer_K_Slip_Dem_F = 0.5;                    % Front rack slip demand gain
ASteer_Slip_Dem_F_rate = 0.1;                 % Rate limiter for the front rack slip demand [m/s]
ASteer_K_Slip_Dem_R = 0.4;                    % Rear rack slip demand gain 
ASteer_Slip_Dem_R_rate = 0.011;               % Rate limiter for the rear rack slip demand [m/s]
ASteer_TP_Vx = [0, 30, 60, 120, 150]/3.6;         % Variable turning point mapping speed [m/s]
ASteer_TP_Bias = [0, 0.5, 1, 1, 1];               % Variable turning point mapping bias value
%Inverse Tire Model 
ASteer_Tire_Full = 0;                         % Slip angle estimation using a full inverse tire model
mfD = 0.67*0.5*g*M_vehicle;                   % Magic Formula parametrs - max force
mfC = 2;                                     % Magic Formula parametrs - shape factor
mfB = 9.1;                                    % Magic Formula parametrs - slope
ASteer_Tire_Linear = 0;                       % Slip angle estimation using a linear tire model
C_Tire_Linear = 150000;                       % Axle cornering stiffness [N/rad]
ASteer_Tire_Saturation = 1;                   % Slip angle estimation using an inverse tire model with slip saturation
Delta_max = 5*pi/180;                         % Saturation slip angle value
K_Tire_Sat = 1/11500;                         % Proportional parameter for the inverse tire model with saturation [1/N]


%COntroller Parameters
% TorqueLoop = 0.002;                         %Torque and Position sample time
%PI COntrol of Rack Travel parameters

%Stability Compensator parameters
LagPoleHz = 1.5;
LagZeroHz = 7;
LeadPoleHz = 110;
LeadZeroHz = 7;
LagPole = 2*pi*LagPoleHz;
LagZero = 2*pi*LagZeroHz;
LeadPole = 2*pi*LeadPoleHz;
LeadZero = 2*pi*LeadZeroHz;
%Motor(Mech) and Rack Parameterssubplot
GR = 111/33*48/7;                           %Gear ratio from motor to column
PulleyFactor = 111/33;                      %Number of tooth on motor pulley=33 and rack side=111 
ScrewLeadFactor = 2*pi/0.007;               %Ball Screw is 0.007 m/rev
Jmot = 95.425e-6;                           %Motor inertia Kg-m^2
Bmot = 0.0005;                              %Motor damping (viscous) Nm-s/rad
Kmot_rack = 2*180/pi;                       %Belt Stiffness between motor and rack (belt stiffness) in Nm/rad
Bmot_rack = 0.052;                          %Motor to rack damping (viscous) Nm-s/rad
Mrack = 489.8262;                           %Mass of the ballnut (trasfered to rack side) and rack Kg
Brack =  31699.25;                          %Rack damping (viscous) N-s/m _ External load
Kr = 5.1404e+05;                            %Tire load N/m _ External load
Friction = 250*0.048/(2*pi);                %Rack friction Nm (reflected to Column Coordinate
PowerDisableMode = 0;                       %Power of the system - Zero torque output
%% Strut model
%Bumpstop data - strut overall bumpstop
Strut_bs_ON = 1;                              %Bumpstop active
Strut_F_bs = [0, 0, 0, 628*10, 628*13];           %Force points [N]
Strut_s_bs = [0, 0.01, 0.067, 0.077, 0.08];           %Displacement [m]
%Airsping data
ASpring_k = 1.4;                             %Airspring polytropic index
ASpring_Bump = 0.08;                          %Airspring bump stroke [m]
ASpring_Reb = -0.08;                          %Airspring rebound stroke [m]
ASpring_Reb_F_ON = 0;                         %Front rebound airspring active -closed volume
ASpring_Reb_R_ON = 0;                         %Rear rebound airspring active -closed volume
ASpring_d_Bore = 3.5*0.0254;                         %Airspring bore [m]
ASpring_d_Shaft = 0.022225;                   %Airspring shaft diameter [m]
ASpring_d_Aux = 0.04;                         %Auxiliary volume piston diamter [m]
ASpring_A_Bump = pi*(ASpring_d_Bore^2)/4;     %Airspring efective area in bump [m^2]
ASpring_A_Reb = pi*((ASpring_d_Bore^2/4) - (ASpring_d_Shaft^2/4));  %Airspring efective are in rebound [m^2]
ASpring_A_Aux = pi*(ASpring_d_Aux^2)/4;       %Auxiliary volume piston area [m2]
ASpring_V0_Bump_F = 0.0017 + 0.0*ASpring_A_Bump;                  %Front bump airspring volume @ design position [m^3]
ASpring_V0_Bump_R = 0.0017 + 0.0*ASpring_A_Bump;                  %Rear bump airspring volume @ design position [m^3]
ASpring_V0_Reb_F=abs(ASpring_Reb*ASpring_A_Reb)+0.00007;    %Front rebound airspring volume @ design height [m^3]
ASpring_V0_Reb_R=abs(ASpring_Reb*ASpring_A_Reb)+0.00007;    %Rear rebound airspring volume @ design height [m^3]

ASpring_BS_ON = 1;                            %toggle air spring bump stop
ASpring_s_bs = ASpring_Reb*ones(1,5);
ASpring_s_bs(1,length(ASpring_s_bs) + 1) = 0;
ASpring_s_bs(1,length(ASpring_s_bs) + 1:length(ASpring_s_bs) + 5) = ASpring_Bump*ones(1,5);
ASpring_s_bs_offset = [-0.003, -0.002, -0.001, -0.0005, 0, 0, 0, 0.0005, 0.001, 0.002, 0.003];
ASpring_s_bs = ASpring_s_bs + ASpring_s_bs_offset;
ASpring_F_bs = 0.8*[-5942.4771, -2368.1816, -685.112, -216.9914, 0, 0, 0, 216.9914, 685.112, 2368.1816, 5942.4771];   % Force points [N]
ASpring_s_bs2 = (0:38)/1000;
ASpring_s_bs2 = [ASpring_Reb, ASpring_s_bs2 + (ASpring_Bump - ASpring_s_bs2(end))];
ASpring_F_bs2 = [
    0, 0, 75.35661461, 150.7132292, 226.0698438, ...
    301.4264584, 376.783073, 452.1396877, 527.4963023, 602.8529169, ...
    678.2095315, 753.5661461, 828.9227607, 904.2793753, 979.6359899, ...
    1054.992605, 1130.349219, 1205.705834, 1281.062448, 1356.419063, ...
    1431.775678, 1507.132292, 1582.488907, 1657.845521, 1733.202136, ...
    1808.558751, 1883.915365, 2092.865911, 2984.629883, 4618.582361, ...
    6994.723346, 10113.05284, 13973.57083, 18576.27734, 23921.17235, ...
    30008.25587, 36837.52789, 44408.98842, 52722.63746, 61778.475
    ];
% Passive suspension settings
Passive_RP_RRatio_front = 0.35;
Passive_RP_RRatio_rear = 0.5;

% Coilspring data
CSpring_ON = 0;                               %Coilspring active
CSpring_K = 30000;                            %Coilspring stiffness [N/m]
CSpring_F0 = 0.25*M_vehicle*g/Motion_Ratio;   %Coilspring preload [N]
% Airspring from file TLII
ASpring_data_ON = 1;                          %Airspring characteristics from data
ASpring_data = dlmread('ZOOX_spring_VH5_TLII.txt','\t');  %Airspring data from a tab separated file - first column displacement [mm]/second column force [N]
ASpring_ds = ASpring_data(:,1);               %Airspring compression [m]
ASpring_Fs = ASpring_data(:,2);               %Airspring force [N]
%Damper data
Damper_Lin_ON = 0;                            %Set to 1 to consider linear damper
Damper_C = 2600;                              %Linear damper coefficient [Ns/m](FOR SIMPLE MODEL)
Damper_data = dlmread('ZOOX_damper_VH5_TLII.txt','\t');    %Damper data from a tab separated file - first column velocity [m/s]/second column force [N]
Damper_vd = Damper_data(:,1);                 %Damper compression velocity [m/s]
Damper_Fd = Damper_data(:,2);                 %Damper force [N]
%% ICLS
%General parameters
ICLS_ON = 1;                                  %ICLS system active
ICLS_variant = 3;                             %type of actuator
ICLS_actuation_toggle = 2;                    %1 = only allows extention if hydraulic pressure is present, 2 = Full actuation, 3 =  only retraction (no hydraulic pressure)
SKYHOOK_ON = 0;
SKYHOOK_ROLL_gain = 0.2;
SKYHOOK_PITCH_gain = 0.6;
SKYHOOK_HEAVE_gain = 0.2*0;
SKYHOOK_WASH_gain = 20;
FA_LOOKAHEAD = 0;
actuation_latency = 0.2;
FA_smoothing = 0.2;
Prescan_states=1;                           %toggle roll,pitch and heave state feedback
Prescan_estimation_toggle=2;                % toggle between estimated vehicle states or CM states
Est_steer_gain=1;
ICLS_Up_Travel = 0.04;                        %ICLS maximum up travel - shorter strut [m]
ICLS_Down_Travel = -0.04;                     %ICLS maximum down travel - longer strut [m]
ICLS_Up_Down = abs(ICLS_Up_Travel/ICLS_Down_Travel);  %ICLS up to down ratio

%bicycle model parameters
bc_m = 2780;                                   % mass [kg]
bc_Jz = 2620;                                    % inertia [kg/m^3]
bc_a = 1.39;                                     % cg to front axle [m]
bc_b = bc_a;                                        % cg to rear axle
bc_c_f = 110000;                               % front axle stiffness [N/rad]
bc_mu_f = 0.9;                                 % front axle friction[unitless]
bc_c_r = 125000;                               % [N/rad]
bc_mu_r = 1.0;                                 % [unitless]

cp_treshold_speed_model = 1.5;                   %Treshold speed when to use the bicycle model.
cp_ICLS_model_K_Ay = 0.0075;                     %Gain for the lateral acceleration
cp_ICLS_model_RB_Ay = 0.7;                       %Roll bias to the front for the lateral acceleration (e.g. 0.6 -> 60% Roll compensation at the front/40% Roll compensation at the rear
cp_ICLS_model_K_Cp = 0.0075;                     %Centripetal gain
cp_ICLS_model_RB_Cp = 0.7;                       %Centripetal Roll balance
cp_norm_ay = 8;                                  % value of ay where expo function output=input
cp_expo_ay = 1;                                  % value of expo
cp_norm_cp = 8;                                  % value of centripetal_ay where expo function output=input
cp_expo_cp = 1;                                  % value of expo
%Steer rate control gains
cp_ICLS_K_SRT = -0.0012;                         %Gain for total steering rate
cp_ICLS_RB_SRT = 0.7;
cp_ICLS_K_SRF = 0;                               %separate for front            
cp_ICLS_RB_SRF = 1;
cp_ICLS_K_SRR = 0;                               %separate for rear
cp_ICLS_RB_SRR = 0;
% Roll rate control gains
cp_ICLS_K_RollR = 0; 
cp_ICLS_RB_RollR = 1;                            %Roll bias to the front for the roll rate (e.g. 0.6 -> 60% Roll compensation at the front/40% Roll compensation at the rear
cp_alpha_roll = 1;                               %Roll rate filt factor 1=no filterering
cp_norm_roll_rate = 0.6;                         %value of roll rate where expo function output=input
cp_expo_roll_rate = 1.9;                         %value of expo
% Pitch control
cp_ICLS_Long_K_Ax = 0.005;                       %Gain for the longitudinal acceleration
cp_alpha_ax = 0.02;                              %filt factor fo ax 1=no filterering
%alpha_ax=0.005;                              %filt factor 1=no filterering older
cp_norm_ax = 8;                                  %value of roll rate where expo function output=input
cp_expo_ax = 1;                                  %value of expo 1+no expo
% ICLS travel limits.
cp_ICLS_up_travel = -ICLS_Down_Travel*1000;                          %Up travel of ICLS [mm]
cp_ICLS_down_travel = -ICLS_Up_Travel*1000;                       %Down travel of ICLS [mm]
cp_ICLS_nom = 0;  
cp_ax_zerospeed_smoothing = 1;

% Hydraulic supply model
% ICLS area
ICLS_A_extend = pi*0.02^2;
ICLS_A_retract = (ICLS_A_extend - pi*(0.0225/2)^2)*0;

% pump controls
Pump_on = 1;       %toggle the hydraulic pumps on/off
Pump_on_P = 120;   %Pump on pressure
Pump_off_P = 170; %Pump off pressure

% accumulator data
acc_p0 = 60;      %accumulator gas preload pressure [bar]
Vdead = 0.2e-4;   %Accumulator vead volume   [m^3]
VC = 1.5*9.3e-4;      %Fluid chamber capacity  [m^3]

% pump data
pH206 = [.67, .46, 207-34, 14, 36, 207-34, 34];  % from parker data sheet
pH206_mod = [.65, .40, 207-34, 13.5, 37, 207-34, 34];  % vh6A pump data
pumpdata = pH206_mod;
Hyd_leak_rate = 1/60*0.0002/170*0; % m^3/s

F_acc_V0 = VC/2;     % Front accumulator starting volume
R_acc_V0 = VC/2;
F_pump_start_state = 1;     % If the pump is running at the start of the simulation
R_pump_start_state = 1;

%% ASCS - pressure control
psi2pa = 6897.76;
ASCS_Pres_Bump_F = psi2pa*(140+0);                %ASCS front strut initial bump airspring pressure [Pa]
ASCS_Pres_Bump_R = psi2pa*(140+0);                %ASCS rear strut initial bump airspring pressure [Pa]
ASCS_Pres_Reb_F = 0;                     %ASCS front strut initial rebound airspring pressure [Pa]
ASCS_Pres_Reb_R = 0;                          %ASCS rear strut initial rebound airspring pressure [Pa]
ASCS_Pres_Bump_adjust_F = -125000*0;            %static change of bump air spring pressure.
ASCS_Pres_Bump_adjust_R = -125000*0;
%% ASAC - volume control

%% ABS Parameters
SoftABS_params
SoftABS_ON = 1;

%% ESC Parameters
vh6_esc_params

%%Long Control
Drive_Mode = 0;                     %0 = Start 2WD, if needed go to 4WD | 2 = 4WD | 1 = FWD
zx_IPGDriver_AX_Pos_Limit = 2;                   % acceleration limit for zx_velocity_control
zx_IPGDriver_AX_Neg_Limit = -4;                  % acceleration limit for zx_velocity_control
zx_IPGDriver_AXdot_Limit_Magnitude = 10;                % jerk limit for zx_velocity_control
brake_pedal_gain = 0;            % gain for still using brake pedal input in zx_brake (set to 0 for velocity controller)brake_pedal_gain
brake_model_select = 0;          % 0 = use the simple slew + delay, 1 = use the same model and parameteization as the brake model built into the suspension controller
                        % brake model dev https://docs.google.com/presentation/d/1MbBqI7BxqHcI7ScaNHaBD4YX2N1Hx8mkZDC_qvqA57M/edit?usp=sharing

mdi_params
rack_params
motor_params
generic_plugin_params
zxe7_params
edu_interface_params

