% Setup Script for the Lane Change Example
%
% This script initializes the lane change 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 2019 The MathWorks, Inc.
exampleHelperCleanUp;
%% General Model Parameters
Ts = 0.1;                   % Simulation sample time     (s)
%% Path following Controller Parameters
timeGap            = 1.5;   % Time gap               (s)
defaultSpacing     = 10;    % Default spacing        (m)
maxAc              = 2;     % Maximum acceleration   (m/s^2)
minAc              = -3;    % Minimum acceleration   (m/s^2)
maxSteer           = 0.26;  % Maximum steering       (rad)
minSteer           = -0.26; % Minimum steering       (rad)
predictionHorizon  = 30;    % Prediction horizon

%% Create driving scenario
% The scenario name is a session file created by the Driving
% Scenario Designer App.
scenariosNames = {
    'scenario_01_SlowMoving.mat',...                     % scenarioId = 1
    'scenario_02_SlowMovingWithPassingCar.mat',...       % scenarioId = 2
    'scenario_03_DisabledCar.mat',...                    % scenarioId = 3
    'scenario_04_CutInWithBrake.mat',...                 % scenarioId = 4
    'scenario_05_SingleLaneChange.mat',...               % scenarioId = 5
    'scenario_06_DoubleLaneChange.mat',...               % scenarioId = 6
    'scenario_07_RightLaneChange.mat',...                % scenarioId = 7
    };

% This is the default scenario. 
% Please change scenarioId from command line insted from the script
% Re-run the script to update the scenario in the model.
isScenarioIdExists = exist('scenarioId', 'var');
if(~isScenarioIdExists)
  scenarioId = 1;
end
% Simulation stop time
simStopTime = exampleHelperReadSimStopTime(scenariosNames{scenarioId});

% Lane change parameters
LCCommandTime    = 0;     % Forced lane change trigger time
maxYawRate       = 20;    % max. yawrate during lane change (deg/s)
maxBrake         = 2.5;   % max braking (m/s^2)
responseTime     = 1.2;   % driver response time (sec)
LCDuration       = 3;     % duration for lane change (sec)
FCWTTC           = 4.5;   % TTC for FCW -> lane change activation (sec)
LCATTC           = 3.5;   % TTC for LCA (sec)
minSpace         = 45;    % min space for lane change activation (m)

% Variations for long distance for lane change
LCVariations = (-0.1:0.025:0.1)' + 1;

% LC settle delay time = 10 x 0.1s = 1s
LCSettleDelay = 10;

%% Tracking and Sensor Fusion Parameters                        Units
clusterSize = 4.7;        % Distance for clustering               (m)
assigThresh = 100;        % Tracker assignment threshold          (N/A)
M           = 4;          % Tracker M value for M-out-of-N logic  (N/A)
N           = 5;          % Tracker M value for M-out-of-N logic  (N/A)
numCoasts   = 1;          % Number of track coasting steps        (N/A)
numTracks   = 100;        % Maximum number of tracks              (N/A)
numSensors  = 9;          % 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].
% constant-velocity extended Kalman filter
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)


% Load the Simulink model
modelName = 'LaneChangeTestBench'; 
if ~bdIsLoaded(modelName)
    load_system(modelName)
end

%% Bus Creation
% Create buses for lane sensor and lane sensor boundaries

exampleHelperCreateLaneSensorBuses;
exampleHelperCreateMIOBus;
exampleHelperCreateLCParamsBus;
exampleHelperCreateLCStatusMIOBus;
exampleHelperCreateLCStatusBus;
load BusLaneChangeParams
exampleHelperCreateActorProfileBus;
exampleHelperCreateBusActors;


% Load the bus for scenario reader
scenarioReaderBlock = find_system(modelName,...
    'System','driving.scenario.internal.ScenarioReader');
hScenarioReader = scenarioReaderBlock{1};

% Set the scenario reader file name to the selected scenario
set_param(scenarioReaderBlock{1},'ScenarioFileName',scenariosNames{scenarioId});

clear BusActorsEgo;

portHandles = get_param(hScenarioReader,'PortHandles');
get(portHandles.Outport(1),'SignalHierarchy');
fullName = driving.scenario.internal.ScenarioReader. ...
    getFullScenarioFileName(scenariosNames{scenarioId});
driving.scenario.internal.Utilities. ...
    generateCompiledScenarioData(fullName,hScenarioReader,Ts);
scenarioData = load(fullName);

%% Extract scenario information
% Road center information
globalPlanPoints = scenarioData.data.RoadSpecifications.Centers;
globalPlanPoints(:,3) = [];
% Number of lanes in the scenario
numLanes = scenarioData.data.RoadSpecifications.Lanes.NumLanes;
% Lane width of the scenario
laneWidth = scenarioData.data.RoadSpecifications.Lanes.Width;
% Get Initail pose of the ego vehicle
EgoActor = driving.scenario.internal.ScenarioReader. ...
    readEgoVehiclePoses(scenariosNames{scenarioId});

%%
% ACC set speed (m/s)
egoSetVelocity = hypot(EgoActor(1).Velocity(1),EgoActor(1).Velocity(2));

% Initial condition for the ego car in ISO 8855 coordinates
% Initial speed of the ego car           (m/s)
initialEgoVelocity   = egoSetVelocity;
% Initial x position of ego car          (m)
initialEgoX   = EgoActor(1).Position(1);
% Initial y position of ego car          (m)
initialEgoY   = EgoActor(1).Position(2);
% Initial yaw angle of ego car           (rad)
initialEgoYaw = deg2rad(EgoActor(1).Yaw(1));

% Convert ISO 8855 to SAE J670E coordinates
initialEgoY      = -initialEgoY;
initialEgoYaw    = -initialEgoYaw;

% read actor profiles
actor_Profiles = driving.scenario.internal.ScenarioReader. ...
    readActorProfiles(scenariosNames{scenarioId});
actor_Profiles(EgoActor(1).ActorID) = [];      % remove ego actor profile

%% Design controller for lane following controller with spacing control
exampleHelperCreateLaneFollowingController

%% Initializing trajectory generator parameters
lateralTerminalStates = [EgoActor(1).Position(2),...
                     EgoActor(1).Position(2) + laneWidth(1),...
                     EgoActor(1).Position(2) - laneWidth(1)];

% Set longitudinal terminal state based on the scenarioId
if(scenarioId == 4)
    longitudinalTerminalStates = 50;
elseif(scenarioId == 6)
    longitudinalTerminalStates = 60;
elseif(scenarioId == 5)
    longitudinalTerminalStates = 70;
else
    longitudinalTerminalStates = 80;
end

