function helperSLHighwayLaneChangeSetup(varargin)
% helperSLHighwayLaneChangeSetup creates base workspace variables required
% for Highway Lane Change Example
%
% helperSLHighwayLaneChangeSetup(scenarioFcnName)
% 
% Optional inputs
%   scenarioFcnName:
%     - Name of function which returns scenario which is
%       compatible with HighwayLaneChangeTestBench.slx
%     - Valid values are:
%         "scenario_LC_01_SlowMoving"
%         "scenario_LC_02_SlowMovingWithPassingCar"
%         "scenario_LC_03_DisabledCar"
%         "scenario_LC_04_CutInWithBrake"
%         "scenario_LC_05_SingleLaneChange"
%         "scenario_LC_06_DoubleLaneChange" [Default]
%         "scenario_LC_07_RightLaneChange"
%         "scenario_LC_08_SlowmovingCar_Curved"
%         "scenario_LC_09_CutInWithBrake_Curved"
%         "scenario_LC_10_SingleLaneChange_Curved"
% Examples of calling this function:
%    helperSLHighwayLaneChangeSetup("scenario_LC_01_SlowMoving")
%    helperSLHighwayLaneChangeSetup("scenario_LC_02_SlowMovingWithPassingCar")
%    helperSLHighwayLaneChangeSetup("scenario_LC_03_DisabledCar")
%    helperSLHighwayLaneChangeSetup("scenario_LC_04_CutInWithBrake")
%    helperSLHighwayLaneChangeSetup("scenario_LC_05_SingleLaneChange")
%    helperSLHighwayLaneChangeSetup("scenario_LC_06_DoubleLaneChange")
%    helperSLHighwayLaneChangeSetup("scenario_LC_07_RightLaneChange")
%    helperSLHighwayLaneChangeSetup("scenario_LC_08_SlowmovingCar_Curved")
%    helperSLHighwayLaneChangeSetup("scenario_LC_09_CutInWithBrake_Curved")
%    helperSLHighwayLaneChangeSetup("scenario_LC_10_SingleLaneChange_Curved")

% This is a helper script for example purposes and may be removed or
% modified in the future.

% Copyright 2019-2020 The MathWorks, Inc.

%% Load the model to have the expected behavior
modelName = "HighwayLaneChangeTestBench";
if ~bdIsLoaded(modelName)
    load_system(modelName);
end

%% Inputs
% Scenario function name
defaultScenarioFcnName = "scenario_LC_06_DoubleLaneChange";
validScenarioFcnNames = [...
        "scenario_LC_01_SlowMoving"
        "scenario_LC_02_SlowMovingWithPassingCar"
        "scenario_LC_03_DisabledCar"
        "scenario_LC_04_CutInWithBrake"
        "scenario_LC_05_SingleLaneChange"
        "scenario_LC_06_DoubleLaneChange"
        "scenario_LC_07_RightLaneChange"
        "scenario_LC_08_SlowmovingCar_Curved"
        "scenario_LC_09_CutInWithBrake_Curved"
        "scenario_LC_10_SingleLaneChange_Curved"];
    
checkScenarioFncName = @(x) any(strcmp(x,validScenarioFcnNames));

% Parse inputs
p = inputParser;
addOptional(p,'ScenarioFcnName',defaultScenarioFcnName,checkScenarioFncName);
parse(p,varargin{:});

scenarioFcnName = p.Results.ScenarioFcnName;

%% Scenario parameters
% Set random seed to ensure reproducibility.
rng(0);

% Call scenario function
scenarioFcnHandle = str2func(scenarioFcnName);
[scenario, ~] = scenarioFcnHandle();

% Default assessments
assessment.TimeGap = 0.8;
assessment.LongitudinalJerkMax = 5;
assessment.LateralJerkMax = 5;

% Assign scenario object and assessment to base workspace
assignin('base', 'scenario', scenario);
assignin('base', 'assessment', assessment);

% Simulation stop time
stopTime = scenario.StopTime;
assignin('base','simStopTime', stopTime);

%% Extract scenario information
% Road center information
assignin('base', 'globalPlanPoints', scenario.RoadSegments.RoadCenters);
evalin('base', 'globalPlanPoints(:,3) = [];');

% Get Ego information from scenario
EgoActor = scenario.Actors;
initialEgoX = EgoActor(1).Position(1);
initialEgoY = EgoActor(1).Position(2);
initialEgoYaw = deg2rad(EgoActor(1).Yaw(1));

