classdef HelperTrajectoryGenerator < matlab.System ...
        & matlab.system.mixin.Propagates ...
        & matlab.system.mixin.CustomIcon
%HelperTrajectoryGenerator generates trajectory using Most Important 
%   Objects(MIOs) information.
%
%   This system object generates trajectory for the ego vehicle at
%   every 1 second 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.
%
%   Step method syntax: [isTrajNew, NumTrajPoints, Trajectory,
%   TrajectoryList, MioPositions, TrajectoryIdx] = step(ReplanFlag,
%   EgoActor,RefPath, MIOs, PlannerBehavior, RefPointOnPath) returns the
%   flag to check whether trajectory is newly generated or not, isTrajNew,
%   number of points in generated trajectory, NumTrajPoints, generated
%   trajectory, Trajectory, array of candidate trajectories,
%   TrajectoryList, detected MIOs position, MioPositions, optimal
%   trajectory index in TrajectoryList, TrajectoryIdx based on replan
%   period flag, ReplanFlag, ego actor information, EgoActor, reference
%   path, RefPath, MIOs, terminal states and deviation, PlannerBehavior and
%   selected reference point on generated trajectory, RefPointOnPath.
%
%   For more information see trajectoryOptimalFrenet and nav.StateValidator
%
%   NOTE: The name of this System Object and it's functionality may 
%   change without notice in a future release, 
%   or the System Object itself may be removed.
%

