classdef velodyneROSMessageReader < handle & matlab.mixin.SetGet ...
        & matlab.mixin.Copyable

    properties (GetAccess = 'public', SetAccess = 'private')
        VelodyneMessages

        DeviceModel

        CalibrationFile

        OrganizePoints logical

        NumberOfFrames

        Duration

        StartTime

        EndTime

        Timestamps
    end

    properties (GetAccess = 'public', SetAccess = 'public', Dependent)

        CurrentTime
    end

    properties (Access = 'private')

        LaserCalibrationData

        UserCurrentTimeFlag

        CurrentTimeInternal

        Version = 1.0;
    end

    properties (Access = 'private', Transient)

        VelodyneROSMessageReaderObj
    end

    methods
        function value = get.CurrentTime(this)
            value = this.CurrentTimeInternal;
        end

        function set.CurrentTime(this, value)
            validateattributes(value, {'duration'}, ...
                               {'nonempty', 'scalar', 'finite'}, 'set', 'CurrentTime');

            % Check for value finiteness and acceptable limits.
            if (value < this.StartTime || value > this.EndTime)
                error(message('ros:mlros:velodyne:invalidCurrentTime', ...
                              char(this.StartTime), char(this.EndTime)));
            end
            this.UserCurrentTimeFlag = true;
            this.CurrentTimeInternal = value;
        end
    end

    methods (Access = 'public')
        function this = velodyneROSMessageReader(velodyneMessages, deviceModel, varargin)
            paramsStruct = iParseAndValidateInputs(velodyneMessages, deviceModel, varargin{:});

            this.VelodyneMessages       = paramsStruct.VelodyneMessages;
            this.DeviceModel            = paramsStruct.DeviceModel;
            this.CalibrationFile        = paramsStruct.CalibrationFile;
            this.OrganizePoints         = paramsStruct.OrganizePoints;

            % Setup calibration data
            [laserCalibrationData, distanceResolution] = iRetrieveCalibrationData( ...
                this.CalibrationFile, this.DeviceModel);

            this.LaserCalibrationData = laserCalibrationData;

            % Create the ROS message reader object
            this.VelodyneROSMessageReaderObj = roscpp.velodyne.internal.VelodyneROSMessageReader();

            openFcnArgs.LaserCalibrations               = this.LaserCalibrationData;
            openFcnArgs.DistanceResolution              = distanceResolution;
            openFcnArgs.SkipPartialFrames               = paramsStruct.SkipPartialFrames; % skipPartialFrames is for internal usage only.
            openFcnArgs.OrganizePoints                  = this.OrganizePoints;

            timeStruct = load(this.VelodyneROSMessageReaderObj, ...
                              this.VelodyneMessages, openFcnArgs, this.DeviceModel);

            % Fill class properties returned from mex call.
            if(~isempty(timeStruct))
                this.NumberOfFrames = timeStruct.NumberOfFrames;
                this.StartTime      = seconds(timeStruct.StartTime);
                this.EndTime        = seconds(timeStruct.EndTime);
                this.Duration       = seconds(timeStruct.Duration);
                this.Timestamps     = seconds(timeStruct.TimestampsVector);
            end

            this.CurrentTimeInternal = this.StartTime;
            this.UserCurrentTimeFlag = false;
        end


        function [ptCloudObj, pointTimestamps] = readFrame(this, varargin)
            narginchk(1,2);

            if nargin>1

                if isduration(varargin{1})

                    durationToSeek = varargin{1};

                    % Validate frameTime
                    validateattributes(durationToSeek, {'duration'}, ...
                                       {'nonempty', 'scalar', 'finite'}, 'readFrame', ...
                                       'frameTime');

                    % Check for valid duration values.
                    if (durationToSeek < this.StartTime || durationToSeek > this.EndTime)
                        error(message( 'ros:mlros:velodyne:invalidTimeDuration', ...
                                       char(this.StartTime), char(this.EndTime)));
                    end

                    % Convert to double, and remove start time
                    durationToSeekSeconds = double(seconds(durationToSeek - this.StartTime));

                    % Call builtin function with duration in seconds.
                    [xyziPoints, intensity, currentTimestamp, ...
                     rangeData, notNaNIndices, numValidPoints, ptTimestamps] = readPointCloud(...
                        this.VelodyneROSMessageReaderObj, this.VelodyneMessages, durationToSeekSeconds);

                elseif isnumeric(varargin{1})

                    frameNum = varargin{1};

                    % Validate frameNumber
                    validateattributes(frameNum, {'numeric'}, ...
                                       {'nonempty', 'scalar', 'finite', 'integer', 'real', 'positive', '<=', this.NumberOfFrames}, ...
                                       'readFrame', 'frameNumber');

                    % Convert to int32 and zero-based indexing
                    frameNumber = int32(frameNum - 1);

                    % Call builtin function with frame number.
                    [xyziPoints, intensity, currentTimestamp, ...
                     rangeData, notNaNIndices, numValidPoints, ptTimestamps] = readPointCloud(...
                        this.VelodyneROSMessageReaderObj, this.VelodyneMessages, frameNumber);
                else
                    % The type of varargin{1} is invalid. Throw an error
                    validateattributes(varargin{1}, {'numeric', 'duration'}, ...
                                       {}, 'readFrame', 'frameNumber or frameTime');
                end

            else

                % Default builtin call if optional arguments not provided.
                if this.UserCurrentTimeFlag
                    % If CurrentTime is set by user, use it to retrieve
                    % point cloud.
                    durationToSeekSeconds = double(seconds(this.CurrentTimeInternal - this.StartTime));

                    [xyziPoints, intensity, currentTimestamp, ...
                     rangeData, notNaNIndices, numValidPoints, ptTimestamps] = readPointCloud(...
                        this.VelodyneROSMessageReaderObj, this.VelodyneMessages, durationToSeekSeconds);
                else
                    % If user does not provide CurrentTime, read next point
                    % cloud in sequence.
                    if hasFrame(this)
                        [xyziPoints, intensity, currentTimestamp, ...
                         rangeData, notNaNIndices, numValidPoints, ptTimestamps] = readPointCloud(...
                            this.VelodyneROSMessageReaderObj, this.VelodyneMessages, int32(-1));
                    else
                        error(message('ros:mlros:velodyne:endOfMessage'));
                    end
                end
            end

            numRows = size(xyziPoints,1);
            numCols = size(xyziPoints,2);


            if this.OrganizePoints
                % For organized point cloud output

                % In the builtin implementation, xyziPoints and other
                % matrices is not initialized with NaN, for better
                % performance. Hence once the points are optained, the
                % indices that are not assigned any value, is assigned with
                % the NaN values.

                % Calculate the indices to be assigned with NaN value
                nanIndices = find(~notNaNIndices);
                nanIndicesAllDims = [nanIndices; nanIndices+(numRows*numCols);...
                                     nanIndices+(numRows*numCols*2)];
                xyziPoints(nanIndicesAllDims) = nan('like', xyziPoints);
                rangeData(nanIndicesAllDims) = nan('like', rangeData);
                intensity(nanIndices) = zeros('like', intensity);
                ptTimestamps(nanIndices) = nan('like', ptTimestamps);
            else
                % For unorganized point cloud output

                % In the builtin implementation, xyziPoints and other
                % matrices are of size equal to the number of points. This
                % is done to maintain a common implementation b/w organized
                % and unorganized output, at the same time pre allocate
                % memory for these matrices. Once we get the output here,
                % only the valid points are obtained.

                % Get indices for valid points
                validIndices = 1:numValidPoints;

                % Get only valid points
                xyziPoints = squeeze(cat(3, xyziPoints(validIndices),...
                                         xyziPoints(validIndices+(numRows*numCols)),...
                                         xyziPoints(validIndices+(numRows*numCols*2))));
                intensity = intensity(validIndices);
                if size(intensity,2) ~= 1
                    intensity = intensity';
                end
                rangeData = squeeze(cat(3, rangeData(validIndices),...
                                        rangeData(validIndices+(numRows*numCols)),...
                                        rangeData(validIndices+(numRows*numCols*2))));
                ptTimestamps = ptTimestamps(validIndices)';
            end

            % Create pointCloud object from xyziPoints, intensity and
            % rangeData
            [ptCloudObj, ptTimestamps] = makePointCloud(this, xyziPoints, intensity, rangeData, ptTimestamps);
            pointTimestamps = seconds(ptTimestamps);
            % Update current time
            this.CurrentTimeInternal = this.StartTime + seconds(currentTimestamp);

            this.UserCurrentTimeFlag = false;
        end

        function flag = hasFrame(this)
            flag = abs(seconds(this.EndTime) - seconds(this.CurrentTimeInternal)) >= 1e-6;
        end

        function reset(this)
            this.CurrentTime = this.StartTime;
        end
    end

    methods (Hidden)
        function delete(this)
            close(this.VelodyneROSMessageReaderObj);
            % Invalidate class properties
            this.VelodyneMessages     = [];
            this.CalibrationFile      = [];
        end
    end

    methods(Access = 'protected')
        function copyObj = copyElement(this)
            if(~isempty(this.CalibrationFile))
                copyObj = velodyneROSMessageReader(this.VelodyneMessages, ...
                                                   this.DeviceModel, 'CalibrationFile', this.CalibrationFile, ...
                                                   'OrganizePoints', this.OrganizePoints);
            else
                copyObj = velodyneROSMessageReader(this.VelodyneMessages, this.DeviceModel, ...
                                                   'OrganizePoints', this.OrganizePoints);
            end
            copyObj.CurrentTime = this.CurrentTime;
        end
    end

    methods(Hidden)
        function s = saveobj(this)
            s.VelodyneMessages = this.VelodyneMessages;
            s.DeviceModel      = this.DeviceModel;
            s.CalibrationFile  = this.CalibrationFile;
            s.OrganizePoints   = this.OrganizePoints;
            s.Version          = this.Version;
            s.CurrentTime      = this.CurrentTime;
        end
    end

    methods (Static, Hidden)
        function this = loadobj(s)

            if ~isfield(s, 'OrganizePoints')
                organizePoints = true;
            else
                organizePoints = s.OrganizePoints;
            end

            if(~isempty(s.CalibrationFile))
                this = velodyneROSMessageReader(s.VelodyneMessages, s.DeviceModel, ...
                                                'CalibrationFile', s.CalibrationFile, ...
                                                'OrganizePoints', organizePoints);
            else
                this = velodyneROSMessageReader(s.VelodyneMessages, s.DeviceModel, ...
                                                'OrganizePoints', organizePoints);
            end

            if s.CurrentTime < this.StartTime
                this.CurrentTime = this.StartTime;
            else
                this.CurrentTime = s.CurrentTime;
            end
        end
    end

    methods (Static)
        function validDeviceModels = validDeviceModels()
            validDeviceModels = {'VLP16', 'VLP32C', 'HDL32E', 'HDL64E'};
        end
    end

    methods(Access = 'private')
        function [ptCloudObj, ptTimestamps]  = makePointCloud(this, xyziPoints, intensity, rangeData, ptTimestamps)
            persistent sortedVerticalAngleIndices;

            if this.OrganizePoints
                if(isempty(sortedVerticalAngleIndices) || length(sortedVerticalAngleIndices)~= size(this.LaserCalibrationData,1))
                    % Laser vertical angles are stored in 2nd column of
                    % LaserCalibrationData.
                    laserVerticalAngles = this.LaserCalibrationData(:, 2);

                    % Sort the vertical angles and obtain indices to sorted
                    % vertical angles.
                    [~, sortedVerticalAngleIndices] = sort(laserVerticalAngles, 'descend');
                end

                xyziPoints = xyziPoints(sortedVerticalAngleIndices, :, :);
                intensity = intensity(sortedVerticalAngleIndices, :);
                rangeData = rangeData(sortedVerticalAngleIndices, :, :);
                ptTimestamps = ptTimestamps(sortedVerticalAngleIndices, :);
            end


            % Create pointCloud object from xyziPoints, with points
            % sorted according to the laser vertical angles.
            ptCloudObj  = pointCloud(xyziPoints, ...
                                     'Intensity', intensity);

            % range, pitch, yaw
            ptCloudObj.RangeData = rangeData;
        end
    end

    methods (Access = ?matlab.unittest.TestCase)

        function laserVerticalAngles = getVerticalAngles(this)
        % Return laser vertical angles from the calibration file

        % Laser vertical angles are stored in 2nd column of
        % LaserCalibrationData.
            laserVerticalAngles = this.LaserCalibrationData(:, 2);

            % Sort the vertical angles
            laserVerticalAngles = deg2rad(sort(laserVerticalAngles, 'descend'));
        end

        function verticalOffsetCorr = getVerticalOffsetCorr(this)
        % Return vertical offset correction for lasers from the calibration
        % file. Note that the values are in centimeters, in order to use
        % this you might have to convert it to meters as the points are in
        % those units.

            laserVerticalAngles = this.LaserCalibrationData(:, 2);

            % Sort the vertical angles and obtain indices to sorted
            % vertical angles.
            [~, sortedVerticalAngleIndices] = sort(laserVerticalAngles, 'descend');


            % Laser vertical offset corrections are stored in 6th column of
            % LaserCalibrationData.
            verticalOffsetCorr = this.LaserCalibrationData(sortedVerticalAngleIndices, 6);
        end
    end