% Ego 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)
assignin('base', 'initialEgoVelocity', egoSetVelocity);
% Initial x position of ego car          (m)
assignin('base', 'initialEgoX', initialEgoX);

% Minimum threshold distance to evaluate isGoalReached
minDistThreshold = 3.7;
assignin('base', 'minDistThreshold', minDistThreshold);

% Convert ISO 8855 to SAE J670E coordinates
assignin('base', 'initialEgoY', -initialEgoY);
assignin('base', 'initialEgoYaw', -initialEgoYaw);

% Read actor profiles data from scenario and create a workspace variable
actor_Profiles = scenario.actorProfiles;
assignin('base', 'actor_Profiles', actor_Profiles');


%% General Model Parameters
assignin('base', 'Ts', 0.1);                   % Simulation sample time (s)

%% Path following Controller Parameters
createPathFollowingControllerParams();

%% Buses Creation
evalin('base','helperCreateLCBusObjects');
script_string = sprintf('helperCreateLCBusActorsEgoObject(%d)', ...
                        length(scenario.actorProfiles) - 1);
evalin('base', script_string);

% Update road network parameters for Behavior layer
roadSegments = scenario.RoadSegments;

if numel(roadSegments) == 1
roadCenters = scenario.RoadSegments.RoadCenters;
roadCenters(:,3);
roadCenters(:,3) = [];
validator = validatorOccupancyMap;
map = binaryOccupancyMap(50,50);
validator.Map = map;

EgoActor = scenario.Actors(1);
currentEgoStates = [EgoActor.Position(1), EgoActor.Position(2), ...
deg2rad(EgoActor.Yaw), 0, norm(EgoActor.Velocity),...
0];

% Update  longitudinal sampling distance planningHorizon
% Default is 70 meters.
planningHorizon = 70;
assignin('base', 'planningHorizon', planningHorizon);

%% Update road network information
% Create referencePathFrenet object to find the initial state of the
% vehicle.
refPathFrenet = referencePathFrenet(roadCenters);
frenetStatesCurrent = global2frenet(refPathFrenet, currentEgoStates(1:6));
initialDeviation = frenetStatesCurrent(1,4);
initialEgoLaneID = currentLane(scenario.Actors(1));
laneType         = scenario.RoadSegments.LaneType;
numLanes         = scenario.RoadSegments.NumLanes;

laneWidth = zeros(1,numLanes);
for i = 1:scenario.RoadSegments.NumLanes
    laneWidth(i) = abs(scenario.RoadSegments.CenterLaneBoundaryLocation(i) - ...
                   scenario.RoadSegments.CenterLaneBoundaryLocation(i+1));
end

assignin('base', 'initialDeviation'   , initialDeviation);
assignin('base', 'initialEgoLaneID'   , initialEgoLaneID);
assignin('base', 'laneType'   , laneType);
assignin('base', 'laneWidth'   , laneWidth);

else
    error('This example supports road networks with single road segment');
end
end

function createPathFollowingControllerParams

    %% Path following Controller(PFC) Constraints
    assignin('base', 'defaultSpacing'   , 10);    % Default spacing        (m)
    assignin('base', 'maxAc'            , 2);     % Maximum acceleration   (m/s^2)
    assignin('base', 'minAc'            , -3);    % Minimum acceleration   (m/s^2)
    assignin('base', 'maxSteer'         , 0.26);  % Maximum steering       (rad)
    assignin('base', 'minSteer'         , -0.26); % Minimum steering       (rad)
    assignin('base', 'predictionHorizon', 30);    % Prediction horizon

    % time constant for longitudinal dynamics 1/s/(tau*s+1)
    tau = 0.5;
    assignin('base', 'tau', tau);

    % PFC Dynamics modeling parameters
    assignin('base', 'm',  1575);     % Total mass of vehicle                     (kg)
    assignin('base', 'Iz', 2875);     % Yaw moment of inertia of vehicle          (m*N*s^2)
    assignin('base', 'lf', 1.2);      % Longitudinal distance from c.g. to front tires (m)
    assignin('base', 'lr', 1.6);      % Longitudinal distance from c.g. to rear tires  (m)
    assignin('base', 'Cf', 19000);    % Cornering stiffness of front tires        (N/rad)
    assignin('base', 'Cr', 33000);    % Cornering stiffness of rear tires         (N/rad)
end
