function laneSpecifications = helperCreateLaneSpecification(detections,waypoints,egoPose,roads,numOfLanes,egoLanePosition,varargin)
% This function creates lane specifications from recorded vehicle data

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

%   Copyright 2021 The MathWorks, Inc.

narginchk(6,10);
nargoutchk(0,1);
% Validation checks on inputs
validateattributes(egoPose,{'struct'},{'nonempty'});
validateattributes(detections,{'struct'},{'nonempty'});
validateattributes(waypoints,{'numeric'},{'size',[NaN,3]});
validateattributes(egoLanePosition,{'numeric'},{'nonempty','nonnan','finite','nonzero','integer'});
validateattributes(roads,{'numeric'},{'nonempty','nonnan','finite','nonzero','integer'});
validateattributes(numOfLanes,{'numeric'},{'nonempty','nonnan','scalar','finite','nonzero','integer','>=',1});
% Parse the optional input arguments
parser = inputParser;
parser.addOptional('showAllLanes',true,@(showAllLanes)validateattributes(showAllLanes,{'logical'},...
    {'nonempty','scalar','finite'}));
parser.addOptional('RoadEdges',[laneMarking('Solid'),laneMarking('Solid')], ...
    @(RoadEdges)validateattributes(RoadEdges,{'driving.scenario.LaneMarking'},...
    {'size',[1,2]}));
parser.parse(varargin{:});
results = parser.Results;
roadEdges = results.RoadEdges;
showAllLanes = results.showAllLanes;

% Get lane width of the driving lane
lanewidth = helperGetLaneWidth(detections,waypoints,egoPose);

% Get lane markings from detections
laneMarkings = helperGetLaneMarking(detections,egoLanePosition,numOfLanes,roadEdges,showAllLanes);
roadNumbers = unique(roads(:));
numOfRoads = length(roadNumbers);
laneSpecificationsMap = cell(numOfRoads,3); %This is a map used for creating lane specifications
% First column stores the road numbers
% Second column stores the lane specifications or array of lane specifications
 % Third column stores the range for lane specifications

for i = 1:length(roadNumbers)
    % Store the road number
    laneSpecificationsMap{i,1} = roadNumbers(i);
    % Get lane width of points belonging to this road
    width = lanewidth(roads == roadNumbers(i));
    % Get waypoints belonging to this road
    waypointsInRoad = waypoints(roads == roadNumbers(i),:);
    % Get the lane markings belonging to this road
    laneMarkingsInRoad = laneMarkings(roads == roadNumbers(i),:);
    totalPoints = length(width);
    startIdx = 1;
    threshold = width(startIdx) * 0.2;
    totalDistance = norm(waypointsInRoad(1,:) - waypointsInRoad(end,:));
    while startIdx < totalPoints
        % Idea is to check for scetions in the road which has lane width significantly different from each other
        %and create a lane specification for each such section

        % Difference of lane width from the first lane width value
        widthDiff = abs(width - width(startIdx));
        %index of the lane width which is different from first lane width
        %value more than the allowed threshold

        % Lane specification will be created for points form "startIdx - next -1"

        % Next poosible lane specification will be created for points
        % from next index
        next = find(widthDiff(startIdx:end) > threshold, 1) + (startIdx -1);
        if isempty(next) || ((totalPoints - next)/totalPoints) < 0.01
            % Last segment
            break;
        end
        % Calculate the range
        range = norm(waypointsInRoad(startIdx,:) - waypointsInRoad(next-1,:))/totalDistance;
        if range < 0.01
            % Very small segment
            threshold = threshold * 2; % Increase threshold to include more points
        else
            start = waypointsInRoad(startIdx,:);
            last =  waypointsInRoad(next-1,:);
            % Create lane marking style for this lane specification
            lm = helperCreateLaneMarking(laneMarkingsInRoad(startIdx:next - 1,:),...
                waypointsInRoad(startIdx:next - 1,:),start,last);
            % Store lane specification
            laneSpecificationsMap{i,2} = [laneSpecificationsMap{i,2},lanespec(numOfLanes,'Width',mean(width(startIdx:next-1)),...
                'Marking',lm)];
            % Store range
            laneSpecificationsMap{i,3} = [laneSpecificationsMap{i,3},range];
            % Update start
            startIdx = next;
            threshold = width(startIdx) * 0.2;
        end
    end
    % Last lane specification
    start = waypointsInRoad(startIdx,:);
    last = waypointsInRoad(end,:);
    range = norm(last - start)/totalDistance;
    % Store range
    laneSpecificationsMap{i,3} = [laneSpecificationsMap{i,3},range];
    % Correction if sum of range is not equal to 1
    if sum(laneSpecificationsMap{i,3}) > 1
        sub =( sum(laneSpecificationsMap{i,3}) - 1)/length(laneSpecificationsMap{i,3});
        laneSpecificationsMap{i,3} = laneSpecificationsMap{i,3} - sub;
    elseif sum(laneSpecificationsMap{i,3}) < 1
        sub =( 1- sum(laneSpecificationsMap{i,3}))/length(laneSpecificationsMap{i,3});
        laneSpecificationsMap{i,3} = laneSpecificationsMap{i,3} + sub;
    end
    % Create lane marking style for this lane specification
    lm = helperCreateLaneMarking(laneMarkingsInRoad(startIdx:totalPoints,:),...
        waypointsInRoad(startIdx:totalPoints,:),start,last);
    % Store lane specification
    laneSpecificationsMap{i,2} = [laneSpecificationsMap{i,2},lanespec(numOfLanes,'Width',...
        mean(width(startIdx:totalPoints)),'Marking',lm)];
end

