classdef drivingScenarioSensorSimImpl < matlabshared.scenario.SensorSimulation.SensorSim

    methods
        function obj = drivingScenarioSensorSimImpl(uniqueID, isSSE)
            %DRIVINGSCENARIOSENSORSIMIMPL Construct an instance of this class
            obj@matlabshared.scenario.SensorSimulation.SensorSim(uniqueID, isSSE);
        end

    end
    
    methods(Access=protected)
        function [allActorProfiles, allActorPoses] = addActorsImpl(obj, actorProfiles, varargin)
            % Get actor pose from scenario
            
            minArgs=3;
            maxArgs=3;
            narginchk(minArgs,maxArgs)
            
            ssfPosesFromDSD = varargin{1};
       
            % Generate actorProfiles for ssf
            numActors = length(actorProfiles);
            allActorProfiles(1,numActors) = mw.scenario.proto.ActorProfile;
            allActorPoses(1,numActors) = mw.scenario.proto.ActorPose;
            for actorIdx = 1:numActors
                profile = actorProfiles(actorIdx);
                pose = ssfPosesFromDSD(actorIdx);
                % IDs
                actorID = profile.ActorID;
                classID = profile.ClassID;
                % Actor position (With Shift)
                actorPos = pose.Position;
                % Orientation
                actorRPY = deg2rad([pose.Roll, pose.Pitch, pose.Yaw]);
                % Velocity
                actorVel = pose.Velocity;
                % Angular Velocity
                actorAngVel = deg2rad(pose.AngularVelocity);
                % Dimension
                actorLWH = [profile.Length, profile.Width, profile.Height];
                % OriginOffset
                originOffset = profile.OriginOffset;

                % Get actorProfile 
                allActorProfiles(actorIdx) = matlabshared.scenario.internal.utils.getActorProfile(actorID,classID,actorLWH,...
                    actorPos,actorRPY,originOffset,profile);
                
                % If mesh is empty assign a default mesh
                driving.internal.drivingScenarioSensorSimImpl.assignMeshIfEmpty(allActorProfiles(actorIdx));

                % Get actorPose
                allActorPoses(actorIdx) = matlabshared.scenario.internal.utils.getActorPose(actorID,actorPos,actorRPY,actorVel,actorAngVel);
            end
       
        end

        function sensorConfigArray = addSensorsImpl(~, sensorObjs, varargin)

            minArgs=3;
            maxArgs=3;
            narginchk(minArgs,maxArgs)
            
            hostID = varargin{1};

            sensorConfigArray(length(sensorObjs),1) = mw.scenario.proto.SensorConfiguration;

            for idx = 1:length(sensorObjs)
                sensorConfigArray(idx) = sensorObjs{idx}.config(hostID);
            end
        end

        function allActorPoses = updateActorsImpl(~, actorPoses)
            % Generate actorProfiles for ssf
            numActors = length(actorPoses);
            allActorPoses(numActors) = matlabshared.scenario.internal.utils.initializeActorPoseStruct();
            for actorIdx = 1:numActors
                pose = actorPoses(actorIdx);
                allActorPoses(actorIdx).ActorID = uint64([pose.ActorID]);
                allActorPoses(actorIdx).Position = pose.Position;
                allActorPoses(actorIdx).Orientation = deg2rad([pose.Roll, pose.Pitch, pose.Yaw]);
                allActorPoses(actorIdx).Velocity = pose.Velocity;
                allActorPoses(actorIdx).AngularVelocity = deg2rad(pose.AngularVelocity);
            end
        end

    end

    methods(Static, Access=private)
        function assignMeshIfEmpty(actorProfile)
            if isempty(actorProfile.mesh_model.vertices) && isempty(actorProfile.mesh_model.triangles)
                % Assign a default mesh based on Length, Width and Height
                mesh = extendedObjectMesh('cuboid');
                dimensions = [actorProfile.bounding_box_dim.length, ...
                    actorProfile.bounding_box_dim.width, ...
                    actorProfile.bounding_box_dim.height];
                originOffset = [actorProfile.origin_offset.x,...
                    actorProfile.origin_offset.y,...
                    actorProfile.origin_offset.z];
                dims = struct('Length',dimensions(1),...
                    'Width',dimensions(2),'Height',dimensions(3),...
                    'OriginOffset',originOffset);
                mesh = scaleToFit(mesh, dims);

                meshStruct = struct('vertices', mesh.Vertices, ...
                    'triangles', mesh.Faces);

                actorProfile.mesh_model = ...
                    matlabshared.scenario.internal.utils.getProto...
                    (meshStruct,'mesh_model');
            end
        end
    end
end




