classdef controllerVFH3D < uav.internal.follower.ObstacleAvoidanceBase
    %controllerVFH3D Compute obstacle-free direction using range sensor data
    %   The vector field histogram 3D (VFH3D) algorithm is used for
    %   obstacle avoidance based on sensor data. Given sensor data in terms of
    %   obstacle 3D position, UAV position, UAV orientation, and a target
    %   position, the controllerVFH3D computes an obstacle-free direction using
    %   the VFH3D algorithm.
    %
    %   VFH3D = controllerVFH3D returns a vector field histogram 3D
    %   object, VFH3D, that computes a desired direction, desired
    %   yaw direction and a status output using the VFH3D algorithm.
    %
    %   VFH3D = controllerVFH3D('PropertyName', PropertyValue, ...)
    %   returns a vector field histogram 3D object, VFH3D, with each
    %   specified property set to the specified value.
    %
    %   Step method syntax:
    %
    %   [DESIREDDIRECTION, DESIREDYAW, STATUS]  = step(VFH3D, POSITION,
    %   ORIENTATION, SENSORPOINTS, TARGETPOSITION) finds a 3-by-1 obstacle
    %   free direction DESIREDDIRECTION, scalar output DESIREDYAW, and
    %   uint8 output STATUS using the VFH3D algorithm for a 3-by-1 input
    %   UAV position, POSITION, 4-by-1 input UAV orientation expressed as
    %   a quaternion, ORIENTATION, N-by-3 3D sensor points, SENSORPOINTS,
    %   where N is the number of sensor points and input 3-by-1 target
    %   position, TARGETPOSITION.
    %
    %   System objects may be called directly like a function instead of using
    %   the step method. For example, y = step(obj, x) and y = obj(x) are
    %   equivalent.
    %
    %   controllerVFH3D methods:
    %
    %   step        - Compute desired direction, desired yaw and status
    %   clone       - Create controllerVFH3D object with same property values
    %   show        - Display controllerVFH3D information in a figure window
    %
    %   controllerVFH3D properties:
    %
    %   HistogramResolution     - Histogram grid resolution
    %   WindowSize              - Histogram window size
    %   HistogramThreshold      - Threshold for computing histogram
    %   MaxAge                  - Maximum age of remembered obstacle points
    %   DistanceLimits          - Limits of sensor range
    %   HorizontalSensorFOV     - Horizontal field of view limits of sensor
    %   VerticalSensorFOV       - Vertical field of view limits of sensor
    %   SensorLocation          - Sensor mounting location on UAV
    %   SensorOrientation       - Orientation of sensor mounted on UAV
    %   VehicleRadius           - Radius of UAV
    %   SafetyDistance          - Safety distance around UAV to obstacle
    %   TargetDirectionWeight   - Cost function weight for moving in target
    %                             direction
    %   CurrentDirectionWeight  - Cost function weight for moving in current
    %                             direction
    %   PreviousDirectionWeight - Cost function weight for moving in previous
    %                             direction
    %
    %   Example:
    %
    %     % Create controllerVFH3D object
    %     vfh3D = controllerVFH3D;
    %
    %     % Create obstacles
    %     az = [-60:-20,20:60]*pi/180;
    %     el = (-30:30)*pi/180;
    %     [El,Az] = meshgrid(el,az);
    %
    %     % Specify obstacle distance
    %     obstacleDist = linspace(15,20,numel(El(:)));
    %     [xSensor, ySensor, zSensor] = sph2cart(Az(:), El(:),obstacleDist');
    %
    %     % Align sensor and histogram frame
    %     vfh3D.SensorOrientation = [-180 0 0];
    %
    %     % Specify sensor range limits
    %     vfh3D.DistanceLimits = [0.2 25];
    %
    %     % Specify inputs
    %     uavPosition = [0;0;0];
    %     uavOrientation = [1;0;0;0];
    %     sensorPoints = [xSensor, ySensor, zSensor];
    %     targetPosition = [20;0;0];
    %
    %     % Compute obstacle free direction, desired yaw and status outputs
    %     [desiredDirection, desiredYaw, status] = vfh3D(uavPosition, ...
    %     uavOrientation, sensorPoints, targetPosition);
    %     % Visualize default histogram using show
    %     show(vfh3D);
    %
    %     % Specify axes and plots to show using Name-Value pair
    %     fh = figure;
    %     ax(1) = subplot(1,2,1);
    %     ax(2) = subplot(1,2,2);
    %     show(vfh3D, Parent = ax, PlotsToShow = ["2D Inflated Histogram",...
    %     "Cost Matrix"]);
    %
    %   See also uavScenario, uavMission

    %   References:
    %
    %   [1] Tanja Baumann., "Obstacle Avoidance for Drones Using a 3DVFH*
    %       Algorithm", Master Thesis, ETH, 2018.


    % Copyright 2022-2023 The MathWorks, Inc.

    %#codegen

    properties(Nontunable)
        %HistogramResolution Specify angular resolution of histogram
        HistogramResolution = 5
    end

    properties(Constant,Hidden)
        %DefaultPlot Default plot when show method is called
        DefaultPlot = "3D Histogram"

        %ExpectedPlots Plot selection options
        ExpectedPlots = ["3D Histogram","2D Memory Histogram", "2D Inflated Histogram", "Cost Matrix", "All"];

    end

    properties (Constant, Hidden)

        % Used as hexadecimal for setting 'Color' property. Semantic values
        % can't be used for 'Color'
        ColorWhite = "--mw-color-tertiary"; % White
        ColorBlack = "--mw-color-primary"; % Black

        % Axes colors
        ColorRed = robotics.utils.internal.ThemeColorUtil.Red; % Red
        ColorGreen = robotics.utils.internal.ThemeColorUtil.Green; % Green
        ColorBlue = robotics.utils.internal.ThemeColorUtil.Blue; % Blue

        % Color for desired cell. Close to green.
        ColorDesiredCell = "--mw-graphics-colorSpace-rgb-green"; % Light Green
        % Color used for magenta. Color for target cell. Close to magenta
        ColorTargetCell = "--mw-color-matlabCharacters"; % Purple

        % Color of target and desired cell edge
        ColorTargetEdge = "--mw-color-tertiary"; % White
        ColorDesiredEdge = "--mw-color-tertiary"; % White

        % Grid color
        ColorGrey = "--mw-borderColor-primary"; % Grey

        % Line Width
        LineWidth = 1.25;  
 

    end

    properties(Access = {?controllerVFH3D, ?matlab.unittest.TestCase})
        %IsShowBeforeStep Flag to prevent calls to show before step
        IsShowBeforeStep = true;

        %TargetPosition Target position for show method
        TargetPosition

        %Position UAV [x y z] position
        Position

        %Orientation UAV orientation in quaternion
        Orientation

        %DesiredDirection Obstacle free desired direction needed for
        %plotting
        DesiredDirection

        %PrevDirection Previous desired direction
        PrevDirection
    end

    methods
        function set.HistogramResolution(obj, histRes)
            %set.HistogramResolution Setter for histogram resolution property

            validateattributes(histRes, {'double'},...
                {'real','finite','positive'}, ...
                'controllerVFH3D', 'Histogram resolution');

            % Valid histogram resolution values
            isValidHistRes = any(obj.ValidHistRes == histRes);
            coder.internal.errorIf(~isValidHistRes, ...
                'uav:robotuav:obstacleavoidance:ValidHistogramResolutionValues');
            obj.HistogramResolution = histRes;
        end

        function obj = controllerVFH3D(varargin)
            %controllerVFH3D Constructor
            setProperties(obj,nargin,varargin{:});
        end
    end

    methods (Access = protected)

        function setupImpl(obj,uavPosition)
            % Perform one-time calculations, such as computing constants

            obj.HistRes = num2str(obj.HistogramResolution);
            obj.setupInternal(uavPosition);
            obj.IsShowBeforeStep = false;
            obj.DesiredDirection = cast([1 0 0]','like',uavPosition);
            obj.PrevDirection = obj.DesiredDirection;

        end

        function [desiredDirection,desiredYaw,status] = stepImpl(obj,uavPosition,uavOrientation,sensorPoints,targetPosition)

            % Validate inputs
            validateattributes(uavPosition, {'double', 'single'}, ...
                {'nonempty', 'nonnan', 'finite', 'real', 'vector','nrows', 3}, ...
                'step','position');

            validateattributes(uavOrientation, {'double', 'single'}, ...
                {'nonempty', 'nonnan', 'finite', 'real', 'vector','nrows', 4}, ...
                'step','orientation');

            validateattributes(sensorPoints,{'double', 'single'},{'real','ncols',3, ...
                'nonempty'},'step','sensor points');

            validateattributes(targetPosition, {'double', 'single'}, ...
                {'nonempty', 'nonnan', 'finite', 'real', 'vector','nrows', 3}, ...
                'step','target position');

            % Data type of position, orientation and target position must
            % be same
            isDataTypeEqual = isequal(class(uavPosition), class(uavOrientation), class(targetPosition));
            coder.internal.errorIf(~isDataTypeEqual, ...
                'uav:robotuav:obstacleavoidance:DataTypeMismatch');

            % Store previous desired direction
            obj.PrevDirection = obj.DesiredDirection;
            % Compute the outputs
            [desiredDirection,desiredYaw,status] = stepInternal(obj,uavPosition,uavOrientation,sensorPoints,targetPosition);

            % Allow show method to be called
            obj.IsShowBeforeStep = false;

            obj.TargetPosition = targetPosition;
            obj.Position = uavPosition;
            obj.Orientation = uavOrientation;
            uavPose = [obj.Position;quat2eul(obj.Orientation','ZYX')'];
            % Desired direction in histogram frame
            obj.DesiredDirection = obj.globalToHistFrame(...
                [uavPose(1) + desiredDirection(1), ...
                uavPose(2) + desiredDirection(2), ...
                uavPose(3) + desiredDirection(3)], uavPose)';

        end

        function num = getNumInputsImpl(~)
            %getNumInputsImpl return number of inputs

            num = 4;

        end

        function num = getNumOutputsImpl(~)
            % Define total number of outputs for system with optional
            % outputs
            num = 3;

        end

        function loadObjectImpl(obj,s,wasLocked)
            % Set properties in object obj to values in structure s

            % Set private and protected properties
            % obj.myproperty = s.myproperty;

            % Set public properties and states
            obj.CombinedCounter = s.CombinedCounter;
            obj.DistanceLayer = s.DistanceLayer;
            obj.ValidRow = s.ValidRow;
            obj.ValidCol = s.ValidCol;

            obj.AngularResolution = s.AngularResolution;
            obj.ElevationRange = s.ElevationRange;
            obj.AzimuthRange = s.AzimuthRange;
            obj.NBinsElevation = s.NBinsElevation;
            obj.NBinsAzimuth = s.NBinsAzimuth;

            obj.OccupancyValue = s.OccupancyValue;

            obj.ElevationRangeFine = s.ElevationRangeFine;
            obj.AzimuthRangeFine = s.AzimuthRangeFine;
            obj.NBinsElevationFine = s.NBinsElevationFine;
            obj.NBinsAzimuthFine = s.NBinsAzimuthFine;

            obj.NumPointsFine = s.NumPointsFine;
            obj.OldPointsGlobal = s.OldPointsGlobal;

            obj.CombinedPointsGlobal = s.CombinedPointsGlobal;
            obj.PrevCandidatePoint = s.PrevCandidatePoint;
            obj.IndexMatrix = s.IndexMatrix;

            obj.IsShowBeforeStep = s.IsShowBeforeStep;
            obj.TargetPosition = s.TargetPosition;
            obj.Position = s.Position;
            obj.Orientation = s.Orientation;
            obj.DesiredDirection = s.DesiredDirection;
            obj.PrevDirection = s.PrevDirection;

            loadObjectImpl@matlab.System(obj,s,wasLocked);

        end

        function s = saveObjectImpl(obj)
            % Set properties in structure s to values in object obj

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

            s.CombinedCounter = obj.CombinedCounter;
            s.DistanceLayer = obj.DistanceLayer;
            s.ValidRow = obj.ValidRow;
            s.ValidCol = obj.ValidCol;

            s.AngularResolution = obj.AngularResolution;
            s.ElevationRange = obj.ElevationRange;
            s.AzimuthRange = obj.AzimuthRange;
            s.NBinsElevation = obj.NBinsElevation;
            s.NBinsAzimuth = obj.NBinsAzimuth;

            s.OccupancyValue = obj.OccupancyValue;

            s.ElevationRangeFine = obj.ElevationRangeFine;
            s.AzimuthRangeFine = obj.AzimuthRangeFine;
            s.NBinsElevationFine = obj.NBinsElevationFine;
            s.NBinsAzimuthFine = obj.NBinsAzimuthFine;

            s.NumPointsFine = obj.NumPointsFine;
            s.OldPointsGlobal = obj.OldPointsGlobal;

            s.CombinedPointsGlobal = obj.CombinedPointsGlobal;
            s.PrevCandidatePoint = obj.PrevCandidatePoint;
            s.IndexMatrix = obj.IndexMatrix;

            s.IsShowBeforeStep = obj.IsShowBeforeStep;
            s.TargetPosition = obj.TargetPosition;
            s.Position = obj.Position;
            s.Orientation = obj.Orientation;
            s.DesiredDirection = obj.DesiredDirection;
            s.PrevDirection = obj.PrevDirection;
        end

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

            obj.PrevDirection = cast([1 0 0]','like',obj.PrevDirection);
            obj.PrevCandidatePoint = cast([1 0 0]','like',obj.PrevCandidatePoint);
            obj.IsShowBeforeStep = true;
        end

        function flag = isInactivePropertyImpl(~,prop)
            % Return false if property is visible based on object
            % configuration, for the command line and System block dialog
            flag = false;
            if strcmp(prop,'HistRes')
                flag = true;
            end
        end
    end

    methods
        function ax = show(obj,varargin)
            %show Display 3-D vector field histograms in figure window
            %show(VFH3D) shows the histogram calculated by the 3DVFH+
            %   algorithm in a figure window. The figure also includes the
            %   parameters of the controllerVFH3D object from the last object
            %   call.
            %
            %   show(VFH3D, Name=Value) specifies options using one or more
            %   name-value arguments in addition to any combination of input
            %   arguments from previous syntaxes.
            %
            %   axHandle = show(___) returns the axes handles of all the
            %   specified histogram plots.
            %
            %   Name-Value arguments for the function:
            %
            %   Parent      - Axes used to plot histograms
            %                 Axes used to plot histograms, specified as an
            %                 axes object or array of axes objects.
            %
            %   PlotsToShow - Histogram plots to show in figure window
            %                 Histogram plots to show in figure window,
            %                 specified as a string scalar or string array
            %                 of available options. The options are any
            %                 combination of "3D Histogram", "2D Memory
            %                 Histogram", "2D Inflated Histogram", and
            %                 "Cost Matrix" or "All".
            %
            %                 Default: "3D Histogram"
            %
            %   Example:
            %
            %     % Create controllerVFH3D object
            %     vfh3D = controllerVFH3D;
            %
            %     % Create obstacles
            %     az = [-60:-20,20:60]*pi/180;
            %     el = (-30:30)*pi/180;
            %     [El,Az] = meshgrid(el,az);
            %
            %     % Specify obstacle distance
            %     obstacleDist = linspace(15,20,numel(El(:)));
            %     [xSensor, ySensor, zSensor] = sph2cart(Az(:), El(:),obstacleDist');
            %
            %     % Align sensor and histogram frame
            %     vfh3D.SensorOrientation = [-180 0 0];
            %
            %     % Specify sensor range limits
            %     vfh3D.DistanceLimits = [0.2 25];
            %
            %     % Specify inputs
            %     uavPosition = [0;0;0];
            %     uavOrientation = [1;0;0;0];
            %     sensorPoints = [xSensor, ySensor, zSensor];
            %     targetPosition = [20;0;0];
            %
            %     % Compute obstacle free direction, desired yaw and status outputs
            %     [desiredDirection, desiredYaw, status] = vfh3D(uavPosition,...
            %     uavOrientation, sensorPoints, targetPosition);
            %
            %     % Visualize default histogram using show
            %     show(vfh3D);
            %
            %     % Specify axes and plots to show using Name-Value pair
            %     fh = figure;
            %     ax(1) = subplot(1,2,1);
            %     ax(2) = subplot(1,2,2);
            %     show(vfh3D, Parent = ax, PlotsToShow = ["2D Inflated Histogram", ...
            %     "Cost Matrix"]);
            %
            %   See also uavScenario, uavMission

            if coder.target ('MATLAB')

                % If step has not been called then error out
                if obj.IsShowBeforeStep
                    error(message('uav:robotuav:obstacleavoidance:ShowBeforeStep'));
                end

                defaultPlot = obj.DefaultPlot;

                p = inputParser;

                addParameter(p,"Parent", [], ...
                    @(x)validateattributes(x, "matlab.graphics.axis.Axes", "vector", "show", "Parent"));

                addParameter(p,"PlotsToShow", defaultPlot, ...
                    @(plotNames)validateattributes(convertCharsToStrings(plotNames), "string", ["nonempty", "vector"], "show", "PlotsToShow"));

                p.parse(varargin{:});

                % Get number of plots
                [numPlots, plotsToShow] = obj.getNumPlots(p);

                % Validate name value pairs
                validateNameValue(obj, p, numPlots);

                % Get histogram data
                elRange = obj.ElevationRange;
                azRange = obj.AzimuthRange;
                uavPose = [obj.Position;quat2eul(obj.Orientation','ZYX')'];

                [targetEl, targetAz] = getTargetCell(obj,uavPose,elRange,azRange);

                % Get azimuth and elevation angles of desired direction.
                [desiredDirAz, desiredDirEl] = getDesiredCell(obj);

                % Get obstacles data
                occData = obj.obstaclePoints(obj.OccupancyValue,obj.DistanceLayer);

                % Get free cells data
                freeData = freeCells(obj);

                % Get memory histogram data
                memoryData = memoryHistogram(obj);

                %Update this to add previous and current yaw direction
                %data
                directionData = [desiredDirEl, desiredDirAz, targetEl, targetAz];

                axHandle = obj.prepareAxesHandle(p,numPlots,plotsToShow);

                % Plot the selected plots
                for k = 1:numel(plotsToShow)

                    plotName = plotsToShow(k);

                    switch plotName

                        case "3D Histogram"
                            % Plot the 3D histogram
                            plot3DHistogram(obj, axHandle(k), uavPose, occData, freeData, directionData);

                        case "2D Memory Histogram"
                            % Plot 2D memory histogram
                            plot2DHistogram(obj, axHandle(k), memoryData, ...
                                directionData, message("uav:robotuav:obstacleavoidance:MemoryHistogramTitle").getString,...
                                'Memory Histogram');

                        case "2D Inflated Histogram"
                            % Plot 2D inflated histogram
                            plot2DHistogram(obj, axHandle(k), occData, ...
                                directionData, message("uav:robotuav:obstacleavoidance:InflatedHistogramTitle").getString,...
                                'Inflated Histogram');

                        case "Cost Matrix"
                            % Plot cost matrix
                            plotCostMatrix(obj,axHandle(k));
                    end
                end
                drawnow limitrate;

                hold(axHandle,'off');

                % Only return handle if user requested it.
                if nargout > 0
                    ax = axHandle;
                end
            end
        end
    end

    methods(Access = {?controllerVFH3D,?matlab.unittest.TestCase})

        function validateNameValue(obj,p,numPlots)
            %validateNameValue Validate name value pair of show method.
            %This function created to reduce complexity.

            % Parent axes is provided by user
            if numel(p.Results.Parent) > 0
                if ~isequal(numel(p.Results.Parent), numPlots)
                    error(message("uav:robotuav:obstacleavoidance:NumParentPlotsMismatch"));
                end
            end

            % Validate name of the plots to show
            if ~all(ismember(p.Results.PlotsToShow,obj.ExpectedPlots))
                error(message("uav:robotuav:obstacleavoidance:ValidPlotsToShow"));
            end
        end

        function axHandle = prepareAxesHandle(~,p,numPlots,plotsToShow)
            %prepareAxesHandle Prepare axes handles

            % Initialize array to hold graphics objects
            axHandle = gobjects(1, numPlots);

            % No parent axes provided by user
            if isempty(p.Results.Parent)
                % Create tiled chart layout
                tiledlayout('flow');
                % fill axHandle
                for idx = 1:numel(plotsToShow)
                    axHandle(idx) = nexttile;
                end
            else
                % Use user provided handles
                for idx = 1:numPlots
                    axHandle(idx) = newplot(p.Results.Parent(idx));
                end
            end
        end

        function [numPlots, plotsToShow] = getNumPlots(~,p)
            %numPlots Get number of plots to show

            plotsToShow = convertCharsToStrings(p.Results.PlotsToShow);

            if ismember("All",plotsToShow)
                numPlots = 4;
                plotsToShow = ["3D Histogram","2D Memory Histogram", "2D Inflated Histogram", "Cost Matrix"];
            else
                numPlots = numel(plotsToShow);
            end
        end

        function p = plotCostMatrix(obj,ax)

            uavPose = [obj.Position;quat2eul(obj.Orientation','ZYX')'];
            windowSize = obj.WindowSize;
            goalPosition = obj.TargetPosition;
            costMatrix = getCostMatrix(obj,uavPose,windowSize,goalPosition);
            [azCost,elCost,radiusCost,azMin,elMin,minVal] = obj.polarCostMatrix(costMatrix,obj.AngularResolution);
            [xCost,yCost,colorCostCell] = obj.patch2DData(azCost, elCost, radiusCost, obj.AngularResolution);
            [xMinCost,yMinCost,colorMinCostCell] = obj.patch2DData(azMin, elMin, minVal, obj.AngularResolution);

            obj.plotGrid(ax,obj.AngularResolution);

            xlabelName = message("uav:robotuav:obstacleavoidance:XLabelName2D").getString;
            ylabelName = message("uav:robotuav:obstacleavoidance:YLabelName2D").getString;
            xlabel(ax,xlabelName);
            ylabel(ax,ylabelName);

            costMatrixTitle = message("uav:robotuav:obstacleavoidance:CostMatrixTitle").getString;
            title(ax,costMatrixTitle);

            p1 = patch(ax,xCost,yCost,colorCostCell,...
                'FaceAlpha',1,'EdgeAlpha',0.1,...
                'Tag','Cost Matrix');
            uav.internal.util.specifyThemePropertyMapping(p1, ...
                'EdgeColor', obj.ColorBlack);

            p2 = patch(ax,xMinCost,yMinCost,colorMinCostCell,...
                'FaceAlpha',1,'EdgeAlpha',1,...
                'Tag','Minimum Cost Cell','LineWidth',obj.LineWidth);
            uav.internal.util.specifyThemePropertyMapping(p2, ...
                'EdgeColor', obj.ColorWhite);

            minLegend = message("uav:robotuav:obstacleavoidance:CostMatrixLegend").getString;
            legend(ax,p2,minLegend);

            p = [p1 p2];

        end

        function p = plot2DHistogram(obj, ax, occData, directionData, plotTitle, tag)

            memEl = occData(:,1); memAz = occData(:,2); memRadius = occData(:,3);
            desiredDirEl = directionData(1); desiredDirAz = directionData(2);
            targetEl = directionData(3); targetAz = directionData(4);

            % Get X-Y coordinates of the cells
            [xMemCell,yMemCell,colorMemCell] = obj.patch2DData(memAz, memEl, memRadius, obj.AngularResolution);
            [xDesDir,yDesDir,~] = obj.patch2DData(desiredDirAz, desiredDirEl, [0 1 0], obj.AngularResolution);
            [xTargetDir,yTargetDir,~] = obj.patch2DData(targetAz, targetEl, [1 0 1], obj.AngularResolution);

            obj.plotGrid(ax,obj.AngularResolution);
            xlabelName = message("uav:robotuav:obstacleavoidance:XLabelName2D").getString;
            ylabelName = message("uav:robotuav:obstacleavoidance:YLabelName2D").getString;
            xlabel(ax,xlabelName);
            ylabel(ax,ylabelName);
            title(ax,plotTitle);

            p1 = patch(ax,xMemCell,yMemCell,colorMemCell,...
                'FaceAlpha',1,'EdgeAlpha',0.1,...
                'Tag','2D Memory Cells');
            uav.internal.util.specifyThemePropertyMapping(p1, ...
                'EdgeColor', obj.ColorBlack);

            p3 = patch(ax,xTargetDir,yTargetDir,'m',...
                'FaceAlpha',1,'EdgeAlpha',1,'Tag',['2D Target Direction ' tag],...
                'LineWidth',obj.LineWidth);
            uav.internal.util.specifyThemePropertyMapping(p3, ...
                'EdgeColor', obj.ColorTargetEdge);
            uav.internal.util.specifyThemePropertyMapping(p3, ...
                'FaceColor',obj.ColorTargetCell);

            if (abs(desiredDirAz - targetAz) < sqrt(eps) && abs(desiredDirEl - targetEl) < sqrt(eps))
                % Added to address g2753107. When desired and target
                % directions overlap, color half the patches with different
                % colors.
                xDesDirTemp = xDesDir;
                % Half patch
                xDesDirTemp(xDesDirTemp == max(xDesDirTemp)) = (min(xDesDirTemp) + max(xDesDirTemp))/2;
                xDesDir = xDesDirTemp;
                p2 = patch(ax,xDesDir,yDesDir,'g',...
                    'FaceAlpha',1,'EdgeAlpha',1,'Tag',['2D Desired Direction ' tag],...
                    'LineWidth',obj.LineWidth);
            else
                p2 = patch(ax,xDesDir,yDesDir,'g',...
                    'FaceAlpha',1,'EdgeAlpha',1,'Tag',['2D Desired Direction ' tag],...
                    'LineWidth',obj.LineWidth);
            end

            uav.internal.util.specifyThemePropertyMapping(p2, ...
                'EdgeColor', obj.ColorDesiredEdge);
            uav.internal.util.specifyThemePropertyMapping(p2, ...
                'FaceColor',obj.ColorDesiredCell);

            % Get legend strings
            desiredLegend = message("uav:robotuav:obstacleavoidance:Histogram2DDesiredLegend").getString;
            targetLegend = message("uav:robotuav:obstacleavoidance:Histogram2DTargetLegend").getString;

            legend(ax,[p2 p3],{desiredLegend, targetLegend});
            colorbar(ax);

            p = [p1 p2 p3];

        end

        function plot3DHistogram(obj, ax, uavPose, occData, freeData, directionData)
            %plot3DHistogram Plot the 3D histogram. Show the occupied
            %cells, inflated cells, free cells, deisred direction, target
            %direction, desired direction cell, target direction cell,
            %previous direction and current direction

            occEl = occData(:,1); occAz = occData(:,2); occRadius = occData(:,3);
            freeEl = freeData(:,1); freeAz = freeData(:,2); freeRadius = freeData(:,3);
            desiredDirEl = directionData(1); desiredDirAz = directionData(2);
            targetEl = directionData(3); targetAz = directionData(4);

            % Get X-Y coordinates of the cells
            [xCell,yCell,zCell,colorCell] = obj.patch3DData(occAz, occEl, occRadius, obj.AngularResolution);
            [xF,yF,zF,cF] = obj.patch3DData(freeAz, freeEl, freeRadius, obj.AngularResolution);

            titleName = message("uav:robotuav:obstacleavoidance:Histogram3DTitle").getString;
            % Add title
            title(ax,titleName);

            % Plot obstacle and free cells
            p1 = patch(ax,xCell,yCell,zCell,colorCell,'FaceColor','flat',...
                'FaceAlpha',1,'EdgeAlpha',0.2,'Tag','Obstacle 3D Cell');
            uav.internal.util.specifyThemePropertyMapping(p1, 'EdgeColor', obj.ColorBlack);
          
            p2 = patch(ax,xF,yF,zF,cF,'FaceColor','flat','FaceAlpha',0.1,...
                'EdgeAlpha',0.1,'Tag','Free 3D Cell');
            uav.internal.util.specifyThemePropertyMapping(p2, 'EdgeColor', obj.ColorBlack);

            if (abs(desiredDirAz - targetAz) < sqrt(eps) && abs(desiredDirEl - targetEl) < sqrt(eps))

                % Get desired direction cell. Desired cell is green with
                % white boundary
                [xD,yD,zD,cD] = obj.splitPatch3DData(desiredDirAz, desiredDirEl, 1, obj.AngularResolution);
                pd = patch(ax,xD,yD,zD,cD,'FaceAlpha',1,...
                    'EdgeAlpha',1,'Tag','Desired 3D Cell','LineWidth',obj.LineWidth);
                % Split the target cell also because when looking from
                % inside the sphere towards outside, the cell looks full in
                % magenta (target cell color)
                [xT,yT,zT,cT] = obj.splitPatch3DData(targetAz + obj.AngularResolution/2, targetEl, 1, obj.AngularResolution);
                pt = patch(ax,xT,yT,zT,cT,'FaceAlpha',1,...
                    'EdgeAlpha',1,'Tag','Target 3D Cell','LineWidth',obj.LineWidth);
            else
                [xD,yD,zD,cD] = obj.patch3DData(desiredDirAz, desiredDirEl, 1, obj.AngularResolution);
                pd = patch(ax,xD,yD,zD,cD,'FaceAlpha',1,...
                    'EdgeAlpha',1,'Tag','Desired 3D Cell','LineWidth',obj.LineWidth);
                [xT,yT,zT,cT] = obj.patch3DData(targetAz, targetEl, 1, obj.AngularResolution);
                pt = patch(ax,xT,yT,zT,cT,'FaceAlpha',1,...
                    'EdgeAlpha',1,'Tag','Target 3D Cell','LineWidth',obj.LineWidth);
            end

            % Set desired cell color
            uav.internal.util.specifyThemePropertyMapping(pd, ...
                'FaceColor',obj.ColorDesiredCell);
            uav.internal.util.specifyThemePropertyMapping(pd, ...
                'EdgeColor',obj.ColorDesiredEdge);

            % Set target cell color
            uav.internal.util.specifyThemePropertyMapping(pt, ...
                'FaceColor',obj.ColorTargetCell);
            uav.internal.util.specifyThemePropertyMapping(pt, ...
                'EdgeColor',obj.ColorTargetEdge);

            plotUAVMesh(obj,ax);
            colorbar(ax);

            prevStatus = ax.NextPlot;
            ax.NextPlot = 'add';
            teardown = onCleanup(@()resetNextPlot(ax, prevStatus));

            function resetNextPlot(ax, status)
                ax.NextPlot = status;
            end
            plot3DDirection(obj,ax,uavPose);

            % Axis view changed to address g2751976
            view(ax,-97,29);

        end

        function [xData,yData,zData,cData] = splitPatch3DData(~, az, el, occDist, res)
            %splitPatch3DData Get the XYZ and CData from azimuth and
            % elevation for half patch

            XL = zeros(4,numel(az)); YL = zeros(4,numel(az));
            ZL = zeros(4,numel(az));
            CL = zeros(4,numel(az));

            for idx = 1:numel(az)

                % Get the azimuth and elevation angles of the four neighbour corner
                % points. az, el is the patch center. The half patch
                % vertices are (azNeg,elNeg), (azMiddle,elNeg),
                % (azMiddle,elPlus) and (azNeg,elPlus)
                azNeg = az(idx) - res/2;
                azMiddle = az(idx);
                elNeg = el(idx) - res/2;
                elPlus = el(idx) + res/2;

                [PHI,THETA] = meshgrid([azNeg azMiddle]*pi/180,[elNeg elPlus]*pi/180);

                % Interchange to get correct patch order
                THETA(:,2) = [THETA(2,2);THETA(1,2)];

                X = 1*cos(THETA).*cos(PHI);
                Y = 1*cos(THETA).*sin(PHI);
                Z = 1*sin(THETA).*cos(PHI*0);
                C = occDist(idx)*ones(size(Z));

                XL(:,idx) = X(:);
                YL(:,idx) = Y(:);
                ZL(:,idx) = Z(:);
                CL(:,idx) = C(:);
            end

            xData = XL;
            yData = YL;
            zData = ZL;
            cData = occDist;

        end

        function [xData,yData,zData,cData] = patch3DData(~, az, el, occDist, res)
            %patch3DData Get the XYZ and CData from azimuth and elevation

            XX = zeros(4,numel(az)); YY = zeros(4,numel(az));
            ZZ = zeros(4,numel(az));
            CC = zeros(4,numel(az));

            for idx = 1:numel(az)

                % Get the azimuth and elevation angles of the four neighbour corner
                % points
                azNeg = az(idx) - res/2;
                azPlus = az(idx) + res/2;
                elNeg = el(idx) - res/2;
                elPlus = el(idx) + res/2;

                [PHI,THETA] = meshgrid([azNeg azPlus]*pi/180,[elNeg elPlus]*pi/180);

                % Interchange to get correct patch order
                THETA(:,2) = [THETA(2,2);THETA(1,2)];

                X = 1*cos(THETA).*cos(PHI);
                Y = 1*cos(THETA).*sin(PHI);
                Z = 1*sin(THETA).*cos(PHI*0);
                C = occDist(idx)*ones(size(Z));

                XX(:,idx) = X(:);
                YY(:,idx) = Y(:);
                ZZ(:,idx) = Z(:);
                CC(:,idx) = C(:);
            end

            xData = XX;
            yData = YY;
            zData = ZZ;
            cData = occDist;

        end

        function [xData,yData,cData] = patch2DData(~, az, el, occDist, res)
            %patch2DData Get the XY and CData from azimuth and elevation

            XX = zeros(4,numel(az)); YY = zeros(4,numel(az));

            for idx = 1:numel(az)

                % Get the azimuth and elevation angles of the four neighbour corner
                % points
                azNeg = az(idx) - res/2;
                azPlus = az(idx) + res/2;
                elNeg = el(idx) - res/2;
                elPlus = el(idx) + res/2;

                pts = [azNeg elNeg;azNeg elPlus;azPlus elPlus;azPlus elNeg];

                XX(:,idx) = pts(:,1);
                YY(:,idx) = pts(:,2);
            end

            xData = XX;
            yData = YY;
            cData = occDist;
        end

        function plotUAVMesh(~,ax)
            %plotUAVMesh Plot the UAV mesh with axes

            % Plot quadrotor mesh
            hplot = plotTransforms([0 0 0],quaternion([1 0 0 0]),...
                'Parent',ax, 'MeshFilePath','multirotor.stl','FrameSize',0.3);
            frame = findobj(hplot, 'type', 'hgtransform', 'tag', 'BodyToInertialTransform');
            xLine = findobj(frame', 'type', 'line', 'tag', 'xAxis');
            yLine = findobj(frame', 'type', 'line', 'tag', 'yAxis');
            zLine = findobj(frame', 'type', 'line', 'tag', 'zAxis');

            % Change x axis line
            xLine.LineWidth = 1;
            xLine.XData = [0 0.5];

            % Change x axis line
            yLine.LineWidth = 1;
            yLine.YData = [0 0.5];
            % Change x axis line
            zLine.LineWidth = 1;
            zLine.ZData = [0 0.5];

        end

        function [desLineHandle, previousHandle, currentHandle] = plot3DDirection(obj,ax, uavPose)
            %plot3DDirection Plot the deired direction, previous direction
            %and current direction

            % Plot desired direction in dashed green color
            desLineHandle = plot3(ax,[0 obj.DesiredDirection(1)],...
                [0 obj.DesiredDirection(2)],[0 obj.DesiredDirection(3)],...
                'LineWidth',2,'LineStyle','--');
            uav.internal.util.specifyThemePropertyMapping(desLineHandle, ...
                'Color', obj.ColorGreen);
            % Plot previous direction in dashed blue color
            previousHandle = plot3(ax,[0 obj.PrevDirection(1)],...
                [0 obj.PrevDirection(2)],[0 obj.PrevDirection(3)],...
                'LineWidth',2,'LineStyle','--');
            uav.internal.util.specifyThemePropertyMapping(previousHandle, ...
                'Color', obj.ColorBlue);

            % Plot current direction in dashed red color
            currentDir = [cos(uavPose(4)) sin(uavPose(4)) 0];
            currentHandle = plot3(ax,[0 currentDir(1)],...
                [0 currentDir(2)],[0 currentDir(3)],...
                'LineWidth',2,'LineStyle','--');
            uav.internal.util.specifyThemePropertyMapping(currentHandle, ...
                'Color', obj.ColorRed);

            legend(ax,[desLineHandle currentHandle previousHandle],...
                {'Desired direction','Current direction','Previous direction'},'Location','northeast');

        end

        function [targetEl, targetAz] = getTargetCell(obj,uavPose,elRange,azRange)
            %getTargetCell Get the azimuth and elevation angles of the
            %target point

            [elTargetIndex,azTargetIndex,targetDist] = getHistogramIndex(obj,...
                obj.TargetPosition', uavPose, elRange, azRange);
            [targetAz,targetEl,~] = histogramIndexToPolar(obj, elTargetIndex,...
                azTargetIndex, targetDist, obj.AngularResolution);

        end

        function [desiredDirAz, desiredDirEl] = getDesiredCell(obj)
            %getDesiredCell Get the azimuth and elevation angles of desired
            %direction

            % Desired direction XYZ in histogram frame
            desiredDirXYZHistFrame = obj.DesiredDirection;

            % Desired direction in polar coordinates
            [desiredDirAz,desiredDirEl,~] = uav.internal.util.cart2sphd(desiredDirXYZHistFrame(1), ...
                desiredDirXYZHistFrame(2), desiredDirXYZHistFrame(3));

        end

        function [az,el,radius,azMin,elMin,minVal] = polarCostMatrix(obj,costMatrix,res)

            [elIdx,azIdx] = find(isfinite(costMatrix));
            minVal = min(min(costMatrix));

            [elMinIdx,azMinIdx] = find(costMatrix == minVal);

            radius = costMatrix(sub2ind(size(costMatrix),elIdx(:),azIdx(:)));
            [az,el,~] = histogramIndexToPolar(obj,elIdx(:),azIdx(:),radius,res);
            [azMin,elMin,~] = histogramIndexToPolar(obj,elMinIdx(:),azMinIdx(:),minVal,res);

        end

        function memData = memoryHistogram(obj)
            %memoryHistogram Get the memory histogram data

            uavPose = [obj.Position;quat2eul(obj.Orientation','ZYX')'];

            % Generate the histogram with updated vehicle and safety
            % distance to get un-inflated histogram. Gets the updated
            % occupancy matrix
            [distLayer,occVal] = generateMemoryHistogram(obj,obj.OldPointsGlobal,uavPose);

            % Get memory cells of the memory histogram
            memData = obj.obstaclePoints(occVal,distLayer);
        end

        function [distLayer,occVal] = generateMemoryHistogram(obj,pointsGlobal,uavPose)
            %generateMemoryHistogram Generate the memory histogram. Here
            %the obstacle points are not inflated taking into account the
            %vehicle radius and safety distance

            %convert points in global frame to histogram frame
            pointsHistFrame = obj.globalToHistFrame(pointsGlobal(:,1:3),uavPose);

            %elevation range
            elRange = obj.ElevationRange;
            %number of bins in elevation
            nbinsX = obj.NBinsElevation;

            %azimuth range
            azRange = obj.AzimuthRange;
            %number of bins in azimuth
            nbinsY = obj.NBinsAzimuth;

            %initialize the distance layer. Each element contains the average
            %distance of all the points that fall into the corresponding
            %cell
            distLayer = zeros(nbinsX,nbinsY);
            %occupancy matrix indicating if a particular cell is occupied
            %or empty. 1 is occupied and 0 is free. Cell is occupied if
            %at least one point falls in a cell
            occVal = zeros(nbinsX,nbinsY);

            %Number of points in a cell. Dimension is same as distLayer
            numPts = distLayer;

            %iterate over all the data/sensor points
            for k = 1: size(pointsHistFrame,1)
                %use only non-nan sensor points
                if ~isnan(pointsHistFrame(k,1)) && ~isnan(pointsHistFrame(k,2)) && ~isnan(pointsHistFrame(k,3))
                    %compute azimuth and elevation angle

                    [azDeg,elDeg,dist] = uav.internal.util.cart2sphd(pointsHistFrame(k,1),pointsHistFrame(k,2),pointsHistFrame(k,3));

                    %histcounts2 replaced for performance improvement.
                    [binX,binY] = histogramBins(obj,elDeg,azDeg,elRange,azRange,obj.AngularResolution);
                    %saturate bins
                    binX = max(1,min(binX,180/obj.AngularResolution));
                    binY = max(1,min(binY,360/obj.AngularResolution));
                    numPts(binX,binY) = numPts(binX,binY) + 1;

                    %make cells around obstacle point as occupied
                    occVal(binX,binY) = 1;

                    %update distance layer to average of all the points
                    %that fall in the bin. Moving average implementation.
                    distLayer(binX,binY) = distLayer(binX,binY)*((numPts(binX,binY)-1)/numPts(binX,binY))...
                        + dist/numPts(binX,binY);
                end
            end
        end

        function  occData = obstaclePoints(obj,occVal,distLayer)
            %obstaclePoints Get the azimuth and elevation angles of the
            %obstacle points from the occupancy value matrix. '1' are
            %occupied cells.

            %find obstacle indices
            [occRowIdx,occColIdx] = find(occVal == 1);

            % Get distace to obstacle
            occRadius = distLayer(sub2ind(size(distLayer),occRowIdx(:),occColIdx(:)));

            [occAz,occEl,occRadius] = histogramIndexToPolar(obj,occRowIdx,occColIdx,occRadius,obj.AngularResolution);

            occData = [occEl,occAz,occRadius];

        end

        function  freeData = freeCells(obj)
            %freeCells Get the aximuth and elevation angles of the free
            %cells

            % Get indices of free cells
            [freeRowIdx,freeColIdx] = find(obj.OccupancyValue == 0);

            % Get the distance of free cells.
            freeRadius = obj.DistanceLayer(sub2ind(size(obj.DistanceLayer),freeRowIdx(:),freeColIdx(:)));

            [freeAz,freeEl,freeRadius] = histogramIndexToPolar(obj,freeRowIdx,freeColIdx,freeRadius,obj.AngularResolution);

            freeData = [freeEl,freeAz,freeRadius];

        end

        function plotGrid(obj,ax,res)

            phiV = -180:res:180;
            thetaV = -90:res:90;

            [row,col] = meshgrid(phiV,thetaV);
            % plot vertical and horizontal grid lines
            verline = line(ax,row,col,'LineWidth',0.5);
            uav.internal.util.specifyThemePropertyMapping(verline, ...
                'Color', obj.ColorGrey);
            horline = line(ax,row',col','LineWidth',0.5);
            uav.internal.util.specifyThemePropertyMapping(horline, ...
                'Color', obj.ColorGrey);

        end
    end
end
