function egoLanePosition = helperGetEgoLanePosition(detections,egoPose,waypoints,startingLane,numOfLanes)
%This helper computes in which lane geo is present for all its waypoints
%using the starting lane provided as input and by calculating number of
%lane changes

% helper function returns "egoLanePosition" which stores the lane number of the
% lane ego is present for all its waypoints

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

%   Copyright 2021 The MathWorks, Inc.

narginchk(5,5);
nargoutchk(0,1);
validateattributes(egoPose,{'struct'},{'nonempty'});
validateattributes(detections,{'struct'},{'nonempty'});
validateattributes(waypoints,{'numeric'},{'size',[NaN,3],'finite','nonnan','nonempty'});
validateattributes(startingLane,{'numeric'},{'nonempty','nonnan','scalar','finite','nonzero','integer','>=',1});
validateattributes(numOfLanes,{'numeric'},{'nonempty','nonnan','scalar','finite','nonzero','integer','>=',1});

if startingLane > numOfLanes
    error('Starting Lane should be less than number of lanes');
end

left = arrayfun(@(x) x.left,detections)';
right = arrayfun(@(x) x.right,detections)';
%left and right are lane marking detections for left and right lane
%markings
% They consists of coefficients for parabolic models of the form y = Ax2 + Bx + C
% Each entry describes a separate parabolic lane boundary model in ego frame
% They can be used to compute multiple sets of co-ordinates of lane markings 
% for every entry in detections by computing
% different y co-ordinates of lane markings in
%ego frame by using different values of distnace from ego (x co-ordinate in ego
%frame)


% code to track lane changes
% Number of detections
numOfWaypoints = length(waypoints);
laneChange = zeros(numOfWaypoints,1);
% To track lane change for every lane marking detection :
% 1)Since using left and  right detection we can find out different sets of
% co-ordinates by changing the distance from ego (x co-ordinate in ego
% frame), find out the next position [x,y] of lane markings, i.e for ith entry
% in detection compute co-ordinates of lane markings at 0 distance from ego
% and at some offset distance from ego
%2) Lane marking co-ordinates found at 0 distance from ego becomes current
% position of lane markings and the one found at some distance becomes next
% possible position of lane marking if ego stays in the same lane
%
% (current)   (next possible lane marking)
% ____       _____
%   .
%  ____      _____
%
%3) Make a bounding box of current lane marking detection position and next position of lane marking detection
%3) This gives us N-1 polygons for N lane marking detections
%4) If ego's position at ith iteration lies inside the i - 1 polygon (made
%from previous lane marking detection) then ego lies in the same lane
%5) else there is a lane change

% Get co-ordinates of left and right lane markings in ego frame, at
%0 x co-ordinate using left and right detections
xDistanceFromEgo = 0;
[leftLanePoints,rightLanePoints] = drivingUtils.getLanePointsEgoFrame(xDistanceFromEgo,left,right);
% Get co-ordinates of left and right lane markings in ego frame, at some distance from ego
xDistanceFromEgo = max(round(max(vecnorm(diff(waypoints),2,2))),2);

[nextleftLanePoints,nextrightLanePoints] = drivingUtils.getLanePointsEgoFrame(xDistanceFromEgo,left,right);

% Convert to World co-ordinates
leftLanePointsW = drivingUtils.convertToWorldFrame(waypoints,egoPose.yaw,egoPose.pitch,egoPose.roll,leftLanePoints);
rightLanePointsW = drivingUtils.convertToWorldFrame(waypoints,egoPose.yaw,egoPose.pitch,egoPose.roll,rightLanePoints);
nextleftLanePointsW = drivingUtils.convertToWorldFrame(waypoints,egoPose.yaw,egoPose.pitch,egoPose.roll,nextleftLanePoints);
nextrightLanePointsW = drivingUtils.convertToWorldFrame(waypoints,egoPose.yaw,egoPose.pitch,egoPose.roll,nextrightLanePoints);


for i=1:numOfWaypoints-1
    % Bounding Box made from current lane marking detection and
    %next lane marking detction in the same lane
    vertices = [[leftLanePointsW(i,:);nextleftLanePointsW(i,:)]; flip([rightLanePointsW(i,:);nextrightLanePointsW(i,:)])];
    [flag,on] = inpolygon(waypoints(i+1,1),waypoints(i+1,2), vertices(:,1), vertices(:,2));
    minDistLeft = min(vecnorm(([leftLanePointsW(i,:);nextleftLanePointsW(i,:)] - [waypoints(i+1,1),waypoints(i+1,2)]),2,2));
    minDistRight = min(vecnorm(([rightLanePointsW(i,:);nextrightLanePointsW(i,:)] - [waypoints(i+1,1),waypoints(i+1,2)]),2,2));
    distanceLimit = 0.1;
    if minDistRight < distanceLimit || minDistLeft < distanceLimit
                flag = true;
    end
    if ~flag && ~on%lane change
        % In case of lane change find out weather it's towards left or right
        laneCenter = (leftLanePointsW(i,:)+rightLanePointsW(i,:))/2;
        nextLaneCenter= (nextleftLanePointsW(i,:)+nextrightLanePointsW(i,:))/2;
        % Vector in the direction of lane centers
        laneVector = [nextLaneCenter(1,1)-laneCenter(1,1),nextLaneCenter(1,2)-laneCenter(1,2)]...
            ./(norm([nextLaneCenter(1,1)-laneCenter(1,1),nextLaneCenter(1,2)-laneCenter(1,2)]) + eps);
        % Normal vector to "laneVector"
        normalLaneVector = [-1*laneVector(1,2),laneVector(1,1)];
        % Vector in the direction ego is travelling
        travelDirection = [waypoints(i+1,1)-waypoints(i,1),waypoints(i+1,2)-waypoints(i,2)]...
            ./norm([waypoints(i+1,1)-waypoints(i,1),waypoints(i+1,2)-waypoints(i,2)] + eps);
        theta = acosd(dot(normalLaneVector,travelDirection));
        if theta < 90
            laneChange(i) = 2; % Left lane change
        elseif theta > 90
            laneChange(i) = 1; % Right lane change
        end
    end
end

% Compute in which lane ego is present using the starting lane and lane
%changes computed before
egoLanePosition = zeros(numOfWaypoints,1);
egoLanePosition(1) = startingLane;
laneChange = diff(laneChange);%diff array of lane changes array stores 2 at position when left lane change starts 
%and -2 when lane change has been completed, similarly for right lane
%change
for i = 1:length(laneChange)
    if laneChange(i) == -1 % Using the instance when right lane change has been completed
        % To use the instance when right lane change started, use 1
        % Right lane change, hence increment the lane number
        egoLanePosition(i+1) = egoLanePosition(i)+1;
    elseif laneChange(i) == -2% Using the instance when left lane change has been completed
        % To use the instance when right lane change started, use 2
        % Left lane change, hence decrease the lane number
        egoLanePosition(i+1) = egoLanePosition(i)- 1;
    else
        % No lane change
        egoLanePosition(i+1) = egoLanePosition(i);
    end
end

if any(egoLanePosition > numOfLanes) || any(egoLanePosition < 1)
    error("Incorrect Starting Lane");
end
end