classdef exampleHelperTrajectoryGenerator < matlab.System ...
        & matlab.system.mixin.Propagates ...
        & matlab.system.mixin.CustomIcon
%exampleHelperTrajectoryGenerator generates trajectory using Most Important 
%Objects(MIOs) information.
%    
%   This system object generates trajectory for the ego vehicle at 
%   every 1sec using trajectoryOptimalFrenet. The trajectoryOptimalFrenet
%   object is created and updated based on the information received from
%   the behavior block (Update Global Plan and Deviation) 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.

%   For more information see trajectoryOptimalFrenet and nav.StateValidator

%   Copyright 2019 The MathWorks, Inc.

    % Public, non-tunable properties
    properties(Nontunable)
        %ReferencePathPoints waypoints for the reference path
        %   Waypoints for the reference path (Global Plan). This was read
        %   from the base workspace.
        %   Default: [0 0; 600, 0]
        %   values scale in meters
        ReferencePathPoints = [0 0; 600 0];
        
        %SetVelocity is the desired maximum velocity that the ego vehicle
        %should attain.
        %   Set Velocity of the ego vehicle. This will be read from the
        %   base workspace.
        %   Default: 10
        %   Value is in m/s
        
        SetVelocity = 10;
        
        %LongitudinalTerminalStates Planner's longitudinal Terminal states 
        %   Longitudinal terminal states for the trajectoryOptimalFrent
        %   specified by the user. This will be read from the base workspace.
        %   Default: [45 60]
        %   Value is in meters
        LongitudinalTerminalStates = [45 60];
        
        %LateralTerminalStates Planner's lateral terminal states.
        %   Lateral terminal states for the trajectoryOptimalFrent
        %   specified by the user. This will be read from the base workspace.
        %   Default: [-3.6 0 3.6]
        %   Value in meters
        LateralTerminalStates = [-3.6 0 3.6];
    end
    
    % Private variables
    properties(Access = private)
        %Planner holds the trajectoryOptimalFrenet object.
        Planner;
        
        %CustomValidator  holds the validatorBoundingBoxList object.
        CustomValidator;               
        
        %Trajectory Trajectory generated by the trajectoryOptimalFrenet
        %   Trajectory variables is a nx7 vector that represents the state
        %   each point on the generated trajectory.
        Trajectory;
        
        %PreviousTime PreviousTime stores the system time at which the
        %trajectory is being generated.
        %   The PreviousTime variables is used to store the system time when
        %   the re-planning has called. This is used to calculate the time
        %   difference at every simulation step and calls re-plan for every
        %   1sec.
        PreviousTime;
        
        %DeviationValue DeviationValue stores the deviation value from
        %current reference path.
        %   The DeviationValue variable is used to store the current deviation
        %   value from the reference path. The deviation value is one of the 
        %   input for this system object. This value is being is computed 
        %   by the Global plan and deviation block.
        DeviationValue
    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.35, 0.9; 3.35, -0.9;...
            -1.35, -0.9;-1.35,0.9]';
        
        %MinRangeMagnitudeMagnitude minimum range magnitude to search for MIOs
        MinRangeMagnitude = 0;
        
        %MaxRangeMagnitude maximum range magnitude to search for MIOs
        MaxRangeMagnitude = 500;
    end
    
    methods
        function obj = exampleHelperTrajectoryGenerator(varargin)
            % exampleHelperTrajectoryGenerator constructor.
            setProperties(obj,nargin,varargin{:})
        end
    end
    
    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.                           
            
            % Initialize validatorBoundingBoxList object
            obj.CustomValidator = validatorBoundingBoxList(obj.CarPolygon);
            
            % DeviationValue defines the deviation from reference path.            
            % Default: 0            
            obj.DeviationValue = 0;
            
            % Planner object from the Trajectory Optimal Frenet for the
            % given reference path and the custom state validator.
            obj.Planner = ...
                trajectoryOptimalFrenet(obj.ReferencePathPoints,...
                obj.CustomValidator);
            % Update terminal states of the planner with the parameter
            % values 
            %
            % Update Terminal speed with user defined value
            obj.Planner.TerminalStates.Speed = obj.SetVelocity;
            
            % Relax FeasibilityParameters for planner
            % Set MaxAcceleration to 5m/s^2 from default 2.5 m/s^2
            obj.Planner.FeasibilityParameters.MaxAcceleration = 5;
            
            % Update longitudinal terminal states
            obj.Planner.TerminalStates.Longitudinal = ...
                obj.LongitudinalTerminalStates;
            
            % Update lateral terminal states
            obj.Planner.TerminalStates.Lateral = obj.LateralTerminalStates;
            
            % 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);
            obj.Planner.TerminalStates.Time = desiredTime;            
            
            % Initialize Obstacles (MIOS) outside of the road boundaries.
            obj.CustomValidator.addObstacle([-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            obj.CustomValidator.addObstacle([-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            obj.CustomValidator.addObstacle([-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            obj.CustomValidator.addObstacle([-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            obj.CustomValidator.addObstacle([-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            obj.CustomValidator.addObstacle([-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            
            % Initializing the PreviousTime variables.
            obj.PreviousTime = 0;
            
        end
  
        function [Trajectory, NumTrajPoints, ...
                isTrajNew, PreviousDeviation, PreviousWaypoints] = ...
                stepImpl(obj, EgoActor, EgoFront, EgoRear, ...
                LeftFront, LeftRear, RightFront, RightRear, ...
                SystemTime, Deviation, DeviationChanged,...
                LanePreferenceCost, UpdatedWaypoints)
            %stepImpl Implements the main logic to perform a lane change
            %   The stepImpl method implements the main logic and returns 
            %   trajectory, numTrajPoints, previousDeviation, previousWaypoints.
            %   This method updates obstacle information and generates a
            %   trajectory for every 1sec.
            %
            %   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 generates optimal trajectory 
            %   based the current ego state(frenet states) using the
            %   updated terminal states. 
 
            % Calculate the time difference between the system time and
            % Previous re-plan time.
            timeDiff = abs(SystemTime - obj.PreviousTime);
            
            % isTrajNew variable indicates state of the trajectory.
            % isTrajNew will become true at every 1sec for the indication 
            % of new trajectory generation from Planner.
            isTrajNew = false;
            
            % If the difference is more than 1s(one sec) re-plan the trajectory.
            if(timeDiff >= 1 || SystemTime == 0)
                
                % preserve the system time for next iteration.
                obj.PreviousTime = SystemTime;
                
                % Get the state of the ego vehicle.
                % state vector holds following data 
                % [x, y, theta, kapa, velocity, acceleration]
                states = [EgoActor.Position(1), EgoActor.Position(2), ...
                    deg2rad(EgoActor.Yaw), 0, norm(EgoActor.Velocity),0];
                
                % Place the other actors based on their positions from scenario
                obj.updateStateValidator(EgoFront, EgoRear, LeftFront, LeftRear, RightFront,RightRear);
                
                % Update the ego vehicle position to state validator               
                obj.CustomValidator.updateEgoState([EgoActor.Position(1:2),...
                            deg2rad(EgoActor.Yaw)], EgoActor.Velocity(1:2),...
                            deg2rad(EgoActor.AngularVelocity(3)));
                
                % 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 + states(1,5))/2));
                obj.Planner.TerminalStates.Time = desiredTime;
                
                % If deviation changes update the Waypoints of the 
                % reference path. Update the terminal states accordingly.
                if DeviationChanged
                    obj.Planner.Waypoints = UpdatedWaypoints;
                    obj.DeviationValue = Deviation;
                    obj.Planner.TerminalStates.Lateral = obj.Planner.TerminalStates.Lateral - Deviation - LanePreferenceCost;
                end
                
                % Get the frenet states from the Cartesian states.
                frenetStates = obj.Planner.cart2frenet(states(1:6));
                
                % Call plan function by passing current ego state in frenet
                obj.Trajectory = obj.Planner.plan(frenetStates);
                
                % Set isTrajNew to inticate the new trajectory.
                isTrajNew = true;                
            end
            % Copy the trajectory points to the Output
            % 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,:);
            
            % Assign deviation value to output variables 
            PreviousDeviation = obj.DeviationValue;
            
            % Assign Waypoints to output variables 
            PreviousWaypoints = obj.Planner.Waypoints(1:2,1:2);
        end
        
        function updateStateValidator(obj, egoFront, egoRear, leftFront, leftRear, rightFront,rightRear)
            %updateStateValidator This will update the state validator with
            %the MIO information
            
            % Update EgoFront MIO position
            if egoFront.RangeMagnitude > obj.MinRangeMagnitude && egoFront.RangeMagnitude < obj.MaxRangeMagnitude
                obj.CustomValidator.updateObstacle('obstacle_1',[egoFront.Position(1:2)', atan2(egoFront.Velocity(2),egoFront.Velocity(1))],...
                    egoFront.Velocity(1:2),obj.CarPolygon,'ego');
            else
                % Place the obstacle_1(egoFront) at (-1000, -1000)
                % coordinate in the state validator. Assuming that this
                % coordinte lies out side the driviable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_1',[-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            end
            
            % Update EgoRear MIO position
            if egoRear.RangeMagnitude > obj.MinRangeMagnitude && egoRear.RangeMagnitude < obj.MaxRangeMagnitude
                obj.CustomValidator.updateObstacle('obstacle_2',[egoRear.Position(1:2)', atan2(egoRear.Velocity(2),egoRear.Velocity(1))],...
                    egoRear.Velocity(1:2),obj.CarPolygon,'ego');
            else
                % Place the obstacle_2(egoRear) at (-1000, -1000)
                % coordinate in the state validator. Assuming that this
                % coordinte lies out side the driviable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_2',[-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            end
            
            % Update Ego LeftFront MIO position
            if leftFront.RangeMagnitude > obj.MinRangeMagnitude && leftFront.RangeMagnitude < obj.MaxRangeMagnitude
                obj.CustomValidator.updateObstacle('obstacle_3',[leftFront.Position(1:2)', atan2(leftFront.Velocity(2),leftFront.Velocity(1))],...
                    leftFront.Velocity(1:2),obj.CarPolygon,'ego');
            else
                % Place the obstacle_3(leftFront) at (-1000, -1000)
                % coordinate in the state validator. Assuming that this
                % coordinte lies out side the driviable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_3',[-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            end
            
            % Update Ego LeftFRear MIO position
            if leftRear.RangeMagnitude > obj.MinRangeMagnitude && leftRear.RangeMagnitude < obj.MaxRangeMagnitude
                obj.CustomValidator.updateObstacle('obstacle_4',[leftRear.Position(1:2)', atan2(leftRear.Velocity(2),leftRear.Velocity(1))],...
                    leftRear.Velocity(1:2),obj.CarPolygon,'ego');
            else
                % Place the obstacle_4(leftRear) at (-1000, -1000)
                % coordinate in the state validator. Assuming that this
                % coordinte lies out side the driviable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_4',[-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            end
            
            % Update Ego RightFront MIO position
            if rightFront.RangeMagnitude > obj.MinRangeMagnitude && rightFront.RangeMagnitude < obj.MaxRangeMagnitude
                obj.CustomValidator.updateObstacle('obstacle_5',[rightFront.Position(1:2)', atan2(rightFront.Velocity(2),rightFront.Velocity(1))],...
                    rightFront.Velocity(1:2),obj.CarPolygon,'ego');
            else
                % Place the obstacle_5(rightFront) at (-1000, -1000)
                % coordinate in the state validator. Assuming that this
                % coordinte lies out side the driviable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_5',[-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            end
            
            % Update Ego LeftRear MIO position
            if rightRear.RangeMagnitude > obj.MinRangeMagnitude && rightRear.RangeMagnitude < obj.MaxRangeMagnitude
                obj.CustomValidator.updateObstacle('obstacle_6',[rightRear.Position(1:2)', atan2(rightRear.Velocity(2),rightRear.Velocity(1))],...
                    rightRear.Velocity(1:2),obj.CarPolygon,'ego');
            else
                % Place the obstacle_6(rightRear) at (-1000, -1000)
                % coordinate in the state validator. Assuming that this
                % coordinte lies out side the driviable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_6',[-1000 -1000 0],[0 0],obj.CarPolygon,'ego');
            end
        end
        %% Backup/restore functions
        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
        
        %% Simulink functions
        function [Trajectory, NumTrajPoints, ...
                isTrajNew, PreviousDeviation,...
                PreviousWaypoints] = getOutputSizeImpl(~)
            % Return size for each output port
            Trajectory = [1000 7];
            NumTrajPoints = 1;
            isTrajNew = 1;
            PreviousDeviation = 1;
            PreviousWaypoints = [2 2];
        end
        
        function [Trajectory, NumTrajPoints, ...
                isTrajNew, PreviousDeviation,...
                PreviousWaypoints] = ...
                getOutputDataTypeImpl(~)
            % Return data type for each output port
            Trajectory = "double";
            NumTrajPoints = "double";
            isTrajNew = "boolean";
            PreviousDeviation = "double";
            PreviousWaypoints = "double";
        end
        
        function [Trajectory, NumTrajPoints, ...
                isTrajNew, PreviousDeviation,...
                PreviousWaypoints] = isOutputComplexImpl(~)
            % Return true for each output port with complex data
            Trajectory = false;
            NumTrajPoints = false;
            isTrajNew = false;
            PreviousDeviation = false;
            PreviousWaypoints = false;
        end
        
        function [Trajectory, NumTrajPoints, ...
                isTrajNew, PreviousDeviation,...
                PreviousWaypoints] = ...
                isOutputFixedSizeImpl(~)
            % Return true for each output port with fixed size
            Trajectory = true;
            NumTrajPoints = true;
            isTrajNew = true;
            PreviousDeviation = true;
            PreviousWaypoints = true;
        end
        
        function icon = getIconImpl(~)
            % Define icon for System block
            icon = mfilename("class");
        end
    end
    
    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
