% ----------------------------------------------------------------------
    % Step 1: Define the required properties describing the algorithm. This
    % includes Name, Description, and UserDirections.
    properties(Constant)
        
        % Name Algorithm Name
        %   Character vector specifying the name of the algorithm.
        Name = 'Lidar Vehicle Detector';
        
        % Description Algorithm Description
        %   Character vector specifying the short description of the algorithm.
        Description = 'Detect vehicles in point cloud using PointPillars network';
        
        % UserDirections Algorithm Usage Directions
        %   Cell array of character vectors specifying directions for
        %   algorithm users to follow to use the algorithm.
        UserDirections = {['ROI Label Definition Selection: select one of ' ...
            'the ROI definitions to be labeled'], ...
            ['Run: Press RUN to run the automation algorithm. '], ...
            ['Review and Modify: Review automated labels over the interval ', ...
            'using playback controls. Modify/delete/add ROIs that were not ' ...
            'satisfactorily automated at this stage. If the results are ' ...
            'satisfactory, click Accept to accept the automated labels.'], ...
            ['Change Settings and Rerun: If automated results are not ' ...
            'satisfactory, you can try to re-run the algorithm with ' ...
            'different settings. To do so, click Undo Run to undo ' ...
            'current automation run, click Settings, make changes to Settings,' ...
            'and press Run again.'], ...
            ['Accept/Cancel: If the results of automation are satisfactory, ' ...
            'click Accept to accept all automated labels and return to ' ...
            'manual labeling. If the results of automation are not ' ...
            'satisfactory, click Cancel to return to manual labeling ' ...
            'without saving the automated labels.']};
    end

    % ---------------------------------------------------------------------
    % Step 2: Define properties you want to use during the algorithm
    % execution.
    properties
        
        % SelectedLabelName 
        %   Name of the selected label. Vehicles detected by the algorithm 
        %   are assigned this variable name.
        SelectedLabelName
        
        % PretrainedNetwork
        %   PretrainedNetwork saves the pretrained PointPillars dlnetwork.
        PretrainedNetwork
        
        %   Range of point clouds along x,y,and z-axis used to crop
        %   full-view point clouds to front-view point clouds.
        %   These parameters guide the calculation of the size of the 
        %   input [xn,yn] passed to the network.

        %   xMin = 0.0;     % Minimum value along X-axis.
        %   yMin = -39.68;  % Minimum value along Y-axis.
        %   zMin = -5.0;    % Minimum value along Z-axis.
        %   xMax = 69.12    % Maximum value along X-axis.
        %   yMax = 39.68;   % Maximum value along Y-axis.
        %   zMax = 5.0;     % Maximum value along Z-axis.
        %   xStep = 0.16;   % Resolution along X-axis.
        %   yStep = 0.16;   % Resolution along Y-axis.
        %   dsFactor = 2.0; % Downsampling factor.
        
        %  Dimensions for the pseudo-image calculated as 
        %  round(((xMax - xMin) / xStep));.
        %  Xn = 432;
        %  Yn = 496;
        
        % GridParams 
        %  Parameter used to crop full-view point cloud to front-view,
        %  defined as {{xMin,yMin,zMin}, {xMax,yMax,zMax},
        %  {xStep,yStep,dsFactor}, {Xn,Yn}};
        GridParams = {{0.0,-39.68,-5.0}, {69.12,39.68,5.0}, {0.16,0.16,2.0}, {432,496}}
        
        % AnchorBoxes 
        %  Predefined bounding box dimensions based on the classes to
        %  detect, defined in the format {length, width, height,
        %  z-center, yaw angle}
        AnchorBoxes = {{3.9, 1.6, 1.56, -1.78, 0}, {3.9, 1.6, 1.56, -1.78, pi/2}};
        
        % P
        %  Number of prominent pillars.
        P = 12000;
        
        % N
        %  Number of points per pillar.
        N = 100;
        
        % ExecutionEnvironment
        %  Set the execution environment.
        ExecutionEnvironment = "auto";
        
        % ConfidenceThreshold
        %  Specify the confidence threshold to use only detections with 
        %  confidence scores above this value.
        ConfidenceThreshold = 0.45;
        
        % OverlapThreshold
        %  Specify the overlap threshold to remove overlapping detections.
        OverlapThreshold = 0.1;            
        
    end

        function isValid = checkSignalType(signalType)            
            % Only point cloud signal data is valid for the Lidar Vehicle
            % detector algorithm.
            isValid = (signalType == vision.labeler.loading.SignalType.PointCloud);           
        end

        function isValid = checkLabelDefinition(~, labelDef)            
            % Only cuboid ROI label definitions are valid for the Lidar
            % vehicle detector algorithm.
            isValid = labelDef.Type == labelType.Cuboid;
        end

        function isReady = checkSetup(algObj)            
            % Is there one selected ROI Label definition to automate.
            isReady = ~isempty(algObj.SelectedLabelDefinitions);
        end

        function settingsDialog(algObj)
            % Invoke dialog with options for modifying the 
            % threshold and confidence threshold. 
            lidarVehicleDetectorSettings(algObj)
        end

        function initialize(algObj,~)           
            % Store the name of the selected label definition. Use this
            % name to label the detected vehicles.
            algObj.SelectedLabelName = algObj.SelectedLabelDefinitions.Name;
            
            % Add the folder containing helper functions to the search path.
            addpath(fullfile(matlabroot,'examples','deeplearning_shared','main'))

            % Point to tempdir, where pretrainedNet was downloaded.
            preTrainedMATFile = fullfile(tempdir,'trainedPointPillarsNet.mat');
            assert(exist(preTrainedMATFile,'file'), ...
                sprintf(['File : %s does not exists \n Download Pretrained PointPillars ' ...
                'network with the link provided in the example'],preTrainedMATFile));
            pretrainedNet = load(preTrainedMATFile);
            algObj.PretrainedNetwork = pretrainedNet.net;           
        end

        function autoLabels = run(algObj, pointCloud)
            bBoxes = [];
            for i = 1:2           
                if i == 2
                    % Rotate the point cloud by 180 degrees.
                    thetha = pi;
                    rot = [cos(thetha) sin(thetha) 0; ...
                        -sin(thetha) cos(thetha) 0; ...
                        0  0  1];
                    trans = [0, 0, 0];
                    tform = rigid3d(rot, trans);
                    pointCloud = pctransform(pointCloud, tform);
                end
                
                % Crop the front view of the point clouds.
                processedPointCloud = helperCropFrontViewFromLidarData(pointCloud, ...
                    algObj.GridParams);
                
                % Detect vehicles using the PointPillars network.
                [box, ~, ~] = generatePointPillarDetections(algObj.PretrainedNetwork,  ...
                    processedPointCloud, algObj.AnchorBoxes, algObj.GridParams, ...
                    {algObj.SelectedLabelName}, algObj.ConfidenceThreshold, ...
                    algObj.OverlapThreshold, algObj.P, algObj.N, algObj.ExecutionEnvironment);

                if ~isempty(box)
                    if i == 2
                        box(:,1) = -box(:,1);
                        box(:,2) = -box(:,2);
                        box(:,9) = -box(:,9);
                    end
                    bBoxes = [bBoxes;box];
                end
            end
            if ~isempty(bBoxes)
                % Add automated labels at bounding box locations detected
                % by the vehicle detector, of type Cuboid and with the name
                % of the selected label.
                autoLabels.Name     = algObj.SelectedLabelName;
                autoLabels.Type     = labelType.Cuboid;
                autoLabels.Position = bBoxes;
            else
                autoLabels = [];
            end
        end

    mkdir('+vision/+labeler');
    copyfile(fullfile(matlabroot,'examples','lidar','main','LidarVehicleDetector.m'), ...
        '+vision/+labeler');

    lidarURL = 'https://www.mathworks.com/supportfiles/lidar/data/WPI_LidarData.tar.gz';
    lidarDataFolder = fullfile(tempdir,'WPI_LidarData',filesep);        
    lidarDataTarFile = lidarDataFolder + "WPI_LidarData.tar.gz";

    if ~exist(lidarDataFolder)
        mkdir(lidarDataFolder)
    end

    if ~exist(lidarDataTarFile, 'file')       
        disp('Downloading WPI Lidar driving data (760 MB)...');
        websave(lidarDataTarFile, lidarURL);
        untar(lidarDataTarFile, lidarDataFolder);
    end
    
    % Check if lidar tar.gz file is downloaded, but not uncompressed.
    if ~exist(fullfile(lidarDataFolder, 'WPI_LidarData.mat'), 'file')
        untar(lidarDataTarFile, lidarDataFolder);
    end

    % Load downloaded lidar data into the workspace.
    load(fullfile(lidarDataFolder, 'WPI_LidarData.mat'),'lidarData');
    lidarData = reshape(lidarData, size(lidarData,2),1);
    
    % Create new folder and write lidar data to PCD files.
    pcdDataFolder = lidarDataFolder + "lidarData";
    if ~exist(pcdDataFolder, 'dir')
        mkdir(fullfile(lidarDataFolder,'lidarData'));
    end

    disp('Saving WPI Lidar driving data to PCD files ...');
    for i = 920:940
        filename = strcat(fullfile(lidarDataFolder,'lidarData',filesep), ...
            num2str(i,'%06.0f'),'.pcd');
        pcwrite(lidarData{i}, filename);
    end

    pointCloudDir = fullfile(lidarDataFolder, 'lidarData');
    lidarLabeler(pointCloudDir);