recordedData = fullfile(toolboxdir('driving'), 'drivingdata', ...
    'udacity', 'drive_segment_09_29_16');
[videoReader, videoTime] = helperLoadCameraData(fullfile(recordedData));

% Show the first frame of the camera data
imageFrame = readFrame(videoReader);
imshow(imageFrame, 'Border', 'tight');

data = load(fullfile(recordedData, 'gpsSequence.mat'));
gpsData = data.gpsTT;

% Plot the full route and the first position recorded from the GPS
gpsPlayer = geoplayer(gpsData.Latitude(1), gpsData.Longitude(1), 18);
plotRoute(gpsPlayer, gpsData.Latitude, gpsData.Longitude);
plotPosition(gpsPlayer, gpsData.Latitude(1), gpsData.Longitude(1));

reader = hereHDLMReader(gpsData.Latitude, gpsData.Longitude);

topologyLayer = read(reader, 'TopologyGeometry')

figure('Name', 'TopologyGeometry');
topologyAxes = plot(topologyLayer);
hold(topologyAxes, 'on');
geoplot(topologyAxes, gpsData.Latitude, gpsData.Longitude, ...
    'bo-', 'DisplayName', 'Route');

topologyTable = helperGetGeometry(topologyLayer.LinksStartingInTile, ...
    {'LinkId', 'Geometry.Here2dCoordinateDiffs'});
topologyTable.Properties.VariableNames = {'LinkId', 'Geometry'};

linkMatcher = HelperLinkMatcher(topologyTable);

% Match first point of the recorded route to the most probable link
[linkId, linkLat, linkLon] = match(linkMatcher, gpsData.Latitude(1), ...
    gpsData.Longitude(1), gpsData.Velocity(1,1:2));

% Plot the shape geometry of the link
geoplot(gpsPlayer.Axes, linkLat, linkLon, 'r.-');

speedLayer = read(reader, 'SpeedAttributes');

speedTable = helperGetSpeedLimits(speedLayer);

% Find the speed limit entry for the matched link
speed = speedTable(speedTable.LinkId == linkId, :);

laneTopologyLayer = read(reader, 'LaneTopology')
laneAxes = helperPlotLayer(laneTopologyLayer, ...
    gpsData.Latitude, gpsData.Longitude);
geolimits(laneAxes, [37.3823, 37.3838], [-122.1151, -122.1128]);

laneGroupFields = {'LaneGroupId', ...
    'BoundaryGeometry.LeftBoundary.Here2dCoordinateDiffs', ...
    'BoundaryGeometry.RightBoundary.Here2dCoordinateDiffs'};
laneTopologyTable = helperGetGeometry(laneTopologyLayer.LaneGroupsStartingInTile, ...
    laneGroupFields);
laneTopologyTable.Properties.VariableNames = {'LaneGroupId', ...
    'LeftGeometry', 'RightGeometry'};

referenceLayer = read(reader, 'LaneRoadReferences');
referenceTable = helperGetReferences(referenceLayer);

laneGroupMatcher = HelperLaneGroupMatcher(referenceTable, laneTopologyTable);

% Match the lane group and relative direction
[laneGroupId, isForward, boundGeometry] = match(laneGroupMatcher, linkId, ...
    gpsData.Latitude(1), gpsData.Longitude(1), gpsData.Velocity(1,1:2));

% Plot the boundary geometry of the lane group
geoplot(gpsPlayer.Axes, boundGeometry(:,1), boundGeometry(:,2), 'm.-');

laneAttributesLayer = read(reader, 'LaneAttributes');
laneAttributesTable = helperGetLaneAttributes(laneAttributesLayer);

% Find the lane attribute entry for the matched lane group
laneAttribute = laneAttributesTable.LaneGroupId == laneGroupId;

hdlmUI = HelperHDLMUI(gpsData.Latitude(1), gpsData.Longitude(1));

% Synchronize the camera and GPS data into a common timetable
synchronizedData = synchronize(videoTime, gpsData);
videoReader.CurrentTime = 0;
maxDisplayRate = videoReader.FrameRate * 5;

% Initialize some variables to maintain history
prevLinkId      = 0;
prevLaneGroupId = 0;