end

% Input parsing and validation
function paramsStruct = iParseAndValidateInputs(velodyneMessages, deviceModel, varargin)

% Validate velodyneMessages
    validateattributes(velodyneMessages,{'cell','struct','ros.msggen.velodyne_msgs.VelodyneScan'},{'nonempty'},'velodyneROSMessageReader','velodyneMessages');

    % Validate device model
    deviceModel = iValidateDeviceModel(deviceModel);

    % Parse name-value inputs
    p = inputParser;
    p.FunctionName = 'velodyneROSMessageReader';

    defaultCalibrationFile = iGetDefaultCalibrationFile(deviceModel);
    addParameter(p, 'CalibrationFile', defaultCalibrationFile);
    addParameter(p, 'SkipPartialFrames', true);    % Note that this parameter is undocumented
    addParameter(p, 'OrganizePoints', true, @(x)iValidateOrganizePoints(x));

    parse(p, varargin{:});

    paramsStruct = p.Results;

    % Check that calibration file exists
    paramsStruct.CalibrationFile   = iValidateCalibrationFile(paramsStruct.CalibrationFile);
    paramsStruct.SkipPartialFrames = iValidateSkipPartialFrames(paramsStruct.SkipPartialFrames);
    paramsStruct.DeviceModel    = deviceModel;
    % Covert VelodyneMessages to cell array with all structs for each cell
    if isa(velodyneMessages, 'ros.msggen.velodyne_msgs.VelodyneScan')
        paramsStruct.VelodyneMessages = arrayfun(@(x) {toStruct(x)}, velodyneMessages);
    elseif isstruct(velodyneMessages)
        paramsStruct.VelodyneMessages = arrayfun(@(x) {x}, velodyneMessages);
    elseif isa(velodyneMessages{1,1}, 'ros.msggen.velodyne_msgs.VelodyneScan')
        paramsStruct.VelodyneMessages = cellfun(@(x) {toStruct(x)}, velodyneMessages);
    else
        paramsStruct.VelodyneMessages = velodyneMessages;
    end

    % Check that one element in VelodyneMessages contains Packets field,
    % and that there is a Data field in Packets to make sure the input
    % message is valid.
    if (~isfield(paramsStruct.VelodyneMessages{1,1},'Packets'))
        error(message('ros:mlros:velodyne:invalidArgTypeForROSMessage'));
    end
    if (isempty(paramsStruct.VelodyneMessages{1,1}.Packets))
        error(message('ros:mlros:velodyne:emptyROSMessage'));
    end
    if (~isfield(paramsStruct.VelodyneMessages{1,1}.Packets,'Data'))
        error(message('ros:mlros:velodyne:invalidArgTypeForROSMessage'));
    end
