% Set up video reader and player
videoFile      = '01_city_c2s_fcw_10s.mp4';
videoReader = VideoReader(videoFile);
videoPlayer = vision.DeployableVideoPlayer;

% Jump to the desired frame
time = 5.9;
videoReader.CurrentTime = time;
frameWithoutAnnotations = readFrame(videoReader);

imshow(frameWithoutAnnotations); 
title('Original Video Frame')

recordingFile  = '01_city_c2s_fcw_10s_sensor.mat';
[visionObjects, laneReports, timeStep, numSteps] = readDetectionsFile(recordingFile);
currentStep = round(time / timeStep) + 1;
videoDetections = processDetections(visionObjects(currentStep));
laneBoundaries = processLanes(laneReports(currentStep));

% Set up the monoCamera object for on-video display
sensor = setupMonoCamera(videoReader);

frameWithAnnotations = updateDisplay(frameWithoutAnnotations, sensor, videoDetections, laneBoundaries);

imshow(frameWithAnnotations); 
title('Annotated Video Frame')

% Reset the time back to zero
currentStep = 0;                % Reset the recorded data timestep
videoReader.CurrentTime = 0;    % Reset the video reader time
while currentStep < numSteps && hasFrame(videoReader)
    % Update scenario counters
    currentStep = currentStep + 1;
    
    % Get the current time
    tic
    
    % Prepare the detections to the tracker
    videoDetections = processDetections(visionObjects(currentStep), videoDetections);

    % Process lanes
    laneBoundaries = processLanes(laneReports(currentStep));
    
    % Update video frame with annotations from the reported objects
    frameWithoutAnnotations = readFrame(videoReader);
    frameWithAnnotations = updateDisplay(frameWithoutAnnotations, sensor, videoDetections, laneBoundaries);
    
    % The recorded data was obtained at a rate of 20 frames per second.
    % Pause for 50 milliseconds for a more realistic display rate. If you
    % process data and form tracks in this loop, you do not need this
    % pause.
    pause(0.05 - toc);
    
    % Display annotated frame
    videoPlayer(frameWithAnnotations);    
end

function sensor = setupMonoCamera(vidReader)
% Define the camera intrinsics from the video information
focalLength    = [1260 1100];                    % [fx, fy]              % pixels
principalPoint = [360 245];                      % [cx, cy]              % pixels
imageSize = [vidReader.height, vidReader.width]; % [numRows, numColumns] % pixels
intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);

% Define the camera mounting (camera extrinsics)
mountingHeight = 1.45;   % height in meters from the ground
mountingPitch  = 1.25;   % pitch of the camera in degrees
mountingRoll   = 0.15;   % roll of the camera in degrees
mountingYaw    = 0;      % yaw of the camera in degrees
sensor = monoCamera(intrinsics, mountingHeight, ...
    'Pitch', mountingPitch, ...
    'Roll',  mountingRoll, ...
    'Yaw',   mountingYaw);
end

function frame = updateDisplay(frame, sensor, videoDetections, laneBoundaries)

% Allocate memory for bounding boxes
bboxes = zeros(numel(videoDetections), 4);