for idx = 1 : height(synchronizedData)

    timeStamp = synchronizedData.Time(idx);

    % Check if the current timestamp has GPS data
    hasGPSFrame = ~(ismissing(synchronizedData.Latitude(idx)) || ...
        ismissing(synchronizedData.Longitude(idx)));

    if hasGPSFrame
        latitude  = synchronizedData.Latitude(idx);
        longitude = synchronizedData.Longitude(idx);
        velocity  = synchronizedData.Velocity(idx, 1:2);

        % Match GPS position to link
        [linkId, linkLat, linkLon] = match(linkMatcher, ...
            latitude, longitude, velocity);

        if linkId ~= prevLinkId
            % Update link
            updateLink(hdlmUI, linkLat, linkLon);
            prevLinkId = linkId;

            % Update speed limit
            speed = speedTable(speedTable.LinkId == linkId, :);
            updateSpeed(hdlmUI, speed.Value);
        end

        % Match GPS position to lane group
        [laneGroupId, isForward, boundGeometry] = match(laneGroupMatcher, linkId, ...
            latitude, longitude, velocity);

        if laneGroupId ~= prevLaneGroupId
            % Update lane group
            updateLaneGroup(hdlmUI, boundGeometry);
            prevLaneGroupId = laneGroupId;

            % Update lane types and boundary markings
            laneAttribute = laneAttributesTable.LaneGroupId == laneGroupId;
            plotLanes(hdlmUI, laneAttributesTable.Lanes{laneAttribute}, ...
                laneAttributesTable.LaneBoundaries{laneAttribute}, isForward);
        end

        updatePosition(hdlmUI, latitude, longitude);
    else
        % Read frame of the video
        imageFrame = readFrame(videoReader);
    end

    updateImage(hdlmUI, imageFrame);
    updateTime(hdlmUI, timeStamp);
    pause(1/maxDisplayRate);
end

function gx = helperPlotLayer(layer, latitude, longitude)
%helperPlotLayer Create geographic plot with layer data and route
%   gx = helperPlotLayer(layer, latitude, longitude) creates a geographic
%   axes plot with the plottable HDLM layer and the route given by latitude
%   and longitude on a new figure.

figure;

% Plot layer
gx = plot(layer);

% Enable adding data to the plot
hold(gx, 'on');

% Plot latitude, longitude data
geoplot(gx, latitude, longitude, 'bo-', 'DisplayName', 'Route');
hold(gx, 'off');
end

function geometryTable = helperGetGeometry(layer, fields)
%helperGetGeometry Create a table with geometry for topology elements
%   geometryTable = helperGetGeometry(layer, fields) returns a table
%   formatted with the specified geometry fields of the TopologyGeometry
%   and LaneTopology layers.

% Pre-allocate struct
S = repmat(struct, size(layer));

for field = fields
    C = strsplit(field{:}, '.');
    for idx = 1:numel(layer)
        fieldname = strjoin(C, '');
        S(idx).(fieldname) = getfield(layer, {idx}, C{:});
    end
end

geometryTable = struct2table(S);
end

function speedTable = helperGetSpeedLimits(layer)
%helperGetSpeedLimits Create a data table with speed limits
%   speedTable = helperGetSpeedLimits(layer) returns a table formatted with
%   the speed limit start, end, direction, and value along a specified link
%   as given by the SpeedAttributes layer object specified by layer.

speed = struct(...
    'LinkId',       {}, ...
    'Start',        {}, ...
    'End',          {}, ...
    'Direction',    {}, ...
    'Value',        {});

for idx = 1 : numel(layer.LinkAttribution)

    % Assign the link ID
    link            = layer.LinkAttribution(idx);
    attributions    = link.ParametricAttribution;

    % Examine each attribute assigned to the link
    for attrIndex = 1 : numel(attributions)

        linkAttr = vertcat(attributions.LinkParametricAttribution);

        % For each attribute, check if the speed limit information is
        % listed. If speed limit is provided, make an entry.
        for linkAttrIndex = 1 : numel(linkAttr)

            if ~isempty(linkAttr(linkAttrIndex).SpeedLimit)

                % Assign speed limit to specified link
                speedLimit = struct;
                speedLimit.LinkId       = link.LinkLocalRef;
                speedLimit.Start        = attributions(attrIndex).AppliesToRange.RangeOffsetFromStart;
                speedLimit.End          = attributions(attrIndex).AppliesToRange.RangeOffsetFromEnd;
                speedLimit.Direction    = attributions(attrIndex).AppliesToDirection;
                speedLimit.Value        = linkAttr(linkAttrIndex).SpeedLimit.Value;

                % Convert KPH to MPH
                if strcmpi(linkAttr(linkAttrIndex).SpeedLimit.Unit, 'KILOMETERS_PER_HOUR')
                    speedLimit.Value = speedLimit.Value / 1.609;
                end

                if strcmpi(speedLimit.Direction, 'BOTH')
                    speed = [speed; speedLimit]; %#ok<AGROW>
                end
            end
        end

    end
