classdef HelperTrajectoryPlanner < matlab.System ...
        & matlab.system.mixin.Propagates ...
        & matlab.system.mixin.CustomIcon
%HelperTrajectoryPlanner generates trajectory using Most Important
%   Objects(MIOs) information.
%
%   This system object generates trajectory for the ego vehicle based on
%   the external replan trigger using trajectoryOptimalFrenet. The
%   trajectoryOptimalFrenet object is created and updated based on the
%   information received from the behavior block (Environment Update) and
%   updated custom state validator. The custom state validator used here
%   will be updated using the MIOs surrounded by the ego vehicle. The state
%   validator used in this example is a dynamic collision checking state
%   validator.
%
%   helperTrajectoryPlanner = HelperTrajectoryPlanner creates a system
%   object, helperTrajectoryPlanner, that generates trajectory using MIOs
%   information.
%
%   helperTrajectoryPlanner = HelperTrajectoryPlanner(Name,Value) creates a
%   system object, helperTrajectoryPlanner, with additional options
%   specified by one or more Name,Value pair arguments:
%
%   'EgoInitialState'       - A 1-by-6 vector representing initial state of
%                             ego vehicle
%
%   'vehicleIndex'          - Number indicates actor id of a vehicle
%
%   Step method syntax: [PrevTimeofReplan, NumTrajPoints, Trajectory,
%   NoValidTrajectory, LateralDeviation] = stepImpl(obj, Clock, ReplanFlag,
%   MIOs, RefPath, Deviation, PlannerTerminalStates, EgoVehicleInfo,
%   RearVehicleID, LeadVehicleID, vehicleBehavior, VehicleCurrentState)
%   returns the planner replan period, PrevTimeofReplan, number of points
%   in generated trajectory, NumTrajPoints, generated trajectory,
%   Trajectory, no valid trajectory flag, NoValidTrajectory, and the
%   lateral deviation from reference path, LateralDeviation based on
%   current simulation time, Clock, planner replan flag, ReplanFlag, Most
%   Important Objects(MIOs)MIOs, reference path, RefPath, deviation offset
%   from reference path, Deviation, terminal states w.r.t. vehicle
%   behavior, PlannerTerminalStates, ego vehicle information,
%   EgoVehicleInfo, real vehicle actor id, RearVehicleID, lead vehicle
%   actor id, LeadVehicleID, selected vehicle behavior, vehicleBehavior,
%   target vehicle current state, VehicleCurrentState
%
%   HelperTrajectoryPlanner properties:
%   EgoInitialState         - Ego initial state represented by 1x6 vector
%                             containing [poseX,poseY, yaw, acceleration,
%                             velocityX, velocityY]
%   vehicleIndex            - Intelligent vehicle Index
%
%   HelperTrajectoryPlanner methods:
%   stepImpl                        - Generates trajectory and additional
%                                     info for a configured intelligent
%                                     vehicle.
%   updateTerminalStates            - Updates terminal states of
%                                     trajectoryOptimalFrenet object based
%                                     the configured vehicle behavior.
%   validateTrajectory              - Validated the trajectory and updates
%                                     flag to initiate replan if the
%                                     trajectory is not valid.
%   constructStopBehaviorTrajectory - Constructors trajectory for stop
%                                     behavior.
%   updateStateValidator            - Updates state validator with current
%                                     state of vehicle and MIOs.
%   updateCartStates                - Update the current state of ego
%                                     vehicle using 1x7 Cartesian vector.
%
%   For more information see trajectoryOptimalFrenet and
%   nav.StateValidator
%
%   NOTE: The name of this class and it's functionality may change without
%   notice in a future release, or the class itself may be removed.