%   Copyright 2019-2020 The MathWorks, Inc.

    % Private variables
    properties(Access = private)
        %ReferencePathPoints waypoints for the reference path
        %   Waypoints for the reference path (Global Plan). This is read
        %   from the base workspace.
        %   Default: [0, 0; 600, 0]
        %   values scale in meters
        ReferencePathPoints;        
        
        %Planner holds the trajectoryOptimalFrenet object.
        Planner;
        
        %CustomValidator  holds the HelperValidatorBoundingBoxList 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;
        
        %TrajectoryIdx TrajectoryIdx stores index of optimal trajectory
        %among the sampled trajectories.
        TrajectoryIdx;
        
        %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
        
        %CartiationAcceleration CartiationAcceleration stores the
        %acceleration of the vehicle.
        CartiationAcceleration
    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;
        
        %MaxTrajectoryPoints maximium number of points in a trajectory.
        MaxTrajectoryPoints = 1000;
    end
    
    %----------------------------------------------------------------------
    % Constructor
    %----------------------------------------------------------------------
    methods
        function obj = HelperTrajectoryGenerator(varargin)
            % HelperTrajectoryGenerator 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 HelperValidatorBoundingBoxList
            %   Planner is the object of trajectoryOptimalFrenet
            %   Initializes obstacles in the state validator.                           

            % Initialize HelperValidatorBoundingBoxList object
            obj.CustomValidator = HelperValidatorBoundingBoxList(obj.CarPolygon,obj.ReferencePathPoints);
            
            % 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);
            
            % 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.
            % Assuming (-1000, -1000) is out side of 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;
            
            % Initializing the Cartesian acceleration variable
            obj.CartiationAcceleration = 0;
            
            % Initializing the TrajectoryIdx variable
            obj.TrajectoryIdx = 0;
        end
  
        %------------------------------------------------------------------
        function [isTrajNew, NumTrajPoints, Trajectory,...
                  TrajectoryList, MioPositions, TrajectoryIdx,...
                  LateralDeviation] = ...
                  stepImpl(obj, ReplanFlag, EgoActor,RefPath, MIOs,...
                  PlannerBehavior, RefPointOnPath)                
            %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 HelperValidatorBoundingBoxList. 
            %   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.            
            
            % Update reference path if required
            if ~isequal(RefPath, obj.ReferencePathPoints)
                obj.ReferencePathPoints = RefPath;
                obj.Planner.Waypoints = obj.ReferencePathPoints;
                obj.CustomValidator.PlannerObj.Waypoints = obj.ReferencePathPoints;
            end
            % Calculate the time difference between the system time and
            % Previous re-plan time.
            timeDiff = abs(getCurrentTime(obj) - 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;
            EgoFront = MIOs.MIOObjects(1,1);
            EgoRear = MIOs.MIOObjects(2,1);
            LeftFront = MIOs.MIOObjects(3,1);
            LeftRear = MIOs.MIOObjects(4,1);
            RightFront = MIOs.MIOObjects(5,1);
            RightRear = MIOs.MIOObjects(6,1);
            MioPositions = zeros(6,3);
            
            % Rotate Ego actor to world coordinates
            Rscene2ego = scenarioToBodyRotator(obj, EgoActor.Roll, EgoActor.Pitch, EgoActor.Yaw);
            
            % Fill MIOs information for visualization
            % Convert Ego Front to world coordinates
            Rscene2tgt = scenarioToBodyRotator(obj, 0, 0, atan2(EgoFront.Velocity(2),EgoFront.Velocity(1)));
            aYaw = egoToScenarioOrientation(obj, Rscene2ego', Rscene2tgt);
            MioPositions(1, 1:2) = EgoFront.Position;
            MioPositions(1, 3) = aYaw;
            
            % Convert Ego Rear to world coordinates
            Rscene2tgt = scenarioToBodyRotator(obj, 0, 0, atan2(EgoRear.Velocity(2),EgoRear.Velocity(1)));
            aYaw = egoToScenarioOrientation(obj, Rscene2ego', Rscene2tgt);
            MioPositions(2, 1:2) = EgoRear.Position;
            MioPositions(2, 3) = aYaw;
            
            % Convert Left Front to world coordinates
            Rscene2tgt = scenarioToBodyRotator(obj, 0, 0, atan2(LeftFront.Velocity(2),LeftFront.Velocity(1)));
            aYaw = egoToScenarioOrientation(obj, Rscene2ego', Rscene2tgt);
            MioPositions(3, 1:2) = LeftFront.Position;
            MioPositions(3, 3) = aYaw;
            
            % Convert Ego Rear to world coordinates
            Rscene2tgt = scenarioToBodyRotator(obj, 0, 0, atan2(LeftRear.Velocity(2),LeftRear.Velocity(1)));
            aYaw = egoToScenarioOrientation(obj, Rscene2ego', Rscene2tgt);
            MioPositions(4, 1:2) = LeftRear.Position;
            MioPositions(4, 3) = aYaw;
            
            % Convert Right Front to world coordinates
            Rscene2tgt = scenarioToBodyRotator(obj, 0, 0, atan2(RightFront.Velocity(2),RightFront.Velocity(1)));
            aYaw = egoToScenarioOrientation(obj, Rscene2ego', Rscene2tgt);            
            MioPositions(5, 1:2) = RightFront.Position;
            MioPositions(5, 3) = aYaw;
            
            % Convert Right Rear to world coordinates
            Rscene2tgt = scenarioToBodyRotator(obj, 0, 0, atan2(RightRear.Velocity(2),RightRear.Velocity(1)));
            aYaw = egoToScenarioOrientation(obj, Rscene2ego', Rscene2tgt);
            MioPositions(6, 1:2) = RightRear.Position;
            MioPositions(6, 3) = aYaw;
                        
            % Convert Cartesian state of the vehicle to Frenet state
            CartStates = UpdateCartStates(obj,EgoActor,RefPointOnPath);
            frenetStates = obj.Planner.cart2frenet(CartStates(1:6));
            
            % If the difference is more than 1s(one sec) re-plan the trajectory.
            if(ReplanFlag || timeDiff >= 1 || getCurrentTime(obj) == 0)

                % preserve the system time for next iteration.
                obj.PreviousTime = getCurrentTime(obj);               
                
                % 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)));
                
                % Update Planner Behavior using the data from the behavior
                % layer
                % 
                % Update Lateral States
                obj.Planner.TerminalStates.Lateral = PlannerBehavior.TerminalStatesSamples.Lateral.Samples(1,1:PlannerBehavior.TerminalStatesSamples.Lateral.NumSamples);
                
                % Update Longitudinal State
                obj.Planner.TerminalStates.Longitudinal = PlannerBehavior.TerminalStatesSamples.Longitudinal.Samples(1,1:PlannerBehavior.TerminalStatesSamples.Longitudinal.NumSamples);
                
                % Update Speed at the terminal state 
                velocitySamples = PlannerBehavior.TerminalStatesSamples.Speed.Samples(1,1:PlannerBehavior.TerminalStatesSamples.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));
                obj.Planner.TerminalStates.Time = desiredTime;
                
                % Update TimeResolution of planner
                obj.Planner.TimeResolution = PlannerBehavior.TimeResolution;
                
                % Set the deviation required for the planner
                obj.Planner.DeviationOffset = PlannerBehavior.DeviationOffset;               

                % Call plan function by passing current ego state in frenet
                [obj.Trajectory, obj.TrajectoryIdx] = obj.Planner.plan(frenetStates);
                
                if isempty(obj.Trajectory)
                    disp('There is no trajectory from planner');
                end
                
                % Set isTrajNew to indicate the new trajectory.
                isTrajNew = true;                
            end
            

            % Copy the trajectory points to the Output
            % Create a fixed size variables for output            
            Trajectory = nan(obj.MaxTrajectoryPoints,7);
            
            % Assigning feasible trajectory index to output variable
            TrajectoryIdx = obj.TrajectoryIdx;
            
            % Finding out generated number of trajectories
            numTrajectories = length(obj.Planner.TrajectoryList);
            
            % Initializing trajectory list variable
            % The trajectory list contains all trajectories. In this
            % example we will be having maximum of three trajectories.
            TrajectoryList = nan(3*obj.MaxTrajectoryPoints,7);
            
            % Get number of points in the trajectory
            NumTrajPoints = size(obj.Trajectory,1);
            
            % Update lateral deviation
            LateralDeviation = frenetStates(4);
            
            % Copy the generated trajectory to the output variables
            Trajectory(1:NumTrajPoints,:) = obj.Trajectory(1:NumTrajPoints,:);
            for i = 1:numTrajectories
                sz = size(obj.Planner.TrajectoryList(i).Trajectory, 1);
                idx = (i-1)*obj.MaxTrajectoryPoints + 1;
                TrajectoryList(idx:idx+sz-1, :) = obj.Planner.TrajectoryList(i).Trajectory(1:sz, :);
            end          
            
            % Update current acceleration
            UpdateCurrCurvAcc(obj,NumTrajPoints,EgoActor);
        end
        
        %------------------------------------------------------------------
        function aYaw = egoToScenarioOrientation(obj, Rego2scene, Rscene2tgt)
            % egoToScenarioOrientation function computes the yaw of actor
            % in world coordinates
            Rego2tgt = Rego2scene' * Rscene2tgt;
            if Rego2tgt(1,1) == 0 && Rego2tgt(2,1) == 0
                aYaw = 0;
            else
                aYaw = atan2(Rego2tgt(2,1),Rego2tgt(1,1));
            end
        end
        
        %------------------------------------------------------------------
        function Rscene2body = scenarioToBodyRotator(obj, roll, pitch, yaw)
            % Rotation matrix to transform from scenario to ego's body frame:
            %   xBody = (xScene - T) * Rscene2body
            %
            % To transform from the body frame to the scenario frame:
            %   xScene = xBody * Rscene2body' + T
            %          = xBody * Rbody2scene  + T
            %
            % Notes:
            %       Rotation matrices for post-multiplication of row vectors
            %       Rbody2scene = Rscene2body'
            %       Rbody2scene*Rscene2body = eye(3)
            
            Rscene2body = [cosd(yaw) -sind(yaw) 0; sind(yaw) cosd(yaw) 0; 0 0 1]...
                    * [cosd(pitch) -sind(pitch) 0; sind(pitch) cosd(pitch) 0; 0 0 1]...
                    * [cosd(roll) -sind(roll) 0; sind(roll) cosd(roll) 0; 0 0 1];
        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
                % coordinate lies out side the drivable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_1',[NaN NaN NaN],[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
                % coordinate lies out side the drivable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_2',[NaN NaN NaN],[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
                % coordinate lies out side the drivable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_3',[NaN NaN NaN],[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
                % coordinate lies out side the drivable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_4',[NaN NaN NaN],[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
                % coordinate lies out side the drivable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_5',[NaN NaN NaN],[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
                % coordinate lies out side the drivable area of the
                % scenario.
                obj.CustomValidator.updateObstacle('obstacle_6',[NaN NaN NaN],[0 0],obj.CarPolygon,'ego');
            end
        end
        
        %------------------------------------------------------------------
        function UpdateCurrCurvAcc(obj,numTrajPoints,EgoActorPoses)
            %UpdateCurrCurvAcc This updates acceleration from the trajectory.
            Dist2Traj = vecnorm(obj.Trajectory(:,1:2) - repmat(EgoActorPoses.Position(1:2),numTrajPoints,1),2,2);
            [~,MinIdxTraj] = min(Dist2Traj,[],1);
            obj.CartiationAcceleration = obj.Trajectory(MinIdxTraj,6);
        end
        
        %------------------------------------------------------------------
        function CartStates = UpdateCartStates(obj,EgoActor,...
                              RefPointOnPath)
            %UpdateCartStates This updates ego current state from the
            %   trajectory and EgoActor bus object
            
            % Calculate curvature from angular velocity of the ego vehicle
            kapa = deg2rad(EgoActor.AngularVelocity(3))/norm(EgoActor.Velocity);
            if (isnan(kapa) || isinf(kapa))
                kapa = 0;
            end
            % Update waypoints
            if getCurrentTime(obj)>0 % after first planning cycle trajectory does exist
                Cart_Acc  = obj.CartiationAcceleration;
                
                % Get the Cartesian states from RefPointOnPath               
                CartStates = [RefPointOnPath.RefPoseRr(1:2) RefPointOnPath.RefPoseRr(3)/180*pi RefPointOnPath.RefCurvature RefPointOnPath.RefVelocity Cart_Acc];
                
            else % before first planning cycle trajectory does not exist
                RefWaypoints = obj.ReferencePathPoints';
                SizeWaypints = size(RefWaypoints);
                Dist2Ref = vecnorm(RefWaypoints - repmat(EgoActor.Position(1:2)',1,SizeWaypints(2)),2,1);
                [~,MinIdx] = min(Dist2Ref,[],2);

                Cart_Acc  = 0;
                % Initial Cartesian states from the EgoActor.
                CartStates = [EgoActor.Position(1:2) EgoActor.Yaw/180*pi kapa (cosd(EgoActor.Yaw)*EgoActor.Velocity(1) + sind(EgoActor.Yaw)*EgoActor.Velocity(2)) 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 [isTrajNew, numTrajPoints, trajectory,...                  
                  trajectoryList, mioPositions, previousDeviation,...
                  trajectoryIdx, lateralDeviation] = getOutputSizeImpl(~)
            % Return size for each output port
            trajectory = [1000 7];
            trajectoryList = [3000 7];
            numTrajPoints = 1;
            isTrajNew = 1;
            previousDeviation = 1;
            mioPositions = [6 3];
            trajectoryIdx = 1;
            lateralDeviation = 1;
        end
        
        %------------------------------------------------------------------
        function [isTrajNew, numTrajPoints, trajectory,...
                  trajectoryList, mioPositions, previousDeviation,...
                  trajectoryIdx, lateralDeviation] = getOutputDataTypeImpl(~)
            % Return data type for each output port
            trajectory        = "double";
            trajectoryList    = "double";
            numTrajPoints     = "double";
            isTrajNew         = "logical";
            previousDeviation = "double";
            mioPositions      = "double";
            trajectoryIdx     = "double";
            lateralDeviation  = "double";
        end
        
        %------------------------------------------------------------------
        function [isTrajNew, numTrajPoints, trajectory,...
                  trajectoryList, mioPositions, previousDeviation,...
                  trajectoryIdx, lateralDeviation] = isOutputComplexImpl(~)
            % Return true for each output port with complex data
            trajectory        = false;
            trajectoryList    = false;
            numTrajPoints     = false;
            isTrajNew         = false;
            previousDeviation = false;
            mioPositions      = false;
            trajectoryIdx     = false;
            lateralDeviation  = false;
        end
        
        %------------------------------------------------------------------
        function [isTrajNew, numTrajPoints, trajectory,...
                  trajectoryList, mioPositions, previousDeviation,...
                  trajectoryIdx, lateralDeviation] = isOutputFixedSizeImpl(~)
            % Return true for each output port with fixed size
            trajectory        = true;
            trajectoryList    = true;
            numTrajPoints     = true;
            isTrajNew         = true;
            previousDeviation = true;
            mioPositions      = true;
            trajectoryIdx     = true;
            lateralDeviation  = true;
        end
        
        %------------------------------------------------------------------
        function icon = getIconImpl(~)
            % Define icon for System block
            icon = mfilename("class");
        end
    end
    
    %----------------------------------------------------------------------
    % Simulink dialog
    %----------------------------------------------------------------------
    methods(Static, Access = protected)
        %------------------------------------------------------------------
        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
