classdef RoadRunnerHDMapBuilder < handle
    %ROADRUNNERHDMAPBUILDER Build RoadRunner HD Map from driving scenarios

    %   Copyright 2022-2023 The MathWorks, Inc.

    methods
        function rrMap = buildMap(~,scenario,exportActors)
            %buildMap Build a RoadRunner HD Map for the given scenario
            % Create an empty map
            if nargin < 3
                exportActors = true;
            end
            rrMap = roadrunnerHDMap;
            if ~isempty(scenario.ParkingLots)
                error(message('driving:scenario:ParkingLotsToRRHD'));
            end
            if isempty(scenario.RoadHistory)
                return;
            end
            % Build the map
            populateHeader(rrMap,scenario);
            populateMap(rrMap,scenario,exportActors);
        end
    end
end

function populateHeader(rrMap,scenario)
% Populate the map header
geoRef = scenario.GeoReference;
if ~isempty(geoRef)
    rrMap.GeoReference = geoRef(1:2);
else
    % Always use some real world GeoReference so that conversion of local
    % coordinates to RoadRunner scene coordinates using UTM has better
    % accuracy than at [0 0].
    rrMap.GeoReference = driving.internal.MapUtils.DefaultGeoReference(1:2);
end
rb = roadBoundaries(scenario);
rb = rb{1}; % Outermost boundary
if scenario.VerticalAxis == "X"
    rb = driving.scene.maputils.transformVerticalAxisToY(rb);
end
minBound = min(rb);
maxBound = max(rb);
bounds = [minBound;maxBound];
rrMap.GeographicBoundary = bounds;
end

function populateMap(rrMap,scenario,exportActors)
% Populate the map
% Lanes and lane boundaries - Driving scenarios use a road centric data
% model. Lanes and lane boundaries are obtained from the RoadSegment
% objects.
createLanesAndBoundaries(rrMap,scenario);
% Lane Markings - In driving scenarios, we have the following lane markings
% Dashed, DashedSolid, DoubleDashed, DoubleSolid, Solid, SolidDashed,
% Unmarked. We will create a marking for each of these and they can be
% referenced in the parametric attribution of lane boundaries above.
% Markings are created for both white and yellow colors.
roadrunner.maputils.createLaneMarkingTypes(rrMap);
% Barriers
if ~isempty(scenario.Barriers)
    % Create Driving Scenario Barrier Types
    roadrunner.maputils.createBarrierTypes(rrMap);
    % Create the Barriers
    driving.scene.maputils.createBarriers(rrMap,scenario);
end
% Static Actors
staticActors = cellfun(@(x)(isa(x,'driving.scenario.Stationary')), {scenario.Actors.MotionStrategy});
if any(staticActors) && exportActors
    % Create the Static Actors and their types
    driving.scene.maputils.createActors(rrMap,scenario,scenario.Actors(staticActors));
end
end

function createLanesAndBoundaries(rrMap,scenario)
% Roads in the driving scenario are converted to lanes, lane boundaries, and lane groups.
allRoadSegms = scenario.RoadSegments(:);
% Road Groups are not supported
allRoadGrpSegms = scenario.RoadGroupSegments;
if ~isempty(allRoadGrpSegms)
    error(message('driving:scenario:RoadGroupsToRRHD'));
end
for rndx = 1:length(allRoadSegms)
    aRoadSegm = allRoadSegms(rndx);
    if aRoadSegm.IsLoop
        driving.scene.maputils.buildLoopingRoad(rrMap,scenario,aRoadSegm);
    else
        driving.scene.maputils.buildSimpleRoad(rrMap,scenario,aRoadSegm);
    end
end
end