end

function deviceModel = iValidateDeviceModel(deviceModel)

    validDeviceModels = velodyneROSMessageReader.validDeviceModels;
    deviceModel = validatestring(deviceModel, validDeviceModels, ...
                                 'velodyneROSMessageReader', 'deviceModel');
end

function defaultCalibrationFile = iGetDefaultCalibrationFile(deviceModel)

    defaultCalibrationFile = fullfile(matlabroot, 'toolbox', 'shared', ...
                                      'pointclouds', 'utilities', 'velodyneFileReaderConfiguration', ...
                                      [char(deviceModel) '.xml']);
end

function calibrationFile = iValidateCalibrationFile(calibrationFile)

    validateattributes(calibrationFile, {'char', 'string'}, ...
                       {'nonempty', 'scalartext'}, 'velodyneROSMessageReader', 'CalibrationFile');

    calibrationFile = iCheckFile(calibrationFile, '.xml', 'Calibration XML');
end

function skipPartialFrames = iValidateSkipPartialFrames(skipPartialFrames)

    validateattributes(skipPartialFrames, {'logical', 'numeric'}, {'binary'}, ...
                       'velodyneROSMessageReader', 'SkipPartialFrames');
end

function iValidateOrganizePoints(organizePoints)

    validateattributes(organizePoints, {'logical'},{'scalar'}, ...
                       'velodyneROSMessageReader', 'OrganizePoints');
