% Load MAT file with ground truth data
loaded = load('caltech_cordova1_laneAndVehicleGroundTruth.mat');

% Create a VideoReader object to read frames of the video.
videoName  = loaded.videoName;
fileReader = VideoReader(videoName);

% The ground truth data is organized in a timetable.
gtdata = loaded.groundTruthData;

% Display the first few rows of the ground truth data.
head(gtdata)

% Read the first frame of the video.
frame = readFrame(fileReader);

% Extract all lane points in the first frame.
lanePoints = gtdata.LaneBoundaries{1};

% Extract vehicle bounding boxes in the first frame.
vehicleBBox = gtdata.Vehicles{1};

% Superimpose the right lane points and vehicle bounding boxes.
frame = insertMarker(frame, lanePoints{2}, 'X');
frame = insertObjectAnnotation(frame, 'rectangle', vehicleBBox, 'Vehicle');

% Display ground truth data on the first frame.
figure
imshow(frame)

% Set up monoSensorHelper to process video.
monoCameraSensor = loaded.sensor;
monoSensorHelper = helperMonoSensor(monoCameraSensor);

% Create new timetable with same Time vector for measurements.
measurements = timetable(gtdata.Time);

% Set up timetable columns for holding lane boundary and vehicle data.
numFrames = floor(fileReader.FrameRate*fileReader.Duration);
measurements.LaneBoundaries    = cell(numFrames, 2);
measurements.VehicleDetections = cell(numFrames, 1);
gtdata.LanesInVehicleCoord     = cell(numFrames, 2);

% Rewind the video to t = 0, and create a frame index to hold current
% frame.
fileReader.CurrentTime = 0;
frameIndex  = 0;

% Loop through the videoFile until there are no new frames.
while hasFrame(fileReader)
    frameIndex = frameIndex+1;
    frame      = readFrame(fileReader);

    % Use the processFrame method to compute detections.
    % This method can be replaced with a custom lane detection method.
    detections = processFrame(monoSensorHelper, frame);

    % Store the estimated lane boundaries and vehicle detections.
    measurements.LaneBoundaries{frameIndex} = [detections.leftEgoBoundary ...
                                               detections.rightEgoBoundary];
    measurements.VehicleDetections{frameIndex} = detections.vehicleBoxes;

    % To facilitate comparison, convert the ground truth lane points to the
    % vehicle coordinate system.
    gtPointsThisFrame = gtdata.LaneBoundaries{frameIndex};
    vehiclePoints = cell(1, numel(gtPointsThisFrame));
    for ii = 1:numel(gtPointsThisFrame)
        vehiclePoints{ii} = imageToVehicle(monoCameraSensor, gtPointsThisFrame{ii});
    end

    % Store ground truth points expressed in vehicle coordinates.
    gtdata.LanesInVehicleCoord{frameIndex} = vehiclePoints;
end

% Rewind video to t = 0.
fileReader.CurrentTime = 0;

% Read the first frame of the video.
frame = readFrame(fileReader);
birdsEyeImage = transformImage(monoSensorHelper.BirdsEyeConfig, frame);

% Extract right lane points for the first frame in Bird's-Eye View.
firstFrameVehiclePoints = gtdata.LanesInVehicleCoord{1};
pointsInBEV = vehicleToImage(monoSensorHelper.BirdsEyeConfig, firstFrameVehiclePoints{2});

% Superimpose points on the frame.
birdsEyeImage = insertMarker(birdsEyeImage, pointsInBEV, 'X', 'Size', 6);

% Display transformed points in Bird's-Eye View.
figure
imshow(birdsEyeImage)

threshold = 0.25; % in meters

[numMatches, numMisses, numFalsePositives, assignments] = ...
    evaluateLaneBoundaries(measurements.LaneBoundaries, ...
                           gtdata.LanesInVehicleCoord, ...
                           threshold);

disp(['Number of matches: ', num2str(numMatches)]);
disp(['Number of misses: ', num2str(numMisses)]);
disp(['Number of false positives: ', num2str(numFalsePositives)]);

averageDistance = helperComputeLaneStatistics(measurements.LaneBoundaries, ...
                                              gtdata.LanesInVehicleCoord, ...
                                              assignments, @mean);

% Plot average distance between estimates and ground truth.
figure
stem(gtdata.Time, averageDistance)
title('Average Distance Between Estimates and Ground Truth')
grid on
ylabel('Distance in Meters')
legend('Left Boundary','Right Boundary')

% Compute the estimated point locations using the monoCamera.
[estVehiclePoints, estImagePoints] = helperGetCorrespondingPoints(monoCameraSensor, ...
                                     measurements.LaneBoundaries, ...
                                     gtdata.LanesInVehicleCoord, ...
                                     assignments);

