classdef drivingUtils
    % DRIVINGUTILS class contains functions related to basic
    %operations related to driving scenario

    %   This is a helper script for example purposes and may be modified in the
    %   future

    %   Copyright 2021 The MathWorks, Inc.

    methods(Static)
        function [yaw,pitch,roll] = getEgoPose(scenario,waypoints)
            % This function returns ego pose - "Yaw, Pitch, Roll"
            %for all waypoints
            count = length(waypoints);
            yaw = zeros(count,1);
            roll = zeros(count,1);
            pitch = zeros(count,1);
            try
                restart(scenario);
                data = arrayfun(@(x) x.ActorPoses,record(scenario));
            catch
                error("Unable to compute ego pose")
            end
            % Get ego positions
            positions = arrayfun(@(x) x.Position,data,'UniformOutput',false);
            positions = reshape([positions{:}],3,[])';
            Yaw  = arrayfun(@(x) x.Yaw,data)';
            Roll = arrayfun(@(x) x.Roll,data)';
            Pitch = arrayfun(@(x) x.Pitch,data)';
            for i =1:count
                % Find the closest point to current waypoint in positions
                %array
                [~,idx] = min(vecnorm(positions-waypoints(i,:),2,2));
                % Store pose information of that point
                yaw(i) = Yaw(idx);
                roll(i) = Roll(idx);
                pitch(i) = Pitch(idx);
            end
        end

        function markingStyle = getLaneType(boundaryType)
            % This helper function return marking styles

            %After observing the camera Images and detections
            % 2 is mapped to 'Solid' Lane Markings
            % 3 is mapped to 'Dashed' Lane Markings
            switch boundaryType
                case 2  % solid
                    markingStyle = 'Solid';
                case 3  % dashed
                    markingStyle = 'Dashed';
                case 4
                    markingStyle = 'BottsDots';
                case 5
                    markingStyle = 'DoubleSolid';
                case 6
                    markingStyle = 'DoubleDashed';
                case 7
                    markingStyle = 'SolidDashed';
                case 8
                    markingStyle = ' DashedSolid';
                otherwise %default
                    markingStyle = 'Dashed';
            end
        end

        function LineStyle = getLaneTypeBEV(boundaryType)
            % This function returns lins styles for Birds eye view plot of
            %detections
            switch boundaryType
                case 2  % solid
                    LineStyle = '-';
                case 3  % dashed
                    LineStyle = '--';
                case 5  % botts dots
                    LineStyle = ':';
                case 6  % road edge
                    LineStyle = '-';
                case 8  % double line
                    LineStyle = '-';
                otherwise
                    LineStyle = '-.';
            end

        end

        function flag = compareSingleLaneMarking(laneMarkinga, laneMarkingb)
            % This function compares lane markings array
            %if they have same lane marking styles for all, returns true
            %else returns false
            if length(laneMarkinga) ~= length(laneMarkingb)
                flag = false;
                return;
            end
            flag = true;
            for i = 1:length(laneMarkinga)
                if laneMarkinga(i).Type ~= laneMarkingb(i).Type
                    flag = false;
                    return;
                end
            end
        end

        function laneMarkingMerged = merge(laneMarkinga, laneMarkingb)
            % This function merges two lane markings array by using Lane
            %Marking styles in "laneMarkingb" where laneMarkinga has none
            if length(laneMarkinga) ~= length(laneMarkingb)
                error('Cannot merge Lane Markings')
            end

            laneMarkingMerged = laneMarkinga;
            for i = 1:length(laneMarkinga)
                if laneMarkinga(i).Type == "Unmarked" && laneMarkingb(i).Type ~= "Unmarked"
                    laneMarkingMerged(i) = laneMarkingb(i);
                end
            end

        end

        function [leftLanePoints,rightLanePoints] = getLanePointsEgoFrame(distanceFromEgo,leftLane,rightLane)
            % This helper computes co-ordinates of lane markings in ego frame

            % Inputs :-
            % distanceFromEgo = x-axis distance from ego in ego frame
            % leftLane =  detections of left lane markings
            % rightLane = detections of right lane markings
            leftLanePoints = zeros(length(leftLane),2);
            rightLanePoints = zeros(length(rightLane),2);
            for i = 1:size(leftLane,1)
                if leftLane(i).isValid == 1 && ~(leftLane(i).headingAngle == -1e9 || leftLane(i).curvature == -1e9)
                    llane = parabolicLaneBoundary(cast([leftLane(i).curvature, leftLane(i).headingAngle,leftLane(i).offset], 'single'));
                    x = distanceFromEgo;
                    leftLanePoints(i,1) = x;
                    y = llane.computeBoundaryModel(x);
                    leftLanePoints(i,2) = y;
                end
            end
            for i = 1:size(rightLane,1)
                if rightLane(i).isValid == 1 && ~(rightLane(i).headingAngle == -1e9 || rightLane(i).curvature == -1e9)
                    rlane = parabolicLaneBoundary(cast([rightLane(i).curvature, rightLane(i).headingAngle, rightLane(i).offset], 'single'));
                    x = distanceFromEgo;
                    rightLanePoints(i,1) = x;
                    y = rlane.computeBoundaryModel(x);
                    rightLanePoints(i,2) = y;
                end
            end
        end

        function pointsInWorld = convertToWorldFrame(waypoints,yaw,pitch,roll,points)
            % This function converts points from ego frame to World
            %frame using ego pose - "Yaw,Pitch,Roll"
            count = size(points,1);
            pointsInWorld = zeros(size(points));
            for i= 1:count
                Rz = drivingUtils.rotationZ(yaw(i));
                Ry = drivingUtils.rotationY(pitch(i));
                Rx = drivingUtils.rotationX(roll(i));
                R = Rz*Ry*Rx;
                R = R';
                newPoint = [points(i,:), 0]*R + [waypoints(i,1),waypoints(i,2),0];
                pointsInWorld(i,:) = newPoint(:,1:2);

            end
        end

        function R = rotationX(angle)
            % This function returns rotation matrix with x-axis
            angle = deg2rad(angle);
            R = [...
                1   0        0;
                0   cos(angle)  -sin(angle);
                0   sin(angle)   cos(angle)];

        end

        function R = rotationY(angle)
            % This function returns rotation matrix with y-axis
            angle = deg2rad(angle);
            R = [...
                cos(angle)  0 sin(angle);
                0       1 0;
                -sin(angle) 0 cos(angle)];
        end

        function R = rotationZ(angle)
            % This function returns rotation matrix with z-axis
            angle = deg2rad(angle);
            R = [...
                cos(angle) -sin(angle) 0;
                sin(angle)  cos(angle) 0;
                0       0      1];
        end
    end
end