end

function [laserCalibrationData, distanceResolution] = iRetrieveCalibrationData(calibrationFile, deviceModel)

% Read Calibration data from XML file.
    [laserCalibrationData, distanceResolution]  = iGetVelodyneCorrectionsFromXML(calibrationFile);

    % Number of lasers based on Velodyne device model.
    if strcmpi('HDL32E', deviceModel) || strcmpi('VLP32C', deviceModel)
        numLasersOfDeviceModel = 32;
    elseif strcmpi('HDL64E', deviceModel)
        numLasersOfDeviceModel = 64;
    elseif strcmpi('VLS128', deviceModel)
        numLasersOfDeviceModel = 128;
    else
        numLasersOfDeviceModel = 16;
    end

    % Validate laser count in calibration data with number of
    % lasers for given device model.
    if size(laserCalibrationData, 1) ~= numLasersOfDeviceModel
        error(message(...
            'ros:mlros:velodyne:invalidCalibrationFileIncorrectEnabledLaserCountForDeviceModel', ...
            deviceModel, numLasersOfDeviceModel));
    end
end

function fileName = iCheckFile(fileName, expectedExtension, fileType)

% Check the file extension
    [~,~,fileExtension] = fileparts(fileName);

    if ~strcmpi(fileExtension, expectedExtension)
        error(message('ros:mlros:velodyne:invalidFileType', fileType, ...
                      expectedExtension, fileExtension));
    end

    % Check if the file exists
    [fid, msg] = fopen(fileName, 'r');

    if fid == -1
        error(message('ros:mlros:velodyne:fileOpenFailed', fileType, ...
                      fileName, msg));
    else
        % Get full path name of file
        fileName = fopen(fid);
        fclose(fid);
    end
