function [gpsData,laneDetectionCamera,cameraImages] = helperLoadFile(startTimeStamp,endTimeStamp)
% This helper loads required data and returns data between the entered timeStamps
%returns - GPS, Camera, Lane Track Detections data

%   This is a helper script for example purposes and may be modified in the
%   future

%   Copyright 2021 The MathWorks, Inc.

% Checks for inputs
narginchk(2,2);
nargoutchk(0,3);
validateattributes(startTimeStamp,{'numeric'},{'nonnegative','nondecreasing','finite','nonnan','scalar','nonempty'});
validateattributes(endTimeStamp,{'numeric'},{'nonnegative','nondecreasing','finite','nonnan','scalar','nonempty'});
if startTimeStamp > endTimeStamp
    error("Invalid Time Stamps");
end

try
    tempFolder = fullfile(tempdir,'AutomotiveDataset');
    if ~isfolder(tempFolder)
        mkdir(tempFolder)
    end
    % GPS data
    url = 'https://ssd.mathworks.com/supportfiles/driving/data/highwayLaneChangeDatase/ins2.mat';
    filePath = fullfile(tempFolder, 'ins.mat');
    if ~isfile(filePath)
        websave(filePath,url); % Downloading the file
    end
    data = load(filePath,'ins'); % Loading the file
    % Keep GPS data between start and end timestamps
    [startStep, endStep] = helperTimestampIndex([data.ins.timeStamp].', startTimeStamp, endTimeStamp);
    gpsData = data.ins(startStep:endStep);
    % Lane track detections
    url = 'https://ssd.mathworks.com/supportfiles/driving/data/highwayLaneChangeDatase/trackList2.mat';
    filePath = fullfile(tempFolder, 'trackList.mat');
    if ~isfile(filePath)
        websave(filePath,url); % Downloading the file
    end
    detection = load(filePath, 'laneDetectionCamera');
    [startStep, endStep] = ...
        helperTimestampIndex([detection.laneDetectionCamera.timeStamp].', ...
        startTimeStamp, endTimeStamp);
    % Keep lane track detections between start and end timestamps
    laneDetectionCamera = detection.laneDetectionCamera(startStep:endStep);
    % Remove invalid detections
    i = 1;
    while i < size(laneDetectionCamera,2)
        l = laneDetectionCamera(i).left;
        r = laneDetectionCamera(i).right;
        if ~(l.isValid) || ~(r.isValid)
            laneDetectionCamera(i) = [];
        elseif (r.headingAngle == -1e9 || r.curvature == -1e9) || (l.headingAngle == -1e9 || l.curvature == -1e9)
            laneDetectionCamera(i) = [];
        else
            i = i+1;
        end
    end

    % Resize gps data and detections if they are not of the same size
    if size(laneDetectionCamera,2) ~= size(gpsData,2)
        [gpsData,laneDetectionCamera] = helperFindIntersection(gpsData,laneDetectionCamera);
    end
    % Camera data
    url = 'https://ssd.mathworks.com/supportfiles/driving/data/highwayLaneChangeDatase/camera2.mat';
    filePath = fullfile(tempFolder, 'camera.mat');
    if ~isfile(filePath)
        websave(filePath,url); % Downloading the file
    end
    camera  = load(filePath, 'imageData');
    % Keep Camera data between start and end timestamps
    [startStep, endStep] = ...
        helperTimestampIndex([camera.imageData.timeStamp].', ...
        startTimeStamp, endTimeStamp);

    cameraImages = camera.imageData(startStep:endStep);
    if size(gpsData) < 100
        warning("Data size very small");
    end
catch
    error('Unable to Load Data');
end
end

function [entryStep, exitStep] = helperTimestampIndex(data, entryTime, exitTime)
% helperTimestampIndex Helper function to fetch 
%  the start and end index of data-points

% Output arguments
% Entry steep index
% Exit steep index

%   Copyright 2021 The MathWorks, Inc.

% Fetch the count of data samples
count = size(data,1);

entryStep = 0;
exitStep = count;

flag = 0; % Pre entry step index
for i = 1 : count
    if flag == 0
        if entryTime <= data(i)
            entryStep = i;
            flag = 1; % Pre exit step index
        end
    elseif flag == 1
        if exitTime <= data(i)
            exitStep = i;
            flag = 2; % Post exit step index
        end
    end
end
end

function [gpsData,laneDetectionCamera] = helperFindIntersection(gpsData,laneDetectionCamera)
% This helper computes the intersection between two data array passed
mn = min(size(laneDetectionCamera,2),size(gpsData,2)); %size of the smaller data
minTime = min(laneDetectionCamera(1).timeStamp,gpsData(1).timeStamp);
lanetimeStamps = cast([laneDetectionCamera.timeStamp].' - minTime,'double') ;% Subtract minimum time to avoid overflow while convertion to double
gpstimeStamps = cast([gpsData.timeStamp].' - minTime,'double');% Subtract minimum time to avoid overflow while convertion to double
if size(laneDetectionCamera,2) < size(gpsData,2)
    indexes = zeros(mn,1);
    for i = 1:length(lanetimeStamps)
        % For ith time stamp in detections find the closest in gps
        timediff = cast(gpstimeStamps - lanetimeStamps(i),'double');
        idx = find(timediff >= 0,1);
        if isempty(idx)
            indexes(i) = length(timediff);
        end
        indexes(i) = idx;%stores the index
    end
    gpsData = gpsData(indexes(:));
elseif size(gpsData,2) < size(laneDetectionCamera,2) 
    indexes = zeros(mn,1);
   for i = 1:length(gpstimeStamps)
        % For ith time stamp in gps find the closest in detections
        timediff = cast(lanetimeStamps - gpstimeStamps(i),'double');
        idx = find(timediff >= 0,1);
        if isempty(idx)
            indexes(i) = length(timediff);
        end
        indexes(i) = idx;
    end
    laneDetectionCamera = laneDetectionCamera(indexes(:));%stores the index
end
end
