% If a previous tracker is defined, clear it
clear trackingForFCW_kernel

% Set up the display
videoFile = '01_city_c2s_fcw_10s.mp4';
sensorConfigFile = 'SensorConfigurationData.mat';
[videoReader, videoDisplayHandle, bepPlotters, sensor] = helperCreateFCWDemoDisplay(videoFile,sensorConfigFile);

% Read the recorded detections file
detfile = '01_city_c2s_fcw_10s_sensor.mat';
[visionObjects, radarObjects, imu, lanes, timeStep, numSteps] = helperReadSensorRecordingsFile(detfile);

% An initial ego lane is calculated. If the recorded lane information is
% invalid, define the lane boundaries as straight lines half a lane
% distance on each side of the car.
laneWidth = 3.6; % meters
egoLane = struct('left', [0 0 laneWidth/2], 'right', [0 0 -laneWidth/2]);

% Prepare some time variables
timeStamp  = 0;    % Time since the beginning of the recording
index = 0;         % Index into the recorded sensor data

% Define the position and velocity selectors:
% The state vector is in constant acceleration: [x;vx;ax;y;vy;ay]
% Constant acceleration position: [x;y] = [1 0 0 0 0 0; 0 0 0 1 0 0] * State
positionSelector = [1 0 0 0 0 0; 0 0 0 1 0 0];
% Constant acceleration velocity: [x;y] = [0 1 0 0 0 0; 0 0 0 0 1 0] * State
velocitySelector = [0 1 0 0 0 0; 0 0 0 0 1 0];

% Allocate memory for number of tracks and time measurement in MATLAB
numTracks = zeros(1, numSteps);
runTimes  = zeros(1, numSteps);
while index < numSteps && ishghandle(videoDisplayHandle)
    % Update scenario counters
    index = index + 1;
    timeStamp = timeStamp + timeStep;
    tic;

    % Call the MATLAB tracking kernel file to perform the tracking
    [tracks, egoLane, numTracks(index), mostImportantObject] = trackingForFCW_kernel(...
        visionObjects(index), radarObjects(index), imu(index), lanes(index), egoLane, timeStamp, positionSelector, velocitySelector);

    runTimes(index) = toc; % Gather MATLAB run time data

    % Update video and bird's-eye plot displays
    frame = readFrame(videoReader);     % Read video frame
    laneBoundaries = [parabolicLaneBoundary(egoLane.left);parabolicLaneBoundary(egoLane.right)];
    helperUpdateFCWDemoDisplay(frame, videoDisplayHandle, bepPlotters, laneBoundaries, sensor, ...
        tracks, mostImportantObject, positionSelector, velocitySelector, visionObjects(index), radarObjects(index));
end

% Define the properties of the input based on the data in the first time frame.
compInputs  = {visionObjects(1), radarObjects(1), imu(1), lanes(1), egoLane, timeStamp, positionSelector, velocitySelector};

% Code generation may take some time.
h = msgbox({'Generating code. This may take a few minutes...';'This message box will close when done.'},'Codegen Message');
% Generate code.
try
    codegen trackingForFCW_kernel -args compInputs;
    close(h)
catch ME
    close(h)
    delete(videoDisplayHandle.Parent.Parent)
    throw(ME)
end

% If a previous tracker is defined, clear it
clear trackingForFCW_kernel_mex

% Allocate memory for number of tracks and time measurement
numTracksMex = zeros(1, numSteps);
runTimesMex  = zeros(1, numSteps);

% Reset the data and video counters
index = 0;
videoReader.CurrentTime = 0;

while index < numSteps && ishghandle(videoDisplayHandle)
    % Update scenario counters
    index = index + 1;
    timeStamp = timeStamp + timeStep;
    tic;

    % Call the generated MEX file to perform the tracking
    [tracks, egoLane, numTracksMex(index), mostImportantObject] = trackingForFCW_kernel_mex(...
        visionObjects(index), radarObjects(index), imu(index), lanes(index), egoLane, timeStamp, positionSelector, velocitySelector);

    runTimesMex(index) = toc; % Gather MEX run time data

    % Update video and bird's-eye plot displays
    frame = readFrame(videoReader);     % Read video frame
    laneBoundaries = [parabolicLaneBoundary(egoLane.left);parabolicLaneBoundary(egoLane.right)];
    helperUpdateFCWDemoDisplay(frame, videoDisplayHandle, bepPlotters, laneBoundaries, sensor, ...
        tracks, mostImportantObject, positionSelector, velocitySelector, visionObjects(index), radarObjects(index));
end

figure(2)
subplot(2,1,1)
plot(2:numSteps, numTracks(2:end), 'rs-', 2:numSteps, numTracksMex(2:end), 'bx-')
title('Number of Tracks at Each Step');
legend('MATLAB', 'MEX')
grid
subplot(2,1,2)
yyaxis left
plot(2:numSteps, runTimesMex(2:end)*1e3);
ylabel('MEX Processing Time (ms)');
yyaxis right
plot(2:numSteps, runTimes(2:end) ./ runTimesMex(2:end))
ylabel('Speed-Up Ratio');
title('MEX Processing Time and Speed-Up Ratio at Each Step')
grid
xlabel('Time Step')

p = profile('info');
speedUpRatio = mean(runTimes(2:end) ./ runTimesMex(2:end));
disp(['The generated code is ', num2str(speedUpRatio), ' times faster than the MATLAB code.']);
disp(['The computer clock speed is ', num2str(p.ClockSpeed / 1e9), ' GHz.']);