% Add estimated lanes to the measurements timetable.
measurements.EstimatedLanes      = estImagePoints;
measurements.LanesInVehicleCoord = estVehiclePoints;

% Create a new timetable with all the variables needed for visualization.
names = {'LanePoints'; 'DetectedLanePoints'};
types = labelType({'Line'; 'Line'});
labelDefs = table(names, types, 'VariableNames', {'Name','Type'});

visualizeInFrame = timetable(gtdata.Time, ...
                             gtdata.LaneBoundaries, ...
                             measurements.EstimatedLanes, ...
                             'VariableNames', names);

% Create groundTruth object.
dataSource = groundTruthDataSource(videoName);
dataToVisualize = groundTruth(dataSource, labelDefs, visualizeInFrame);

% Save all the results of the previous section in distanceData.mat in a
% temporary folder.
dataToLoad = [tempdir 'distanceData.mat'];
save(dataToLoad, 'monoSensorHelper', 'videoName', 'measurements', 'gtdata', 'averageDistance');

function stat = helperComputeLaneStatistics(estModels, gtPoints, assignments, fcnHandle)

    numFrames = length(estModels);
    % Make left and right estimates NaN by default to represent lack of
    % data.
    stat = NaN*ones(numFrames, 2);

    for frameInd = 1:numFrames
        % Make left and right estimates NaN by default.
        stat(frameInd, :) = NaN*ones(2, 1);

        for idx = 1:length(estModels{frameInd})
            % Ignore false positive assignments.
            if assignments{frameInd}(idx) == 0
                continue;
            end

            % The kth boundary in estModelInFrame is matched to kth
            % element indexed by assignments in gtPointsInFrame.
            thisModel = estModels{frameInd}(idx);
            thisGT = gtPoints{frameInd}{assignments{frameInd}(idx)};
            thisGTModel = driving.internal.piecewiseLinearBoundary(thisGT);
            if mean(thisGTModel.Points(:,2)) > 0
                % left lane
                xPoints = thisGTModel.Points(:,1);
                yDist = zeros(size(xPoints));
                for index = 1:numel(xPoints)
                    gtYPoints   = thisGTModel.computeBoundaryModel(xPoints(index));
                    testYPoints = thisModel.computeBoundaryModel(xPoints(index));
                    yDist(index) = abs(testYPoints-gtYPoints);
                end
                stat(frameInd, 1) = fcnHandle(yDist);
            else % right lane
                xPoints = thisGTModel.Points(:,1);
                yDist = zeros(size(xPoints));
                for index = 1:numel(xPoints)
                    gtYPoints   = thisGTModel.computeBoundaryModel(xPoints(index));
                    testYPoints = thisModel.computeBoundaryModel(xPoints(index));
                    yDist(index) = abs(testYPoints-gtYPoints);
                end
                stat(frameInd, 2) = fcnHandle(yDist);
            end
        end
    end
end

function [vehiclePoints, imagePoints] = helperGetCorrespondingPoints(monoCameraSensor, estModels, gtPoints, assignments)

    numFrames = length(estModels);
    imagePoints = cell(numFrames, 1);
    vehiclePoints = cell(numFrames, 1);

    for frameInd = 1:numFrames
        if isempty(assignments{frameInd})
            imagePointsInFrame = [];
            vehiclePointsInFrame = [];
        else
            estModelInFrame = estModels{frameInd};
            gtPointsInFrame = gtPoints{frameInd};
            imagePointsInFrame = cell(length(estModelInFrame), 1);
            vehiclePointsInFrame = cell(length(estModelInFrame), 1);
            for idx = 1:length(estModelInFrame)

                % Ignore false positive assignments.
                if assignments{frameInd}(idx) == 0
                    imagePointsInFrame{idx} = [NaN NaN];
                    continue;
                end

                % The kth boundary in estModelInFrame is matched to kth
                % element indexed by assignments in gtPointsInFrame.
                thisModel = estModelInFrame(idx);
                thisGT = gtPointsInFrame{assignments{frameInd}(idx)};
                xPoints = thisGT(:, 1);
                yPoints = thisModel.computeBoundaryModel(xPoints);

                vehiclePointsInFrame{idx} = [xPoints, yPoints];
                imagePointsInFrame{idx} = vehicleToImage(monoCameraSensor, [xPoints yPoints]);
            end
        end
        vehiclePoints{frameInd} = vehiclePointsInFrame;
        imagePoints{frameInd}   = imagePointsInFrame;
        % Make imagePoints [] instead of {} to comply with groundTruth object.
        if isempty(imagePoints{frameInd})
            imagePoints{frameInd} = [];
        end
        if isempty(vehiclePoints{frameInd})
            vehiclePoints{frameInd} = [];
        end
    end
end