% lanespecifications is a structure which stores 'RoadID' of a road and it's
%corresponding lanespecifications
laneSpecifications = cell(size(laneSpecificationsMap,1),1);
for i = 1:size(laneSpecificationsMap,1)
    if size(laneSpecificationsMap{i,3},2) == 1
        laneSpecifications{i} = struct('RoadID',laneSpecificationsMap{i,1},'specifications',laneSpecificationsMap{i,2});
    else
        % Create composite lanes pecifications
        clspec = compositeLaneSpec(laneSpecificationsMap{i,2},'SegmentRange',[laneSpecificationsMap{i,3}]);
        laneSpecifications{i} = struct('RoadID',laneSpecificationsMap{i,1},'specifications',clspec);
    end
end

warning('off','driving:scenario:PositionNotMatchWithLaneSpec');

end

function laneMarkings = helperCreateLaneMarking(markings,waypoints,start,last)
% This function returns lane marking or composite lane marking(if lane marking changes) for lane
%specification object

% Lane marking styles can change if there is a lane change done by the ego
%vehicle
numOfLaneMarkings = size(markings,2);
laneMarkings = repmat(laneMarking('Unmarked'),1,numOfLaneMarkings);
startIdx = 1;
markingsTemp = repmat(laneMarking('Unmarked'),100,numOfLaneMarkings); %variable to store lane markings
range = zeros(100,1); %vaiable to store range values
found = 0;
totalDistance = norm(start-last);
totalPoints = length(waypoints);
while startIdx < totalPoints
    next = startIdx +1;
    while next < totalPoints
        flag = drivingUtils.compareSingleLaneMarking(markings(startIdx,:),markings(next,:));
        if ~flag
            curRange = norm(waypoints(startIdx,:) - waypoints(next-1,:))/totalDistance;
            if curRange >  0.05
                % Lane marking style has changed
                break;
            end

        end
        next = next +1;
    end
    if next == totalPoints
        if sum(range) == 0
            laneMarkings = markings(end,:);
            return;
        end
        markingsTemp(found+1,:) = markings(end,:);
        range(found+1) = norm(last - waypoints(startIdx,:))/totalDistance;
        found = found +1;
        break;
    end
    markingsTemp(found+1,:) = markings(startIdx,:);
    range(found+1) = norm(waypoints(startIdx,:) - waypoints(next-1,:))/totalDistance;
    found = found +1;
    adjust = range(found) * 0.1;
    % Adjustment in case of change in lane marking styles
    
    if adjust > 0.05
        % In case of change in lane marking styles from lane Marking 1 to
        %lane Marking 2
        % Create a small portion which has both lane marking styles to
        % Create a smooth change in lane marking
        range(found) = range(found) - adjust;
        temp = drivingUtils.merge(markings(next-1,:),markings(next,:));
        markingsTemp(found+1,:) = temp;
        range(found+1) = adjust;
        found = found +1;
    end
    startIdx = next;
end
range(found+1:end) =[];
markingsTemp(found+1:end,:) = [];

% Correction if sum of range is greater than 1
if sum(range) > 1
    sub = (sum(range) - 1)/length(range);
    range = range - sub;
elseif sum(range) < 1
    sub = (1 - sum(range))/length(range);
    range = range + sub;
end

% Create Composite lane marking styles
for k = 1:length(laneMarkings)
    laneMarkings(k) =laneMarking(markingsTemp(:,k)','SegmentRange',range(:));
end
end

function markings = helperGetLaneMarking(detections,egoLanePosition,numOfLanes,roadEdges,showAllLanes)
% This function fetches lane marking from detections
left = arrayfun(@(x) x.left,detections)';
right = arrayfun(@(x) x.right,detections)';
leftMarking =  arrayfun(@(x) x.boundaryType,left);
rightMarking = arrayfun(@(x) x.boundaryType,right);
count = length(leftMarking);
% Create an array of lane marking styles with default value
markings = repmat(laneMarking('Unmarked'),count,numOfLanes+1);
% Update the marking styles for road edges
markings(:,1) = roadEdges(1);
markings(:,end) = roadEdges(2);
for i = 1:count
    % On observation it was found that detections have 0 value in case of
    %lane change or when lane marking styles changes
    if leftMarking(i) == 0 && i > 2
        leftMarking(i) = leftMarking(i-1); %use value of previous lane marking
    end
    if rightMarking(i) == 0 && i > 2
        rightMarking(i) = rightMarking(i-1); %use value of previous lane marking
    end
    % Depending on the lane ego is present update the lane marking styles
    markings(i,egoLanePosition(i)) = laneMarking(drivingUtils.getLaneType(leftMarking(i)));
    if showAllLanes
        markings(i,2:egoLanePosition(i)-1) = markings(i,egoLanePosition(i));
    end
    markings(i,egoLanePosition(i) + 1) = laneMarking(drivingUtils.getLaneType(rightMarking(i)));
    if showAllLanes
        markings(i,egoLanePosition(i)+2:end-1) = markings(i,egoLanePosition(i)+1);
    end
end
end

function width = helperGetLaneWidth(detections,waypoints,egoPose)
% This function calculates lane width
left = arrayfun(@(x) x.left,detections)';
right = arrayfun(@(x) x.right,detections)';
% Get co-ordinates of left and right lane markings in ego frame
[leftLanePoints,rightLanePoints] = drivingUtils.getLanePointsEgoFrame(0,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);
% Lane width is taken as the distance between the left and right lane
%marking
width = vecnorm(leftLanePointsW - rightLanePointsW,2,2);
window = round(length(width) * 0.2);
% Smooth width to avoid sudden change
width = smoothdata(width,'rloess',window);
end