% Create the bounding boxes
for i = 1:numel(videoDetections)
    % Use monoCamera sensor to convert the position in vehicle coordinates 
    % to the position in image coordinates. 
    % Notes: 
    %   1. The width of the object is reported and is used to calculate the
    %      size of the bounding box around the object (half width on each
    %      side). The height of the object is not reported. Instead, the 
    %      function uses a height/width ratio of 0.85 for cars and 3 for
    %      pedestrians.
    %   2. The reported location is at the center of the object at ground 
    %      level, i.e., the bottom of the bounding box.
    xyLocation1 = vehicleToImage(sensor, videoDetections(i).positions' + [0,videoDetections(i).widths/2]);
    xyLocation2 = vehicleToImage(sensor, videoDetections(i).positions' - [0,videoDetections(i).widths/2]);
    dx = xyLocation2(1) - xyLocation1(1);
    
    % Define the height/width ratio based on object class
    if strcmp(videoDetections(i).labels, 'Car')
        dy = dx * 0.85;
    elseif  strcmp(videoDetections(i).labels, 'Pedestrian')
        dy = dx * 3;
    else
        dy = dx;
    end
    
    % Estimate the bounding box around the vehicle. Subtract the height of
    % the bounding box to define the top-left corner.
    bboxes(i,:) =[(xyLocation1 - [0, dy]), dx, dy];
end

% Add labels
labels = {videoDetections(:).labels}';

% Add bounding boxes to the frame
if ~isempty(labels)
    frame = insertObjectAnnotation(frame, 'rectangle', bboxes, labels,...
        'Color', 'yellow', 'FontSize', 10, 'TextBoxOpacity', .8, 'LineWidth', 2);
end

% Display the lane boundary on the video frame
xRangeVehicle = [1, 100];
xPtsInVehicle = linspace(xRangeVehicle(1), xRangeVehicle(2), 100)';
frame = insertLaneBoundary(frame, laneBoundaries(1), sensor, xPtsInVehicle, ...
    'Color', 'red');
frame = insertLaneBoundary(frame, laneBoundaries(2), sensor, xPtsInVehicle, ...
    'Color', 'green');
end

function [visionObjects, laneReports, timeStep, numSteps] = readDetectionsFile(filename)
A = load(strcat(filename));
visionObjects = A.vision;
laneReports = A.lane;

% Prepare some time variables
timeStep = 0.05;                 % Lane data is provided every 50 milliseconds
numSteps = numel(visionObjects); % Number of recorded timesteps
end

function videoDetections = processDetections(visionData, videoDetections)
% The video sensor reports a classification value as an integer
% according to the following enumeration (starting from 0)
ClassificationValues = {'Unknown', 'Unknown Small', 'Unknown Big', ...
    'Pedestrian', 'Bike', 'Car', 'Truck', 'Barrier'};

% The total number of objects reported by the sensor in this frame
numVideoObjects = visionData.numObjects;

% The video objects are reported only 10 times per second, but the video
% has a frame rate of 20 frames per second. To prevent the annotations from
% flickering on and off, this function returns the values from the previous
% timestep if there are no video objects.
if numVideoObjects == 0
    if nargin == 1 % Returning a result even if there is no previous value
        videoDetections = struct('positions', {}, 'labels', {}, 'widths', {});
    end
    return;
else
    % Prepare a container for the relevant properties of video detections
    videoDetections = struct('positions', [], 'labels', [], 'widths', []);
    for i = 1:numVideoObjects
        videoDetections(i).widths = visionData.object(i).size(2);
        videoDetections(i).positions = visionData.object(i).position(1:2);
        videoDetections(i).labels = ClassificationValues{visionData.object(i).classification + 1};
    end
end
end

function laneBoundaries = processLanes(laneReports)
% Return processed lane boundaries

% Boundary type information
types = {'Unmarked', 'Solid', 'Dashed', 'Unmarked', 'BottsDots', ...
    'Unmarked', 'Unmarked', 'DoubleSolid'};

% Read the recorded lane reports for this frame
leftLane    = laneReports.left;
rightLane   = laneReports.right;

% Create parabolicLaneBoundary objects for left and right lane boundaries
leftParams = cast([leftLane.curvature, leftLane.headingAngle, leftLane.offset], 'double');
leftBoundaries = parabolicLaneBoundary(leftParams);
leftBoundaries.BoundaryType = types{leftLane.boundaryType};

rightParams  = cast([rightLane.curvature, rightLane.headingAngle, rightLane.offset], 'double');
rightBoundaries = parabolicLaneBoundary(rightParams);
rightBoundaries.BoundaryType = types{rightLane.boundaryType};

laneBoundaries = [leftBoundaries, rightBoundaries];
end