classdef uavLidarPointCloudGenerator < matlab.System
%uavLidarPointCloudGenerator Generate point cloud from meshes
%   LIDAR = uavLidarPointCloudGenerator returns a statistical sensor
%   model to generate point cloud for a lidar.
%
%   LIDAR = uavLidarPointCloudGenerator(___, 'Name', value) returns a
%   uavLidarPointCloudGenerator object by specifying its properties as
%   name-value pair arguments. Unspecified properties have default
%   values. See the list of properties below.
%
%   uavLidarPointCloudGenerator properties:
%     UpdateRate                - Sensor update frequency
%     MaxRange                  - Maximum detection range
%     RangeAccuracy             - Accuracy of range measurements (m)
%     AzimuthResolution         - Azimuthal resolution of lidar (deg)
%     ElevationResolution       - Elevation resolution of lidar (deg)
%     AzimuthLimits             - Azimuthal limits of lidar (deg)
%     ElevationLimits           - Elevation limits of lidar (deg)
%     HasNoise                  - Add noise to measurements
%     HasOrganizedOutput        - Output organized point cloud locations
%
%   To generate point clouds, call the lidar object using the following
%   syntax:
%
%   PTCLOUD = lidar(TGTS ,SIMTIME) generates a lidar point cloud from
%   the N-element array of structs, TGTS, at the current simulation
%   time, SIMTIME. SIMTIME is a scalar value in seconds. PTCLOUD is a
%   pointCloud object that stores a 3-D point cloud. The lidar
%   generates point clouds at the rate defined by the UpdateRate
%   property.
%
%   The fields that can be specified in the TGTS struct are listed
%   below:
%
%       Position       A 3-element vector defining the xyz-position of
%                      the target with respect to the sensor frame.
%
%       Orientation    A quaternion object or a 3-by-3 matrix defining
%                      the orientation of the target with respect to
%                      the sensor frame.
%
%       Mesh           An extendedObjectMesh object representing the
%                      geometry of the target in its own coordinate
%                      frame.
%
%   The Location property of the returned pointCloud object is reported
%   in the sensor frame, where positive x points forward, positive y
%   points to the left, and positive z points upward.
%
%
%   [...,ISVALIDTIME] = lidar(...) optionally returns ISVALIDTIME.
%
%   ISVALIDTIME indicates if the simulation time is a multiple of the
%   sensor's update interval, i.e. 1/UpdateRate.
%
%   Point clouds can only be reported by the lidarPointCloudGenerator
%   at rate specified by the UpdateRate property. The ISVALIDTIME flag
%   is set to false when detection updates are requested at times that
%   are not aligned with the configured update rate.
%
%   Example:
%
%      % Create a default lidar sensor
%      lidar = uavLidarPointCloudGenerator("HasOrganizedOutput", false);
%
%      % Create a floor
%      tgts.Mesh = scale(extendedObjectMesh('cuboid'), [100,100,2]);
%      % setup floor's relative position and orientation to the lidar
%      tgts.Position = [0 0 -10];
%      tgts.Orientation = quaternion([1 0 0 0]);
%
%      % Generate point cloud from floor
%      ptcloud = lidar(tgts, 0);
%
%      % Visualize the point cloud and mesh
%      figure;
%      show(translate(tgts.Mesh, tgts.Position));
%      hold on
%      scatter3(ptcloud.Location(:,1), ptcloud.Location(:,2), ...
%      ptcloud.Location(:,3));

