function [roads,roadData] = helperGetRoadsInEgoPath(scenario,waypoints)
%This helper function performs map matching step for ego waypoints

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

%   Copyright 2021 The MathWorks, Inc.

% Checks for inputs
narginchk(2,2);
nargoutchk(0,2);
validateattributes(scenario, {'drivingScenario'}, {'nonempty'});
validateattributes(waypoints,{'numeric'},{'size',[NaN,3],'finite','nonnan','nonempty'});

% Fetch relevant road related data like road centers, road left and right
%boundary and etc.
roadData = helperGetRoadSegmentsAttributes(scenario);

count = size(waypoints,1);
intersectingRoads = cell(count,1); %This is a map that stores for every waypoint the
% intersecting road found, in the form RoadID
distanceLimit = 1.5;% Threshold for checking if any point belongs to a road
%on increasing its value, it can allow detection of irrelevant roads
%on decreasing its value, we may not be able to detect few roads

boundingBoxes =  arrayfun(@(x) [x.LeftBoundary; flip(x.RightBoundary)], ...
    roadData,'UniformOutput',false);% Compute the Bounding box for all road
% objects using its Left and right Boundary
for i=1:count
    intersection = zeros(100,1);% Variable for temporary stores of road/roads found for ith waypoint
    roadFound = 0;
    for j = 1:length(roadData)
        vertices = boundingBoxes{j};
        [flag,on] = inpolygon(waypoints(i,1),waypoints(i,2), vertices(:,1), vertices(:,2));
        if ~flag && ~on
            % If waypoint doesn't lies inside or on the boundary of the
            %road, check if it's at a minimum distance
            minDistLeft = min(vecnorm((roadData(j).LeftBoundary(:,1:2) - [waypoints(i,1),waypoints(i,2)]),2,2));
            minDistRight = min(vecnorm((roadData(j).RightBoundary(:,1:2) - [waypoints(i,1),waypoints(i,2)]),2,2));
            if minDistRight < distanceLimit || minDistLeft < distanceLimit
                flag = true;
            end
        end
        if (flag || on)
            intersection(roadFound+1) = roadData(j).ID;
            roadFound = roadFound+1;
        end
    end

    intersectingRoads{i} = intersection(1:roadFound);
end

% Check the count of waypoints with no roads detected
idx = cellfun(@(x) isempty(x),intersectingRoads);
failedDetectionsRate = length(idx(idx))/length(intersectingRoads);
% Error out if not able to detect roads for greater number of waypoints
limit = 0.3;
if failedDetectionsRate > limit
    error('Unable to detect roads');
end

% Create a sequence of roads in which ego is travelling, without junctions
roads = zeros(count,1);
prev = -1;
startIdx = 1;
i = 1;
while i <=  length(intersectingRoads)
    if length(intersectingRoads{i}) == 1
        % Only one road
        if prev == -1
            prev = intersectingRoads{i};
        elseif prev ~= intersectingRoads{i}
            % New road
            roads(startIdx:i-1) = prev;
            prev = intersectingRoads{i};
            startIdx = i;
        end
    elseif length(intersectingRoads{i}) > 1
        if prev ~= -1 && ~any(ismember(intersectingRoads{i},prev))
            roads(startIdx:i-1) = prev;
            %previous road not part of junction, so find next road
            startIdx = i;
            j = i+1;
            while j <= length(intersectingRoads) && length(intersectingRoads{j}) > 1
                j = j+1;
            end
            %if j is greater than length of intersectingRoads, ie not able
            %to find any road
            if j > length(intersectingRoads)
                break;
            end
            i = j;
            prev = intersectingRoads{j};
        end
    end
    i = i+1;
end
if prev == -1
    error("Unable to detect roads");
end
roads(startIdx:end) = prev;
if any(roads == 0)
    error("Unable to detect roads");
end

% Delete roads not present in ego trajectory
roadNumbers = unique(roads(:));
idx = arrayfun(@(x) ~ismember(x.ID,roadNumbers),roadData);
roadData(idx) = [];

% Trim road centers to match total road length with the length of ego
%trajectory

% Trim road cenetrs of first road according to first waypoint
centers = roadData(1).RoadCenters;
i = 1;
toDelete = 1;
while i <= size(centers,1)
    [flag,on] = inpolygon(centers(i,1),centers(i,2),[min(waypoints(1,1),waypoints(end,1)),max(waypoints(1,1),waypoints(end,1))], ...
        [min(waypoints(1,2),waypoints(end,2)),max(waypoints(1,2),waypoints(end,2))]);
    if flag || on
        toDelete = i;
        break;
    end
    i = i+1;
end
% Delete road cenetrs which are not in the limits of waypoints
centers(1:toDelete-1,:) =[];

% Add first waypoint as first road center to match the starting point of
%road centers and waypoints
offset = 1 *(centers(1,2) - centers(2,2))/abs(centers(1,2) - centers(2,2)); % Offset added to y co-ordinate of first road center
% to take care of ego dimension in y axis and place it properly on the road
centers = [[waypoints(1,1),waypoints(1,2)+offset,centers(1,3)];centers];
roadData(1).RoadCenters = centers;

% Trim road cenetrs of last road according to last waypoint
centers = roadData(end).RoadCenters;
i = length(centers);
toDelete = length(centers);
while i > 1
    [flag,on]=  inpolygon(centers(i,1),centers(i,2),[min(waypoints(1,1),waypoints(end,1)),max(waypoints(1,1),waypoints(end,1))], ...
        [min(waypoints(1,2),waypoints(end,2)),max(waypoints(1,2),waypoints(end,2))]);
    if flag || on
        toDelete = i;
        break;
    end
    i = i-1;
end
% Delete road cenetrs which are not in the limits of waypoints
centers(toDelete+1:end,:) =[];
% add last waypoint as last road center to match the ending of
%road centers and waypoints
offset = 1*(centers(end,2) - centers(end-1,2))/abs(centers(end,2) - centers(end-1,2));% Offset added to y co-ordinate of last road center
% to take care of ego dimension in y axis and place it properly on the road
centers = [centers;[waypoints(end,1),waypoints(end,2)+offset,centers(end,3)]];
roadData(end).RoadCenters = centers;
totalRoadLength = 0;
for i = 1:length(roadData)
    roadCenters= roadData(i).RoadCenters;
    totalRoadLength = totalRoadLength + norm(roadCenters(1,:)-roadCenters(end,:));
end
egoTrajectoryLength = norm(waypoints(1,:) - waypoints(end,:));
if abs(totalRoadLength - egoTrajectoryLength) > 5
    error("Unable to match Roads total length with trajectory length");
end
end