% Set up and Run Model Script for the Autonomous Emergency Brake (AEB) Example
%
% This script initializes the AEB example model. It loads necessary control
% constants and sets up the buses required for the referenced model
%
%   This is a helper script for example purposes and may be removed or
%   modified in the future.

%   Copyright 2018 The MathWorks, Inc.

%% General Model parameters
% This sample time is used across the model
% and should not be modified
Ts = 0.1;               % Simulation sample time  (s)

%% Add library of Prebuilt Scenarios to MATLAB path
Path = fullfile(matlabroot,'toolbox','shared','drivingscenario', ...
    'PrebuiltScenarios', 'EuroNCAP', 'AutonomousEmergencyBraking');
addpath(genpath(Path)); % put scenario on path 

%% Create driving scenario
% The scenario name is a scenario file created by the Driving Scenario Designer App. 

scenariosNames = {                                          % scenarioNumber
    'AEB_CCRs_100overlap.mat',...                           % 1
    'AEB_CCRm_100overlap.mat',...                           % 2
    'AEB_CCRb_2_initialGap_12m_stop_Inf.mat',...            % 3
    'AEB_CCRb_6_initialGap_40m_stop_inf.mat',...            % 4   
    'AEB_PedestrianChild_Nearside_50width_overrun.mat'};    % 5
    
scenarioNumber = 1;

if scenarioNumber == 5 
    bep_xRange = [0, 30]; % x, y ranges of bird's-eye plot
    bep_yRange = [-10,10];
    simStopTime = 5;   % simulation stop time (sec)
else
    bep_xRange = [0, 60];
    bep_yRange = [-20,20];
    simStopTime = 9;   % simulation stop time (sec)
end

% The scenario file is converted to a drivingScenario object,
% initial conditions of ego car and target actor profiles
[scenario,egoCar,actor_Profiles] = helperSessionToScenario(scenariosNames{scenarioNumber});

K = 0;              % curvature = 0 : straight path
v_set = egoCar.v0;  % set ego velocity (m/s)

%% Initial conditions for the ego car
v0_ego   = v_set - 1;           % Initial speed of the ego car           (m/s)
x0_ego   = egoCar.x0;           % Initial x position of ego car          (m)
y0_ego   = egoCar.y0;           % Initial y position of ego car          (m)
yaw0_ego = egoCar.yaw0;         % Initial yaw angle of ego car           (rad)

%% FCW parameters
FCW.timeToReact  = 1.2;         % driver reaction time                   (sec)
FCW.driver_decel = 4.0;         % driver braking deceleration            (m/s^2)

%% AEB controller parameters
AEB.PB1_decel = 3.8;            % 1st stage Partial Braking deceleration (m/s^2)
AEB.PB2_decel = 5.3;            % 2nd stage Partial Braking deceleration (m/s^2)
AEB.FB_decel  = 9.8;            % Full Braking deceleration              (m/s^2)
AEB.headwayOffset = 3.7;        % headway offset                         (m)
AEB.timeMargin = 0;             % headway time margin                    (sec)

%% Tracking and Sensor Fusion Parameters                        Units
clusterSize = 4;        % Distance for clustering               (m)
assigThresh = 50;       % Tracker assignment threshold          (N/A)
M           = 2;        % Tracker M value for M-out-of-N logic  (N/A)
N           = 3;        % Tracker M value for M-out-of-N logic  (N/A)
numCoasts   = 3;        % Number of track coasting steps        (N/A)
numTracks   = 20;       % Maximum number of tracks              (N/A)
numSensors  = 2;        % Maximum number of sensors             (N/A)

% Position and velocity selectors from track state
% The filter initialization function used in this example is initcvekf that 
% defines a state that is: [x;vx;y;vy;z;vz]. 
posSelector = [1,0,0,0,0,0; 0,0,1,0,0,0]; % Position selector   (N/A)
velSelector = [0,1,0,0,0,0; 0,0,0,1,0,0]; % Velocity selector   (N/A)

%% Ego Car Parameters
% Dynamics modeling parameters
m       = 1575;     % Total mass of vehicle                          (kg)
Iz      = 2875;     % Yaw moment of inertia of vehicle               (m*N*s^2)
lf      = 1.2;      % Longitudinal distance from c.g. to front tires (m)
lr      = 1.6;      % Longitudinal distance from c.g. to rear tires  (m)
Cf      = 19000;    % Cornering stiffness of front tires             (N/rad)
Cr      = 33000;    % Cornering stiffness of rear tires              (N/rad)
tau1     = 0.5;     % Longitudinal time constant (throttle)          (N/A)
tau2     = 0.07;    % Longitudinal time constant (brake)             (N/A)

%% Speed Controller Parameters
speedController.Kp = 1.1;           % Proportional Gain of speed controller
speedController.Ki = 0.1;           % Integral Gain of speed controller
speedController.Amax = 3;           % Maximum acceleration                  (m/s^2)
speedController.Amin = -3;          % Minimum acceleration                  (m/s^2)
speedController.Kd = 0;
speedController.FilterCoeff = 0;

%% Driver steering control paramaters
driver.Kp         = 0.2;  % Proportional gain                     (N/A)
driver.Ki         = 0.1;  % Integral gain                         (N/A)
driver.yawErrGain = 2;    % Yaw error gain                        (N/A)

% clear BusActors bus if it exists
if exist('BusActors','var')
    clear BusActors
    clear BusActorsActors
end

%% Bus Creation
% Create the bus of actors from the scenario reader
modelName = 'AEBTestBenchExample';
wasModelLoaded = bdIsLoaded(modelName);
if ~wasModelLoaded
    load_system(modelName)
end

% Set the right scenario file name on the block
blk=find_system(modelName,'System','driving.scenario.internal.ScenarioReader');
preserve_dirty = Simulink.PreserveDirtyFlag(modelName,'blockDiagram');
set_param(blk{1},'ScenarioFileName',scenariosNames{scenarioNumber});

s = get_param(blk{1},'PortHandles');
get(s.Outport(1),'SignalHierarchy');

% Create the bus of tracks (output from referenced model)
refModel = 'AEBWithSensorFusionMdlRef';
wasReModelLoaded = bdIsLoaded(refModel);
if ~wasReModelLoaded
    load_system(refModel)
    blk=find_system(refModel,'System','multiObjectTracker');
    multiObjectTracker.createBus(blk{1});
    close_system(refModel)
else
    blk=find_system(refModel,'System','multiObjectTracker');
    multiObjectTracker.createBus(blk{1});
end

if ~wasModelLoaded
    close_system(modelName)
end

disp(['>>> Scenario loaded: ', scenariosNames{scenarioNumber}]);

%% Code generation
% Uncomment this if you would like to generate code.
% rtwbuild(refModel);