%   Copyright 2020-2023 The MathWorks, Inc.

    properties (Dependent, Nontunable)
        %UpdateRate Update rate of the lidar sensor (Hz)
        %   The update rate is specified as a scalar in Hz. It determines
        %   how frequent lidar point clouds are generated
        %
        %   Default: 10
        UpdateRate

        %MaxRange Maximum detection range
        %   Specify the maximum detection range of the sensor using a
        %   positive scalar value.
        %
        %   Default: 120
        MaxRange

        %RangeAccuracy Accuracy of range measurements (m)
        %   Specify the one standard deviation (sigma) accuracy of the
        %   sensor range measurements.
        %
        %   Default: 0.002
        RangeAccuracy

        %AzimuthResolution Azimuthal resolution of lidar (deg)
        %  Specify the resolution of the lidar sensor in azimuth as a
        %   positive scalar. The sensor's azimuth resolution defines the
        %   number of points per elevation channel of the lidar
        %
        %   Default: 0.16
        AzimuthResolution

        %ElevationResolution Elevation resolution of lidar (deg)
        %   Specify the resolution of the lidar sensor in elevation as a
        %   positive scalar. The sensor's elevation resolution defines the
        %   number of elevation channels.
        %
        %   Default: 1.25
        ElevationResolution

        %AzimuthLimits Azimuthal limits of lidar (deg)
        %   Specify the field of view of the sensor in azimuth using a
        %   2-element vector, [minAz maxAz].
        %
        %   Default: [-180 180]
        AzimuthLimits

        %ElevationLimits Elevation limits of lidar (deg)
        %   Specify the field of view of the sensor in elevation using a
        %   2-element vector, [minEl maxEl].
        %
        %   Default: [-20 20]
        ElevationLimits

        %HasNoise Add noise to measurements
        %   Set this property to true to add noise to the point cloud
        %   locations. The sensor adds random Gaussian noise to each point
        %   using RangeAccuracy property as one standard deviation
        %
        %   Default: true
        HasNoise

        %HasOrganizedOutput Output organized point cloud locations
        %   Set this property to true to output point cloud locations
        %   organized in elevation and azimuth channels. When specified
        %   as true, the Location property of the point cloud is a
        %   N-by-M-by-3 matrix, where N is the number of elevation channels
        %   and M is the number of azimuth channels. When specified as false,
        %   the Location property of the point cloud is a N-by-3 matrix,
        %   where N is the product of number of azimuth and elevation
        %   channels.
        %
        %   Default: true
        HasOrganizedOutput
    end

    properties (Access = {?uavLidarPointCloudGenerator, ...
            ?uav.internal.scenario.sensor.SensorAdaptor, ...
            ?uav.internal.scenario.sensor.PointCloudGeneratorAdaptor, ...
            ?matlab.unittest.TestCase})
        %SensorImpl Implementation of the point cloud generator
        SensorImpl
    end

    methods
        function obj = uavLidarPointCloudGenerator(varargin)
        % Support name-value pair arguments when constructing object
            obj.SensorImpl = uav.internal.sensor.LidarPointCloudGenerator(varargin{:});
        end

        function set.UpdateRate(obj, r)
            obj.SensorImpl.UpdateRate = r;
        end

        function r = get.UpdateRate(obj)
            r = obj.SensorImpl.UpdateRate;
        end

        function set.MaxRange(obj, r)
            obj.SensorImpl.MaxRange = r;
        end

        function r = get.MaxRange(obj)
            r = obj.SensorImpl.MaxRange;
        end

        function set.RangeAccuracy(obj, r)
            obj.SensorImpl.RangeAccuracy = r;
        end

        function r = get.RangeAccuracy(obj)
            r = obj.SensorImpl.RangeAccuracy;
        end

        function set.AzimuthResolution(obj, r)
            obj.SensorImpl.AzimuthResolution = r;
        end

        function r = get.AzimuthResolution(obj)
            r = obj.SensorImpl.AzimuthResolution;
        end

        function set.ElevationResolution(obj, r)
            obj.SensorImpl.ElevationResolution = r;
        end

        function r = get.ElevationResolution(obj)
            r = obj.SensorImpl.ElevationResolution;
        end

        function set.AzimuthLimits(obj, r)
            obj.SensorImpl.AzimuthLimits = r;
        end

        function r = get.AzimuthLimits(obj)
            r = obj.SensorImpl.AzimuthLimits;
        end
        function set.ElevationLimits(obj, r)
            obj.SensorImpl.ElevationLimits = r;
        end

        function r = get.ElevationLimits(obj)
            r = obj.SensorImpl.ElevationLimits;
        end

        function set.HasNoise(obj, r)
            obj.SensorImpl.HasNoise = r;
        end

        function r = get.HasNoise(obj)
            r = obj.SensorImpl.HasNoise;
        end

        function set.HasOrganizedOutput(obj, r)
            obj.SensorImpl.HasOrganizedOutput = r;
        end

        function r = get.HasOrganizedOutput(obj)
            r = obj.SensorImpl.HasOrganizedOutput;
        end

    end

    methods(Access = protected)

        function setupImpl(obj, varargin)
        % Perform one-time calculations, such as computing constants
        
            if obj.SensorImpl.ConnectedToScenario
                obj.SensorImpl.setup(struct.empty, obj.SensorImpl.SensorSim.CurrentSimTime);
            else
                obj.SensorImpl.setup(varargin{:});
            end
        end

        function cobj = cloneImpl(obj)
        % Override system.object clone behavior

            cobj = cloneImpl@matlab.System(obj);
            % clone SensorImpl
            cobj.SensorImpl = clone(obj.SensorImpl);
        end

        function [pt, isvalidtime] = stepImpl(obj, varargin)
        % Implement algorithm. Calculate y as a function of input u and
        % discrete states.

            if obj.SensorImpl.ConnectedToScenario
                narginchk(1,1);
                tgts = struct.empty;
                time = obj.SensorImpl.SensorSim.CurrentSimTime;
            else
                narginchk(3,3);
                tgts = varargin{1};
                time = varargin{2};
            end
            [pt, config] = obj.SensorImpl(tgts, time);
            isvalidtime = config.IsValidTime;
        end

        function resetImpl(obj)
        % Initialize / reset discrete-state properties

            obj.SensorImpl.reset();
        end

        function releaseImpl(obj)
        % Release resources, such as file handles

            obj.SensorImpl.release();
        end

        function loadObjectImpl(obj,s,wasLocked)
        %saveObjectImpl overrides default load behavior
        %   Set properties in object obj to values in structure s

        % Set private properties and states
            if isfield(s, 'SensorImpl')
                obj.SensorImpl = s.SensorImpl;
            else
                coder.internal.warningIf(true, 'uav:robotuav:uavlidar:MissingDataInMatFile');
            end

            % Set public properties and states
            loadObjectImpl@matlab.System(obj,s,wasLocked);
        end

        function s = saveObjectImpl(obj)
        %saveObjectImpl overrides default save behavior
        %   Set properties in structure s to values in object obj, then
        %   save s

        % Set public properties and states
            s = saveObjectImpl@matlab.System(obj);
            % Set private properties and states
            s.SensorImpl = obj.SensorImpl;
        end
    end

end