%   Copyright 2020 The MathWorks, Inc.

    properties(Access = public)
        %Ego initial state representing
        %[poseX, poseY, yaw, acceleration, velocityX, velocityY]
        %   Default: [0 0 0 0 0 0]
        EgoInitialState = [0 0 0 0 0 0];
        
        %Intelligent vehicle Index
        %
        %   Default: 1
        vehicleIndex = 1;
        
    end
    properties(Access = private)
        %ReferencePathPoints waypoints for the reference path and values
        %   scale in meters
        %   Waypoints for the reference path (Global Plan). Initialized
        %   to a default value of 600m long straight road
        %
        %   Default: [0, 0; 600, 0]
        ReferencePathPoints;
        
        %Planner holds the trajectoryOptimalFrenet object.
        Planner;
        
        %CustomValidator  holds the validatorBoundingBoxList object.
        CustomValidator;
        
        %Trajectory holds the trajectory generated by the
        %trajectoryOptimalFrenet.
        %   Trajectory variables is a nx7 vector that represents the state
        %   of each point on the generated trajectory.
        Trajectory;
        
        %TimeOfReplan stores the time at which replan is performed
        % TimeOfReplan holds previous simulation time at which replan is
        % invoked. This helps in calculating delta time for next replan
        % cycle
        %   Default: 0
        TimeOfReplan;
        
    end
    properties (Constant)
        %CarPolygon vehicle dimensions
        %   The CarPolygon variables is a 4x2 matrix representing the four
        %   vertices of a polygon. The polygon represents the vertices of
        %   the vehicle.
        %   Vehicle dimensions are created by assuming the center at the
        %   rear axial of the vehicle.
        CarPolygon = [3.7, 0.9; 3.7, -0.9;...
                     -1.0, -0.9;-1.0,0.9]';
        
        %MinRangeMagnitude minimum range magnitude to search for MIOs
        %
        MinRangeMagnitude = 0;
        
        %MaxRangeMagnitude maximum range magnitude to search for MIOs
        %
        MaxRangeMagnitude = 500;
    end
    
    %----------------------------------------------------------------------
    % Constructor
    %----------------------------------------------------------------------
    methods
        %------------------------------------------------------------------
        function obj = HelperTrajectoryPlanner(varargin)
            % HelperTrajectoryPlanner constructor.
            setProperties(obj,nargin,varargin{:})
            
            % Make ReferencePathPoints variable variable sized
            coder.varsize('referencePathPoints');
            referencePathPoints = [0 0; 600, 0];
            obj.ReferencePathPoints = referencePathPoints;
        end
    end
    
    %----------------------------------------------------------------------
    % Main algorithm
    %----------------------------------------------------------------------
    methods(Access = protected)
        %------------------------------------------------------------------
        function setupImpl(obj)
            %setupImpl Perform one-time calculations and initializations.
            %   The setupImpl method is used to initialize CarPolygon,
            %   CustomValidator, Planner and DeviationValue properties.
            %   CustomValidator is the object of validatorBoundingBoxList
            %   Planner is the object of trajectoryOptimalFrenet
            %   Initializes obstacles in the state validator.
            
            % Create and assign validatorBoundingBoxList object
            obj.CustomValidator = HelperValidatorBoundingBoxList(obj.CarPolygon, obj.ReferencePathPoints);
            
            % Planner object from the Trajectory Optimal Frenet for the
            % given reference path and the custom state validator.
            obj.Planner = ...
                trajectoryOptimalFrenet(obj.ReferencePathPoints,...
                obj.CustomValidator);
            
            % Relax FeasibilityParameters for planner
            % Set MaxAcceleration to 5m/s^2 from default 2.5 m/s^2
            obj.Planner.FeasibilityParameters.MaxAcceleration = 5;
            
            % Update TerminalStates.Time. initializing this using the
            % longitudinal distance and the terminal speed. At
            % initialization we are assuming both initial and terminal
            % speed as same.
            desiredTime = ((obj.Planner.TerminalStates.Longitudinal)/...
                obj.Planner.TerminalStates.Speed);
            
            % Checking with upper bound for desiredTime
            % Upper bound value(99.9) is calculated based on max limit
            % of trajectory points(1000)
            % desiredTime = maxTrajectoryPoints(1000) *
            %               timeForEachTrajPointCalc(0.1) - 0.1;
            if (desiredTime > 99.9)
                desiredTime = 99.9;
            end
            
            % Update the terminal state time with the calculated time value
            obj.Planner.TerminalStates.Time = desiredTime;
            
            % Initialize Obstacles (MIOS) outside of the road boundaries.
            obj.CustomValidator.addObstacle([NaN NaN NaN],[0 0],obj.CarPolygon,'world');
            obj.CustomValidator.addObstacle([NaN NaN NaN],[0 0],obj.CarPolygon,'world');
            obj.CustomValidator.addObstacle([NaN NaN NaN],[0 0],obj.CarPolygon,'world');
            obj.CustomValidator.addObstacle([NaN NaN NaN],[0 0],obj.CarPolygon,'world');
            obj.CustomValidator.addObstacle([NaN NaN NaN],[0 0],obj.CarPolygon,'world');
            
            % Initialize the TimeOfReplan to zero to indicate the start of
            % simulation
            obj.TimeOfReplan = 0;
            
        end
        
        %------------------------------------------------------------------
        function [PrevTimeofReplan, NumTrajPoints, Trajectory,...
                NoValidTrajectory, LateralDeviation] = ...
                stepImpl(obj,  Clock, ReplanFlag, MIOs, RefPath, Deviation,...
                PlannerTerminalStates, Ego, RearVehicleID, LeadVehicleID,...
                vehicleBehavior, VehicleCurrentState)
            %stepImpl Implements the main logic to generate trajectory for
            %a configured vehicle behavior.
            %   The stepImpl method implements the main logic to generate
            %   trajectory and returns PrevTimeofReplan, NumTrajPoints,
            %   Trajectory, NoValidTrajectory, LateralDeviation. This method
            %   updates obstacle information and generates a trajectory for
            %   based the ReplanFlag.
            %
            %   This method updates obstacle information based on MIOs
            %   information and current ego information to
            %   validatorBoundingBoxList. Also updates the terminal states
            %   of trajectoryOptimalFrenet before generating a trajectory.
            %   The trajectoryOptimalFrenet samples trajectories based the
            %   current ego state(Frenet states) using the updated terminal
            %   states. These sampled trajectories are being validated for
            %   feasibility and collision and outputs a feasible, optimal
            %   and collision free trajectory.
            
            % Update reference path if it is not the same as the one that
            % is already available.
            if ~isequal(RefPath, obj.ReferencePathPoints)
                obj.ReferencePathPoints = RefPath;
                obj.Planner.Waypoints = obj.ReferencePathPoints;
                obj.CustomValidator.PlannerObj.Waypoints = obj.ReferencePathPoints;
            end
            
            % Convert Cartesian state of the ego vehicle to Frenet state
            CartStates = updateCartStates(obj,obj.EgoInitialState,VehicleCurrentState);
            frenetStates = obj.Planner.cart2frenet(CartStates(1:6));
            
            % Initialize NoValidTrajectory to false
            % This will be set to true when there
            % is no valid trajectory from trajectoryOptimalFrenet and this
            % internally enables ReplanFlag.
            NoValidTrajectory = false;
            % Check for ReplanFlag
            if ReplanFlag
                % Set the terminal states for the planner based on the
                % vehicle behavior. Terminal states are being computed in
                % terminal state samplers.
                updateTerminalStates(obj, PlannerTerminalStates, vehicleBehavior);
                
                % Update TimeOfReplan with current time
                obj.TimeOfReplan = Clock;
                
                % Place the other actors based on their positions from scenario
                updateStateValidator(obj, VehicleCurrentState, MIOs, Ego,...
                    RearVehicleID, LeadVehicleID, vehicleBehavior);
                
                
                % Calculate the desired time for achieving the longitudinal
                % terminal state by taking the average of current ego
                % velocity and terminal velocity.
                desiredTime = ((obj.Planner.TerminalStates.Longitudinal)/...
                    ((obj.Planner.TerminalStates.Speed + CartStates(1,5))/2));
                
                % Checking with upper bound for desiredTime
                % Upper bound value(99.9) is calculated based on max limit
                % of trajectory points(1000)
                % desiredTime = maxTrajectoryPoints(1000) *
                %               timeForEachTrajPointCalc(0.1) - 0.1;
                if (desiredTime > 99.9)
                    desiredTime = 99.9;
                end
                
                % Update the terminal state time with the calculated time value
                obj.Planner.TerminalStates.Time = desiredTime;
                
                % Set the deviation required for the planner
                obj.Planner.DeviationOffset = Deviation;
                
                % Call plan function by passing current ego state in Frenet
                % frame
                obj.Trajectory = obj.Planner.plan(frenetStates);
                
                % Validate the trajectory. If there is no valid trajectory
                % and the vehicle behavior is LaneChange, configure
                % trajectoryOptimalFrenet with LaneFollowing behavior.
                % If the behavior is either LaneFollowing or
                % VelocityKeeping then goto stop behavior.
                NoValidTrajectory = validateTrajectory(obj,...
                    PlannerTerminalStates, CartStates,...
                    VehicleCurrentState, frenetStates, vehicleBehavior);
            end
            % Update output variables
            
            % Update lateral deviation
            LateralDeviation = frenetStates(4);
                 
            % Create a fixed size variables for output
            Trajectory = nan(1000,7);
            
            % Get number of points in the trajectory
            NumTrajPoints = size(obj.Trajectory,1);
            
            % Copy the generated trajectory to the output variables
            Trajectory(1:NumTrajPoints,:) = obj.Trajectory(1:NumTrajPoints,:);
            
            % Output the previous TimeOfReplan
            PrevTimeofReplan = obj.TimeOfReplan;
        end
        
        %------------------------------------------------------------------
        function updateTerminalStates(obj, PlannerTerminalStates, vehicleBehavior)
            %updateTerminalStates Updates terminal states
            %   updateTerminalStates function updates terminal states of
            %   planner based on the vehicle behavior. PlannerTerminalStates
            %   are defined using the samplers.These terminal states are
            %   derived for each of the behavior of the vehicle.
            %   PlannerTerminalStates is an array of structure contains
            %   following fields:
            %     Longitudinal
            %         NumSamples
            %         Samples
            %     Lateral
            %         NumSamples
            %         Samples
            %     Speed
            %         NumSamples
            %         Samples
            %     Acceleration
            %         NumSamples
            %         Samples
            %     Time
            %   PlannerTerminalStates(1) : Contains states for
            %   VelocityKeeping behavior
            %
            %   PlannerTerminalStates(2) : Contains states for LaneFollowing
            %   behavior
            %   
            %   PlannerTerminalStates(3) : Contains states for LaneChange
            %   behavior
                        
            % Check for the vehicle behavior
            switch vehicleBehavior

                % Update terminal states for velocity keeping behavior
                case VehicleBehavior.VelocityKeeping
                    obj.Planner.TerminalStates.Longitudinal =  PlannerTerminalStates(1).Longitudinal.Samples(:, 1:PlannerTerminalStates(1).Longitudinal.NumSamples);
                    obj.Planner.TerminalStates.Lateral      =  PlannerTerminalStates(1).Lateral.Samples(:, 1:PlannerTerminalStates(1).Lateral.NumSamples);
                    velocitySamples                         =  PlannerTerminalStates(1).Speed.Samples(:, 1:PlannerTerminalStates(1).Speed.NumSamples);
                    obj.Planner.TerminalStates.Speed        =  velocitySamples(1);
                    
                    % Update terminal states for lane following behavior
                case VehicleBehavior.LaneFollowing
                    obj.Planner.TerminalStates.Longitudinal =  PlannerTerminalStates(2).Longitudinal.Samples(:, 1:PlannerTerminalStates(2).Longitudinal.NumSamples);
                    obj.Planner.TerminalStates.Lateral      =  PlannerTerminalStates(2).Lateral.Samples(:, 1:PlannerTerminalStates(2).Lateral.NumSamples);
                    velocitySamples                         =  PlannerTerminalStates(2).Speed.Samples(:, 1:PlannerTerminalStates(2).Speed.NumSamples);
                    obj.Planner.TerminalStates.Speed        =  velocitySamples(1);
                    
                    % Update terminal states for lane change behavior
                case VehicleBehavior.LaneChange
                    obj.Planner.TerminalStates.Longitudinal =  PlannerTerminalStates(3).Longitudinal.Samples(:, 1:PlannerTerminalStates(3).Longitudinal.NumSamples);
                    obj.Planner.TerminalStates.Lateral      =  PlannerTerminalStates(3).Lateral.Samples(:, 1:PlannerTerminalStates(3).Lateral.NumSamples);                    
                    velocitySamples                         =    PlannerTerminalStates(3).Speed.Samples(:, 1:PlannerTerminalStates(3).Speed.NumSamples);
                    obj.Planner.TerminalStates.Speed        =  velocitySamples(1);
                    % Error out if the specified behavior is undefined.
                otherwise
                    error("Unsupported VehicleBehavior. VehicleBehavior must be 'Lane Following' or 'Velocity Keeping' or 'Lane Change'");
            end
        end
        
        %------------------------------------------------------------------
        function triggerReplan = validateTrajectory(obj,...
                plannerTerminalStates, cartStates, vehicleCurrentState,...
                frenetStates, vehicleBehavior)
            %validateTrajectory validates trajectory based on it's size
            %   validateTrajectory function validates the generated
            %   trajectory. If there is no trajectory from the
            %   planner(trajectoryOptimalFrenet) validateTrajectory function
            %   does the following:
            %   If there is no trajectory from the planner
            %      Check the vehicle behavior. If the vehicle behavior is
            %      LaneChange, then configure the planner with
            %      LaneFollowing behavior by updating the terminal states.
            %      Invoke plan function after updating the terminal states.
            %      If there is no trajectory from planner with
            %      LaneFollowing behavior then goto stop mode. If the
            %      behavior is not LaneChange then go to stop mode
            %      directly.
            
            triggerReplan = false;
            % Check if the trajectory is empty (No feasible trajectory from
            % Planner)
            if isempty(obj.Trajectory)
                
                % Check if the behavior is LaneChange
                if vehicleBehavior == VehicleBehavior.LaneChange
                    
                    % Configure planner with Lane Following behavior by
                    % updating terminal states.
                    obj.Planner.TerminalStates.Longitudinal =  plannerTerminalStates(2).Longitudinal.Samples(:, 1:plannerTerminalStates(2).Longitudinal.NumSamples);
                    obj.Planner.TerminalStates.Lateral      =  plannerTerminalStates(2).Lateral.Samples(:, 1:plannerTerminalStates(2).Lateral.NumSamples);
                    velocitySamples                         =  plannerTerminalStates(2).Speed.Samples(:, 1:plannerTerminalStates(2).Speed.NumSamples);
                    obj.Planner.TerminalStates.Speed        =  velocitySamples(1);                    
                    
                    % Calculate the desired time for achieving the longitudinal
                    % terminal state by taking the average of current ego
                    % velocity and terminal velocity.
                    desiredTime = ((obj.Planner.TerminalStates.Longitudinal)/...
                        ((obj.Planner.TerminalStates.Speed + cartStates(1,5))/2));
                    
                    % Checking with upper bound for desiredTime
                    % Upper bound value(99.9) is calculated based on max limit
                    % of trajectory points(1000)
                    % desiredTime = maxTrajectoryPoints(1000) *
                    %               timeForEachTrajPointCalc(0.1) - 0.1;
                    if (desiredTime > 99.9)
                        desiredTime = 99.9;
                    end
                    
                    % Update the time state of the planner
                    obj.Planner.TerminalStates.Time = desiredTime;
                    
                    % Invoke plan method to generate trajectory
                    obj.Trajectory = obj.Planner.plan(frenetStates);
                    
                    % Check if the trajectory is empty for lane following
                    % behavior
                    if isempty(obj.Trajectory)
                        % Construct trajectory with stop behavior
                        [triggerReplan,...
                            stopBehaviorTrajectory] = constructStopBehaviorTrajectory(obj, vehicleCurrentState);
                        obj.Trajectory   = stopBehaviorTrajectory;
                    end
                else
                    % If the behavior is either LaneFollowing or
                    % VelocityKeeping, then go to stop behavior
                    [triggerReplan, stopBehaviorTrajectory] = constructStopBehaviorTrajectory(obj, vehicleCurrentState);
                    obj.Trajectory   = stopBehaviorTrajectory;
                end
            end
        end
        
        %------------------------------------------------------------------
        function [triggerReplan,...
                stopBehaviorTrajectory] = constructStopBehaviorTrajectory(...,
                obj, VehicleCurrentState)
            %constructStopBehaviorTrajectory constructs trajectory for stop
            %   behavior
            %   constructStopBehaviorTrajectory function constructs trajectory
            %   for stop behavior using the vehicle current state.
            %   The constructed trajectory contains same values for each
            %   trajectory point.
            
            % Generate time vector for the trajectory similar to planner.
            timeVec          = (0:obj.Planner.TimeResolution:obj.Planner.TerminalStates.Time)';
            
            % get the size of the time vector to calculate the number of
            % points in the trajectory
            trajPoints       = size(timeVec);
            
            % Check if this is the first simulation step
            if getCurrentTime(obj)>= 1
                
                % Use vehicle current state if this is not the first
                % planning cycle
                currentTrajPoint = [VehicleCurrentState.PositionX, VehicleCurrentState.PositionY,...
                    VehicleCurrentState.Yaw/180*pi, VehicleCurrentState.Curvature,...
                    0, 0];
            else
                % Use vehicle initial state if this is first planning cycle
                currentTrajPoint = [obj.EgoInitialState(1:2) obj.EgoInitialState(3)/180*pi 0 0 0];
            end
            
            % Repeat the current state to construct the trajectory for
            % total time
            stopBehaviorTrajectory  = repmat(currentTrajPoint,[trajPoints(1), 1]);
            
            % Append time state as the trajectory is a nx7 matrix.
            stopBehaviorTrajectory  = [stopBehaviorTrajectory, timeVec];
            triggerReplan          = true;
        end
        
        %------------------------------------------------------------------
        function updateStateValidator(obj, vehicleCurrentState, MIOs,...
                Ego, rearVehicleID, leadVehicleID, vehicleBehavior)
            %updateStateValidator This will update the state validator with
            %   the vehicle current state and MIO information
            
            % Update the vehicle current position to state validator
            if getCurrentTime(obj)>= 1
                
                % If this is not first planning cycle use the
                % vehicleCurrentState to update the ego state
                obj.CustomValidator.updateEgoState([vehicleCurrentState.PositionX,...
                    vehicleCurrentState.PositionY, deg2rad(vehicleCurrentState.Yaw)],...
                    vehicleCurrentState.Velocity(1:2),...
                    vehicleCurrentState.AngularVelocity(1));
            else
                % If this is the first planning cycle use the
                % EgoInitialState to update the ego state
                obj.CustomValidator.updateEgoState([obj.EgoInitialState(1:2),...
                    obj.EgoInitialState(3)/180*pi],...
                    [obj.EgoInitialState(5), obj.EgoInitialState(6)],...
                    deg2rad(0));
            end
            
            % Update the MIO information. This function considers all the
            % vehicles as MIOs as we fixed number of vehicles in the
            % scenario.
            % MIO information is being updated based on vehicle behavior as
            % follows:
            %
            % VelocityKeeping behavior: This behavior will not have any
            % MIOs.
            %
            % Lane Following: This behavior considers lead vehicle as MIO.
            %
            % LaneChange behavior: This behavior excludes current vehicle
            %                      and the rear vehicle.
            
            % Check if the behavior is LaneChange
            if vehicleBehavior == VehicleBehavior.LaneChange                
                for i=1:numel(MIOs)
                    % Update obstacleId with the obstacle index
                    obstacleId = sprintf('obstacle_%u',cast(i,'uint32'));
                    
                    % Ignore current vehicle and rear vehicle
                    if (i ~= obj.vehicleIndex && i ~= rearVehicleID - 1)
                        
                        % Update the state validator with obstacle
                        % information
                        obj.CustomValidator.updateObstacle(obstacleId,...
                            [MIOs(i).Position(1:2), MIOs(i).Yaw],...
                            MIOs(i).Velocity(1:2),obj.CarPolygon,'world');
                    else
                        % Update the state validator with NaN values
                        % if the vehicle is not an MIO
                        obj.CustomValidator.updateObstacle(obstacleId,...
                            [NaN NaN NaN],...
                            [NaN, NaN],obj.CarPolygon,'world');
                    end
                end
                
                % Update the ego vehicle information to state validator
                
                % Check if the ego vehicle(Vehicle under test) is rear
                % vehicle
                if Ego.ActorID == rearVehicleID
                    
                    % Update the obstacle information with NaN values
                    obj.CustomValidator.updateObstacle('egoVehicle',...
                        [NaN, NaN, NaN], [NaN, NaN],obj.CarPolygon,'world');
                else
                    % Update the obstacle information current position and
                    % velocity of the ego vehicle
                    obj.CustomValidator.updateObstacle('egoVehicle',...
                        [Ego.Position(1:2), Ego.Yaw],...
                        Ego.Velocity(1:2),obj.CarPolygon,'world');
                end
            elseif vehicleBehavior == VehicleBehavior.LaneFollowing
                % Update the lead vehicle information as MIO information to
                % state validator.
                if Ego.ActorID == leadVehicleID
                    obj.CustomValidator.updateObstacle('egoVehicle',...
                        [Ego.Position(1:2), Ego.Yaw],...
                        Ego.Velocity(1:2),obj.CarPolygon,'world');
                else
                    for i=1:numel(MIOs)
                        obstacleId = sprintf('obstacle_%u',cast(i,'uint32'));
                        if i == leadVehicleID - 1
                            obj.CustomValidator.updateObstacle(obstacleId,...
                                [MIOs(i).Position(1:2), MIOs(i).Yaw],...
                                MIOs(i).Velocity(1:2),obj.CarPolygon,'world');
                        else
                            obj.CustomValidator.updateObstacle(obstacleId,...
                                [NaN NaN NaN],...
                                [NaN, NaN],obj.CarPolygon,'world');
                        end
                    end
                end
            else
                % For VelocityKeeping behavior, there won't be any MIOs.
                % Update the state validator with NaN values for all the
                % vehicles.
                for i=1:numel(MIOs)
                    obstacleId = sprintf('obstacle_%u',cast(i,'uint32'));
                    obj.CustomValidator.updateObstacle(obstacleId,...
                        [NaN NaN NaN],...
                        [NaN, NaN],obj.CarPolygon,'world');
                end
                obj.CustomValidator.updateObstacle('egoVehicle',...
                    [NaN, NaN, NaN], [NaN, NaN],obj.CarPolygon,...
                    'world');
                
            end
        end
        
        %------------------------------------------------------------------
        function CartStates = updateCartStates(obj,egoInitialState,...
                vehicleCurrentState)
            %UpdateCartStates This updates ego current state from the
            %   trajectory and EgoActor bus object to extract the 1x7 state
            %   vector.
            
            if getCurrentTime(obj)>= 1 % after first planning cycle trajectory does exist
                % Use vehicleCurrentState to 
                CartStates = [vehicleCurrentState.PositionX, vehicleCurrentState.PositionY,...
                    vehicleCurrentState.Yaw/180*pi, vehicleCurrentState.Curvature,...
                    norm(vehicleCurrentState.Velocity), vehicleCurrentState.Acceleration];
            else
                % before first planning cycle trajectory does not exist
                % Use the initial state of the vehicle
                Cart_Acc  = 0;
                CartStates = [egoInitialState(1:2) egoInitialState(3)/180*pi 0 egoInitialState(5) Cart_Acc];
            end
        end
    end
    
    %----------------------------------------------------------------------
    % Common methods
    %----------------------------------------------------------------------
    methods(Access = protected)
        %------------------------------------------------------------------
        function s = saveObjectImpl(obj)
            % Save Object
            s = saveObjectImpl@matlab.System(obj);
        end
        
        %------------------------------------------------------------------
        function loadObjectImpl(obj,s,wasLocked)
            % Load Object
            loadObjectImpl@matlab.System(obj,s,wasLocked);
        end
    end    
    
    %----------------------------------------------------------------------
    % Simulink-only methods
    %----------------------------------------------------------------------
    methods(Access = protected)
        %------------------------------------------------------------------
        function [PrevTimeofReplan, NumTrajPoints, Trajectory, NoValidTrajectory,...
                  LateralDeviation] = getOutputSizeImpl(~)
            % Return size for each output port
            Trajectory       = [1000 7];
            NumTrajPoints    = 1;
            PrevTimeofReplan = 1;
            LateralDeviation = 1;
            NoValidTrajectory    = 1;
        end
        
        %------------------------------------------------------------------
        function [PrevTimeofReplan, NumTrajPoints, Trajectory, NoValidTrajectory,...
                  LateralDeviation] = getOutputDataTypeImpl(~)
            % Return data type for each output port
            Trajectory       = "double";
            NumTrajPoints    = "double";
            PrevTimeofReplan = "double";
            LateralDeviation = "double";
            NoValidTrajectory    = "boolean";
        end
        
        %------------------------------------------------------------------
        function [PrevTimeofReplan, NumTrajPoints, Trajectory, NoValidTrajectory,...
                LateralDeviation] = isOutputComplexImpl(~)
            % Return true for each output port with complex data
            Trajectory       = false;
            NumTrajPoints    = false;
            PrevTimeofReplan = false;
            LateralDeviation =  false;
            NoValidTrajectory    = false;
        end
        
        %------------------------------------------------------------------
        function [PrevTimeofReplan, NumTrajPoints, Trajectory, NoValidTrajectory,...
                 LateralDeviation] = isOutputFixedSizeImpl(~)
            % Return true for each output port with fixed size
            Trajectory       = true;
            NumTrajPoints    = true;
            PrevTimeofReplan = true;
            LateralDeviation = true;
            NoValidTrajectory    = true;
        end
        
        %------------------------------------------------------------------
        function icon = getIconImpl(~)
            % Define icon for System block
            icon = mfilename("class");
        end
        
        %------------------------------------------------------------------
        function flag = supportsMultipleInstanceImpl(~)
            flag = true;
        end
    end
    
    %----------------------------------------------------------------------
    % Simulink dialog
    %----------------------------------------------------------------------
    methods(Static, Access = protected)
        % Simulink customization functions
        function header = getHeaderImpl
            % Define header panel for System block dialog
            header = matlab.system.display.Header(mfilename("class"));
        end
        
        function group = getPropertyGroupsImpl
            % Define property section(s) for System block dialog
            group = matlab.system.display.Section(mfilename("class"));
        end
    end
end
