classdef HelperEnvironmentUpdater  < matlab.System & matlab.system.mixin.Propagates
%HelperEnvironmentUpdater  Updates the information about environment
%based on the current state of vehicle.
%
%   This system object updates the information about lead vehicle, rear
%   vehicle, vehicle's current lane, and the existence of adjacent lanes
%   based on the current ego state. This system object computes distance to
%   lead vehicle and velocity of the lead vehicle using the actors ground
%   truth information. It calculates the current lane of the vehicle using
%   the lateral deviation from vehicle's initial lane centers. It uses
%   lane direction and lane number to find the existence of adjacent lanes.
% 
%   helperEnvironmentUpdater = HelperEnvironmentUpdater creates a system
%   object, helperEnvironmentUpdater, that estimates the current ego
%   lane, lead vehicle information, read vehicle id and existence of
%   adjacent lanes(NoLeftLane and NoRightLane)
%
%   helperEnvironmentUpdater = helperEnvironmentUpdater(Name,Value)
%   creates a system object, helperEnvironmentUpdater, with additional
%   options specified by one or more Name,Value pair arguments:
%
%   'InitialLaneNumber'         -  Initial lane number
%                                  Default: 1
%
%   'ActorIndex'                -  Actor id of vehicle
%                                  Default: 1
%
%   'SetVelocity'               -  Set velocity of the vehicle
%                                  Default: 0
%   Step method syntax: 
%   [DistanceToLeadVehicle, LeadVehicleVelocity,
%   CurrentVehicleVelocity, VehicleCurrentLane, NoLeftLane, NoRightLane,
%   LeadVehicleID, RearVehicleID] = stepImpl(ActorsInWorld, EgoActor, LaneInfo,
%   CurrentActorInWorld, LateralDeviation) returns distance to lead
%   vehicle, DistanceToLeadVehicle, lead vehicle velocity,
%   LeadVehicleVelocity, velocity of vehicle, CurrentVehicleVelocity,
%   current lane of vehicle, VehicleCurrentLane, flag denotes existence of
%   left lane, NoLeftLane, flag denotes existence of right lane,
%   NoRightLane, actor id of lead vehicle, LeadVehicleID, actor id of rear
%   vehicle, RearVehicleID based on all actor data in scenario in world
%   co-ordinates, ActorsInWorld, ego actor data, EgoActor, lane centers
%   information, LaneInfo, current actor info, CurrentActorInWorld, lateral
%   deviation, LateralDeviation.
%
%   HelperEnvironmentUpdater methods:
%   stepImpl                   - Computes information about environment
%                                around the vehicle such as existence of
%                                adjacent lanes, information about lead and
%                                rear vehicles. 
%   targetsToEgo               - Converts all other vehicles in the
%                                scenario into ego coordinates.
%   scenarioToEgoOrientation   - Converts orientation of the vehicle into
%                                ego orientation 
%   scenarioToBodyRotator      - Converts vehicle coordinates from scenario
%                                to ego's body frame 
%
%   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.
    
    % Public, tunable properties
    properties (Access = public)
        
        %InitialLaneNumber contains initial lane number
        %
        % Default = 1
        InitialLaneNumber = 1;
        
        %ActorIndex contains ActorID
        % Default = 1
        ActorIndex = 1;
        

        
        %SetVelocity contains driver set velocity of the vehicle
        %
        % Default = 0;
        SetVelocity = 0;
    end
   
    % Precomputed constants
    properties(Access = private)                    
        % Variable containing number of iterations for the lane following
        % behavior. This increments on number of times the vehicle is in
        % cruising
        % Default = 0
        Count = 0;
        
        % Variable containing lateral terminal states for planner
        % Default = zeros(1,5)
        LateralSamples = zeros(1,5);
        
        % Variable containing longitudinal terminal states for planner
        % Default = zeros(1,5)
        LongitudinalSamples = zeros(1,5);
        
        % Variable containing velocity terminal states for planner
        % Default = zeros(1,5)
        VelocitySamples = zeros(1,5);
        
        % Variable containing time states for planner. This is a constant
        % value
        % Default = zeros(1,5)
        TimeSamples = zeros(1,5);
        
        % Variable containing acceleration states for planner. This is a
        % constant value
        % Default = zeros(1,5)
        AccelerationSamples = zeros(1,5);
        
        % Variable containing number of lateral samples
        % Default = 0
        NumLatSamples = 0;
        
        % Variable containing number of longitudinal samples
        % Default = 0
        NumLongSamples = 0;
        
        % Variable containing number of velocity samples
        % Default = 0
        NumVeloSamples = 0;
        
        % Variable containing number of acceleration samples
        % Default = 0
        NumAccSamples = 0;
    end
    methods
        function obj = HelperEnvironmentUpdater (varargin)
            % HelperEnvironmentUpdater  constructor.            
            setProperties(obj,nargin,varargin{:})
        end
    end
    methods(Access = protected)
        function setupImpl(obj)                        
            % Initialize Count
            obj.Count = 0;
        end
        
        function [DistanceToLeadVehicle, LeadVehicleVelocity,...
                CurrentVehicleVelocity, VehicleCurrentLane, NoLeftLane,...
                NoRightLane, LeadVehicleID, RearVehicleID] = stepImpl(obj,...
                ActorsInWorld, EgoActor, LaneInfo, LateralDeviation)
            %stepImpl Implements the main logic that defines the
            %terminal states and deviation for the planner
            %   The setupImpl method implements the main logic that defines
            %   terminal states and deviation for the planner based on
            %   vehicle behavior type, lane information, current ego state
            %   and lead vehicle information and Update Ego vehicle states
            %   from the current ego state.
            
            % Update terminal state samples with default values at every
            % simulation step
            obj.LateralSamples      = zeros(1,5);
            obj.LongitudinalSamples = zeros(1,5);
            obj.VelocitySamples     = zeros(1,5);
            obj.TimeSamples         = zeros(1,5);
            obj.AccelerationSamples = zeros(1,5);
            
            % Update number of state samples with default values at every
            % simulation step
            obj.NumLatSamples       = 0;
            obj.NumLongSamples      = 0;
            obj.NumVeloSamples      = 0;
            obj.NumAccSamples       = 0;
            
            % set vehicleCurrentLane with vehicle initial lane number
            vehicleCurrentLane = obj.InitialLaneNumber;
            
            % Compute current vehicle lane number based on lateral
            % deviation from the reference path.
            % Here the assumption is that we are only changing one
            % lane from the reference path
            if (LateralDeviation <= -(LaneInfo(vehicleCurrentLane).LaneWidth/2))
                vehicleCurrentLane = vehicleCurrentLane + 1;
            elseif (LateralDeviation > (LaneInfo(vehicleCurrentLane).LaneWidth/2))
                vehicleCurrentLane = vehicleCurrentLane - 1;
            end
            
            % get width of vehicle current lane
            vehicleCurrentLaneWidth = LaneInfo(vehicleCurrentLane).LaneWidth;
            
            % Calculate left and right lane numbers from the current
            % vehicle lane number
            leftLaneNumber  = vehicleCurrentLane - 1;
            rightLaneNumber = vehicleCurrentLane + 1;
            
            % default value for currentActorInWorld is actor 1
            currentActorInWorld = ActorsInWorld(1);
            
            % Update the currentActorInWorld based on the current actor ID
            for i=1:numel(ActorsInWorld)
                if(ActorsInWorld(i).ActorID == obj.ActorIndex)
                    currentActorInWorld = ActorsInWorld(i);
                    break;
                end
            end
            
            % Add Ego actor to the other actors list
            actorsInWorld = vertcat(ActorsInWorld, EgoActor);
            
            % Initialize variables NoLeftLane and NoRightLane with default
            % values
            NoLeftLane           = false;
            NoRightLane          = false;
            
            % Check if left and right lanes exist based on LaneDirection
            % and lane number
            if leftLaneNumber < 1
                NoLeftLane = true;
            elseif ~isequal(LaneInfo(leftLaneNumber).LaneDirection, LaneInfo(vehicleCurrentLane).LaneDirection)
                NoLeftLane = true;
            end
            if rightLaneNumber > numel(LaneInfo)
                NoRightLane = true;
            elseif ~isequal(LaneInfo(rightLaneNumber).LaneDirection, LaneInfo(vehicleCurrentLane).LaneDirection)
                NoRightLane = true;
            end
            
            % Check for existence of lead vehicle in the same lane
            ActorsInEgo = targetsToEgo(obj, actorsInWorld, currentActorInWorld);
            
            % Update lead vehicle information
            [distanceToLeadVehicle,leadVehicleVelocity, leadVehicleID] = helperFindLeadVehicle(ActorsInEgo,...
                currentActorInWorld,...
                vehicleCurrentLaneWidth);
            
            % Check for the existence of rear vehicle in the same lane
            % and update the rearVehicleID
            rearVehicleID = helperFindRearVehicle(ActorsInEgo,...
                currentActorInWorld, vehicleCurrentLaneWidth);
            
            % Assign output variables
            
            % Vehicle current velocity
            CurrentVehicleVelocity = norm(currentActorInWorld.Velocity);
            
            % Distance to lead vehicle
            DistanceToLeadVehicle = distanceToLeadVehicle;
            
            % Velocity of the lead vehicle
            LeadVehicleVelocity   = leadVehicleVelocity;
            
            % Lead vehicle ID
            LeadVehicleID         = leadVehicleID;
            
            % Rear vehicle ID
            RearVehicleID         = rearVehicleID;
            
            % Current lane number of the vehicle
            VehicleCurrentLane    = vehicleCurrentLane;            
        end
        
        function targetStruct = targetsToEgo(obj, targetActors, egoActor)
            %targetsToEgo converts vehicles into ego coordinate system
            % targetsToEgo function converts the target vehicles
            % information from world coordinates to ego coordinates
            
            % Ego actor information
            egoPos = egoActor.Position;
            egoAngVel = egoActor.AngularVelocity;
            
            % Convert world coordinates to body coordinates
            Rscene2ego = scenarioToBodyRotator(obj, egoActor);
            
            % initialize target structure to same size and field names as
            % targetActors
            targetStruct = targetActors;
            
            % Convert each target information to ego coordinate system
            % except vehicle under test (Ego)
            for i = 1:numel(targetActors)
                targetActor = targetActors(i);
                if ~isequal(egoActor.ActorID, targetActor.ActorID)
                    % translate and rotate to ego coordinates
                    aPos = (targetActor.Position - egoPos)*Rscene2ego;
                    aVel = (targetActor.Velocity);
                    [aRoll, aPitch, aYaw, Rego2tgt] = scenarioToEgoOrientation(obj, targetActor,Rscene2ego);
                    
                    % target's angular velocity vector in the ego's body
                    % frame
                    aAngVel = targetActor.AngularVelocity*Rego2tgt' - egoAngVel;
                    
                    % store into target structure array
                    targetStruct(i).Position = aPos;
                    targetStruct(i).Velocity = aVel;
                    targetStruct(i).Roll = aRoll;
                    targetStruct(i).Pitch = aPitch;
                    targetStruct(i).Yaw = aYaw;
                    targetStruct(i).AngularVelocity = aAngVel;
                end
            end
        end
        
        function [aRoll, aPitch, aYaw, Rego2tgt] = scenarioToEgoOrientation(obj, actor, Rscene2ego)
            % scenarioToEgoOrientation converts orientation of the vehicle
            % into ego orientation
            Rscene2tgt = scenarioToBodyRotator(obj, actor);
            Rego2tgt = Rscene2ego' * Rscene2tgt;
            if Rego2tgt(1,1) == 0 && Rego2tgt(2,1) == 0
                aRoll = atan2d(Rego2tgt(1,2),Rego2tgt(2,2));
            else
                aRoll = atan2d(Rego2tgt(3,2),Rego2tgt(3,3));
            end
            if Rego2tgt(1,1) == 0 && Rego2tgt(2,1) == 0
                aPitch = 90;
            else
                aPitch = atan2d(-Rego2tgt(3,1),sqrt(sum(Rego2tgt(1:2,1).^2)));
            end
            if Rego2tgt(1,1) == 0 && Rego2tgt(2,1) == 0
                aYaw = 0;
            else
                aYaw = atan2d(Rego2tgt(2,1),Rego2tgt(1,1));
            end
        end
        
        function Rscene2body = scenarioToBodyRotator(~, ego)
            % 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)
            
            roll = ego.Roll;
            pitch = ego.Pitch;
            yaw = ego.Yaw;
            Rscene2body = matlabshared.tracking.internal.fusion.rotZ(yaw) ...
                * matlabshared.tracking.internal.fusion.rotY(pitch) ...
                * matlabshared.tracking.internal.fusion.rotX(roll);
            
        end
        
        function [DistanceToLeadVehicle, LeadVehicleVelocity,...
                CurrentVehicleVelocity, VehicleCurrentLane, NoLeftLane,...
                NoRightLane, LeadVehicleID, RearVehicleID] = getOutputSizeImpl(~)
            % Return size for each output port
            NoLeftLane              = 1;
            NoRightLane             = 1;
            DistanceToLeadVehicle   = 1;
            LeadVehicleVelocity     = 1;
            VehicleCurrentLane      = 1;
            RearVehicleID           = 1;
            LeadVehicleID           = 1;
            CurrentVehicleVelocity  = 1;
        end
        
        function [DistanceToLeadVehicle, LeadVehicleVelocity,...
                CurrentVehicleVelocity, VehicleCurrentLane, NoLeftLane,...
                NoRightLane, LeadVehicleID, RearVehicleID] = getOutputDataTypeImpl(~)
            % Return data type for each output port
            NoLeftLane              = "boolean";
            NoRightLane             = "boolean";
            DistanceToLeadVehicle   = "double";
            LeadVehicleVelocity     = "double";
            VehicleCurrentLane      = "double";
            RearVehicleID           = "double";
            LeadVehicleID           = "double";
            CurrentVehicleVelocity  = "double";
        end
        
        function [DistanceToLeadVehicle, LeadVehicleVelocity,...
                CurrentVehicleVelocity, VehicleCurrentLane, NoLeftLane,...
                NoRightLane, LeadVehicleID, RearVehicleID] = isOutputComplexImpl(~)
            % Return true for each output port with complex data
            NoLeftLane              = false;
            NoRightLane             = false;
            DistanceToLeadVehicle   = false;
            LeadVehicleVelocity     = false;
            VehicleCurrentLane      = false;
            RearVehicleID           = false;
            LeadVehicleID           = false;
            CurrentVehicleVelocity  = false;
        end
        
        function [DistanceToLeadVehicle, LeadVehicleVelocity,...
                CurrentVehicleVelocity, VehicleCurrentLane, NoLeftLane,...
                NoRightLane, LeadVehicleID, RearVehicleID] = isOutputFixedSizeImpl(~)
            % Return true for each output port with fixed size
            NoLeftLane              = true;
            NoRightLane             = true;
            DistanceToLeadVehicle   = true;
            LeadVehicleVelocity     = true;
            VehicleCurrentLane      = true;
            RearVehicleID           = true;
            LeadVehicleID           = true;
            CurrentVehicleVelocity  = true;
        end
        function flag = supportsMultipleInstanceImpl(~)
            flag = true;
        end
    end
end