end

speedTable = struct2table(speed);
end

function laneRoadReferenceTable = helperGetReferences(layer)
%helperGetReferences Create a data table with lane road references
%   laneRoadReferenceTable = helperGetReferences(layer) returns a table
%   formatted with a list of all lane groups existing on a specified link
%   as given by the LaneRoadReferences layer object specified by layer.

numLinks = numel(layer.LinkLaneGroupReferences);
reference = repmat(struct('LinkId', {}, 'LaneGroupId', {}), numLinks, 1);

% Get references from links to lane groups
for idx = 1 : numLinks
    link = layer.LinkLaneGroupReferences(idx);
    laneGroups = vertcat(link.LaneGroupReferences.LaneGroupRef);

    reference(idx).LinkId = link.LinkLocalRef;
    reference(idx).LaneGroupId = [laneGroups(:).LaneGroupId]';
end

laneRoadReferenceTable = struct2table(reference);
end

function laneAttributesTable = helperGetLaneAttributes(layer)
%helperGetLaneAttributes Create a table with lane and boundary types
%   laneAttributesTable = helperGetLaneAttributes(layer) returns a table
%   formatted with the lane types and the lane boundary markings for each
%   lane group in the LaneAttributes layer object specified by layer.

for laneGroupAttrIndex = 1 : numel(layer.LaneGroupAttribution)
    laneGroup = layer.LaneGroupAttribution(laneGroupAttrIndex);
    attributes(laneGroupAttrIndex).LaneGroupId = laneGroup.LaneGroupRef; %#ok

    % Get lane types for each lane group
    for laneAttrIndex = 1 : numel(laneGroup.LaneAttribution)

        lane = laneGroup.LaneAttribution(laneAttrIndex);

        laneAttr = vertcat(lane.ParametricAttribution);
        laneAttr = vertcat(laneAttr.LaneParametricAttribution);

        for idx = 1 : numel(laneAttr)
            if ~isempty(laneAttr(idx).LaneType)
                attributes(laneGroupAttrIndex).Lanes{lane.LaneNumber} = ...
                    laneAttr(idx).LaneType;
            end
        end
    end

    % Get lane boundaries for each lane group
    for laneBoundaryIndex = 1 : numel(laneGroup.LaneBoundaryAttribution)
        laneBoundary = laneGroup.LaneBoundaryAttribution(laneBoundaryIndex);
        boundaries   = vertcat(laneBoundary.ParametricAttribution.LaneBoundaryParametricAttribution);

        attributes(laneGroupAttrIndex).LaneBoundaries{laneBoundary.LaneBoundaryNumber} = ...
            boundaries.LaneBoundaryMarking;
    end

end

laneAttributesTable = struct2table(attributes);
end

function [videoReader, videoTime] = helperLoadCameraData(dirName)
%helperLoadCameraData Load camera images from folder in a timetable
%   [videoReader, videoTime] = helperLoadCameraData(dirName) loads a video
%   from the folder dirName. Timestamps for the video are read from a
%   MAT-file in the folder named timeStamps.mat.

if ~isfolder(dirName)
    error('Expected dirName to be a path to a folder.')
end

matFileName = fullfile(dirName, 'centerCameraTime.mat');
if exist(matFileName, 'file') ~= 2
    error('Expected dirName to have a MAT-file named centerCameraTime.mat containing timestamps.')
end

% Load the MAT-file with timestamps
ts = load(matFileName);
fieldNames = fields(ts);
Time = ts.(fieldNames{1});

videoFileName = fullfile(dirName, 'centerCamera.avi');
if exist(matFileName, 'file') ~= 2
    error('Expected dirName to have a video file named centerCamera.avi.')
end

% Load the video file
videoTime = timetable(Time);
videoReader = VideoReader(videoFileName);
end