classdef mathUtils
    %MATHUTILS this class contains the basic math functions

    %   Copyright 2021 The MathWorks, Inc.

    methods(Static)
        function line = getLineEquation(point1, point2)
            % GETLINEEQUATION Finding the equation of line given two points
            % in the format
            % Line: Ax + By + C = 0;
            %
            % Inputs:
            %  point1: first point of the line.
            %  point2: second point of the line.
            %
            % Outputs:
            %  line: line equation in the format Ax + By + C = 0.
            A = point1(2) - point2(2);
            B = point2(1) - point1(1);
            C = point1(1)*point2(2) - point2(1)*point1(2);
            line = [A  B C];
        end

        function point = getIntersectionPointLL(line1, line2)
            % GETINTERSECTIONPOINT Finding the intersecting  of line given
            % a two lines in the format
            % Line: Ax + By + C = 0;
            %
            % Inputs:
            %  line1: equation of the first line,
            %  line2: equation of the second line,
            %
            % Outputs:
            %  point: intersecting point , nan in case parallel
            if (abs(abs(line1(2)/line1(1)) - abs(line2(2)/line2(1))) < 1e-07 ...
                    || (abs(line1(1)) <= 1e-07 && abs(line2(1)) <= 1e-07))
                point = [nan nan];
            else
                y = (line1(1)*line2(3) - line2(1)*line1(3))/...
                (line2(1)*line1(2) - line1(1)*line2(2));
                x = (line1(2)*line2(3) - line2(2)*line1(3))/...
                (line1(1)*line2(2) - line1(2)*line2(1));
                point = [x y];
            end

        end

        function line = getShiftedLineEquation(lineIn, dist)
            % GETSHIFTEDLINEEQUATION Finding the equation of line given two
            % points in the format
            % Line: Ax + By + C = 0;
            %
            % Inputs:
            %  lineIn: input line to shift
            %  dist: dist to shift
            %
            % Outputs:
            %  line: shifted line equation in the format Ax + By + C = 0.
            A = lineIn(1);
            B = lineIn(2);
            C = lineIn(3);

            C = C + dist * sqrt(A*A + B*B);
            line = [A  B C];
        end

        function shiftedPath = shiftPoints(pathIn, shiftDist, shiftDir)
            %SHIFTPOINTS this function shifts the points in specific
            %direction
            %
            % Inputs:
            %   pathIn: input waypoint of the path,
            %   shiftDist: the distance to shift,
            %   shiftDir: direction in which to shift.
            %
            % Outputs:
            %   shiftedPath: the shifted path

            % The basic idea is to calculate lines connecting the centerpoints
            % of the road and then shift the path given width and direction.

            % Intialize the value of the previous point and adjust the position
            % of the first center. This done by first computing the angle of
            % the lines connecting the two points. Once the angle is obtained,
            % it is used to adjust the value of the coordinate acoordingly so
            % as to keep the point parallel to the lane.

            % get length and adjust the shift distance accordingly.
            sizeR = size(pathIn,1);
            if(size(shiftDist,1) == 1)
                shiftDist = shiftDist.*shiftDir;
                shiftDist = shiftDist.*ones(size(pathIn,1),1);
            else
                shiftDist = shiftDist.*shiftDir;
            end
            pointDiff = pathIn(2,1:2) - pathIn(1,1:2);
            sSlope = atan2(pointDiff(2),pointDiff(1));
            pointDiff = pathIn(end,:) - pathIn(end-1,:);
            eSlope = atan2(pointDiff(2),pointDiff(1));

            ndPathX = zeros(sizeR,1);
            ndPathY = zeros(sizeR,1);
            ndPathZ = zeros(sizeR,1);

            % Adjusting the value for the first point.
            ndPathX(1) = pathIn(1,1) + shiftDist(1) * sin(sSlope);
            ndPathY(1) = pathIn(1,2) - shiftDist(1) * cos(sSlope);
            ndPathZ(1) = pathIn(1,3);
            cLine = mathUtils.getLineEquation(pathIn(1,:),pathIn(2,:));
            cLine = mathUtils.getShiftedLineEquation(cLine,shiftDist(1));
            for idx = 2:sizeR-1
                % get line equation
                fLine = mathUtils.getLineEquation(pathIn(idx,1:2),pathIn(idx+1,1:2));
                fLine = mathUtils.getShiftedLineEquation(fLine,shiftDist(idx));
                point = mathUtils.getIntersectionPointLL(cLine,fLine);
                % check if the lines are parallel then shift the point
                % as per angle value otherwise use the intersection of
                % the shifted line.
                if(isnan(point(1)))
                    pointDiff = pathIn(idx+1,1:2) - pathIn(idx,1:2);
                    slope = atan2(pointDiff(2),pointDiff(1));
                    ndPathX(idx) = pathIn(idx,1) + shiftDist(end) * sin(slope);
                    ndPathY(idx) = pathIn(idx,2) - shiftDist(end) * cos(slope);
                else
                    ndPathX(idx) = point(1);
                    ndPathY(idx) = point(2);
                end
                ndPathZ(idx) = pathIn(idx,3);
                cLine = fLine;
            end
            % this condition works only for last point
            % Shift the point based on the angle.
            ndPathX(end) = pathIn(end,1) + shiftDist(end) * sin(eSlope);
            ndPathY(end) = pathIn(end,2) - shiftDist(end) * cos(eSlope);
            ndPathZ(end) = pathIn(end,3);

            % Return the shifted trajectory
            shiftedPath = [ndPathX ndPathY ndPathZ];
        end
    end
end