end

% helper function to load laser calibration data from XML file
function [laserCorrections, distLSB] = iGetVelodyneCorrectionsFromXML(xmlFile)

% Parse the provided Velodyne laser calibration XML file.
    try
        xmlParser = matlab.io.xml.dom.Parser;
        xmlParser.Configuration.AllowDoctype = true;
        domNode = xmlParser.parseFile(xmlFile);
    catch ex
        newEx = MException(message('ros:mlros:velodyne:calibrationFileReadError'));
        throw(newEx.addCause(ex))
    end
    % Get distLSB_ value
    distLSB = single(0.2);
    distLSBNode = domNode.getElementsByTagName('distLSB_');
    isDistLSBTagFound = distLSBNode.getLength() > 0;
    if(isDistLSBTagFound)
        distLSB = single(iGetTagValue(domNode,'distLSB_'));
    end

    % Count number of lasers enabled
    enabled = domNode.getElementsByTagName('enabled_');
    isEnabledTagMissing = enabled.getLength() <= 0;
    if(isEnabledTagMissing)
        error(message('ros:mlros:velodyne:invalidCalibrationFileTagNotFound', ...
                      'enabled_'));
    end
    enabledCount = 0;
    for k = 0 : enabled.getLength-1
        enabledItem   = enabled.item(k);
        enabledItemId = enabledItem.getElementsByTagName('item');
        for i= 0 : enabledItemId.getLength()-1
            if(str2double(enabledItemId.item(i).getTextContent) > 0)
                enabledCount = enabledCount+1;
            end
        end
    end

    isValidLaserCount = (enabledCount == 16 || enabledCount == 32 || enabledCount == 64 || enabledCount == 128);
    if(enabledCount <= 0 || ~isValidLaserCount)
        error(message('ros:mlros:velodyne:invalidCalibrationFileIncorrectEnabledLaserCount'));
    end
    % Read laser corrections
    px = domNode.getElementsByTagName('px');

    isPxTagMissing = (px.getLength() <= 0);
    if(isPxTagMissing)
        error(message('ros:mlros:velodyne:invalidCalibrationFileTagNotFound', 'px'));
    end
    isPxTagCountLessThanEnabledCount = (px.getLength() < enabledCount);
    if(isPxTagCountLessThanEnabledCount)
        error(message('ros:mlros:velodyne:invalidCalibrationFileMissingCalibrationValues'));
    end
    laserCorrections = zeros(enabledCount, 9, 'single');
    % Read the following tag values into laserCorrections array.
    %   <rotCorrection_>-5.3328056</rotCorrection_>
    %   <vertCorrection_>-7.2988362</vertCorrection_>
    %   <distCorrection_>111</distCorrection_>
    %   <distCorrectionX_>118</distCorrectionX_>
    %   <distCorrectionY_>118</distCorrectionY_>
    %   <vertOffsetCorrection_>19.736338</vertOffsetCorrection_>
    %   <horizOffsetCorrection_>2.5999999</horizOffsetCorrection_>
    %   <focalDistance_>0</focalDistance_>
    %   <focalSlope_>0</focalSlope_>

    for k = 0 : enabledCount-1
        pxItem = px.item(k);
        if(pxItem.getLength() <= 0)
            error(message('ros:mlros:velodyne:invalidCalibrationFileTagNotFound', 'px'));
        end
        laserCorrections(k+1, 1) = iGetTagValue(pxItem, 'rotCorrection_');
        laserCorrections(k+1, 2) = iGetTagValue(pxItem, 'vertCorrection_');
        laserCorrections(k+1, 3) = iGetTagValue(pxItem, 'distCorrection_');
        laserCorrections(k+1, 4) = iGetTagValue(pxItem, 'distCorrectionX_');
        laserCorrections(k+1, 5) = iGetTagValue(pxItem, 'distCorrectionY_');
        laserCorrections(k+1, 6) = iGetTagValue(pxItem, 'vertOffsetCorrection_');
        laserCorrections(k+1, 7) = iGetTagValue(pxItem, 'horizOffsetCorrection_');
        laserCorrections(k+1, 8) = iGetTagValue(pxItem, 'focalDistance_');
        laserCorrections(k+1, 9) = iGetTagValue(pxItem, 'focalSlope_');
    end
end

% helper function to parse a numeric tag value for a given XML tag
function tagValue = iGetTagValue(pxItem, tagName)
% Utility function to parse, validate and return tag
% value for a given tag in XML file.
    pxItemId = pxItem.getElementsByTagName(tagName);
    if(pxItemId.getLength() > 0)
        tagValue = str2double(pxItemId.item(0).getTextContent);
        if(isnan(tagValue))
            error(message('ros:mlros:velodyne:invalidCalibrationFileNonnumericTagValue', tagName));
        end
    else
        error(message('ros:mlros:velodyne:invalidCalibrationFileTagNotFound', tagName));
    end

end

% Copyright 2022 The MathWorks, Inc.
