classdef uavCoverageSpace<handle & uav.internal.InternalAccess
    %UAVCOVERAGESPACE 2D coverage area for coverage planner
    %   The UAVCOVERAGESPACE object represents an area for a UAV to cover
    %   with a sensor. The coverage space is represented within the object
    %   as polygons, which form the configuration space. The object is
    %   typically used to interface with coveragePlanner that require a
    %   uavCoverageSpace as inputs.
    %
    %   space = UAVCOVERAGESPACE creates an empty coverage space object.
    %
    %   space = UAVCOVERAGESPACE(Polygons=POLYGONS) creates a coverage
    %   space defined by convex polygons POLYGONS and sets the Polygons
    %   property. POLYGONS is an M-element cell array of N-by-2 matrices,
    %   where each matrix contains the vertices that define a polygon. M is
    %   the number of polygons and N is the number of vertices for the
    %   polygon. If any polygons in POLYGONS are concave, use the
    %   coverageDecomposition function to decompose the polygons into
    %   convex polygons.
    %
    %   space = UAVCOVERAGESPACE(...,Name=Value) sets properties using one
    %   or more name-value arguments.
    %
    %   UAVCOVERAGESPACE properties:
    %      Polygons             - Structure of polygons
    %      Overlap              - Overlap between footprint in a single row
    %      Sidelap              - Overlap between footprint of two rows
    %      UnitWidth            - UAV traversal footprint perpendicular 
    %                             to sweep path
    %      UnitLength           - UAV traversal footprint along sweep path
    %      ReferenceHeight      - Flight altitude of UAV
    %      UseLocalCoordinates  - Compute path in local or LLA format
    %      ReferenceLocation    - Georeference to compute path LLA 
    %                             coordinates
    %
    %   UAVCOVERAGESPACE methods:
    %      setCoveragePattern   - Set the sweep angle for the coverage area
    %      show                 - Visualize the coverage space polygons
    %
    %   Example 1 : Plan In Local Coordinates
    %   -------------------------------------------------
    %      % Create an empty uavCoverageSpace object.
    %      SS = uavCoverageSpace;
    %
    %      % Define polygons with vertices in local coordinates.
    %      rectA = [0 0; 0 100; 200 100; 200 0];
    %      rectB = [200 0; 200 100; 450 100; 450 0];
    %      polygons={rectA,rectB};
    %
    %      % Create a uavCoverageSpace with the rectangle polygons and set
    %      % the UnitWidth property to 1.
    %      space = uavCoverageSpace('Polygons',polygons,UnitWidth=1);
    %
    %      % Set SweepAngle for polygon 1 to 45 degrees.
    %      setCoveragePattern(space,1,'SweepAngle',45);
    %
    %      % Create a coverage planner with the coverage space and plan a
    %      % path between the vertices, [0 0 0], and [450 0 0].
    %      cp1 = uavCoveragePlanner(space);
    %      wpts1 = plan(cp1,[0 0 0],[450 0 0]);
    %
    %      % Visualize polygons and plot the lines.
    %      show(space);
    %      hold on
    %      plot(wpts1(:,1),wpts1(:,2));
    %      hold off 
    %
    %   Example 2 : Show Polygon in Geodetic Coordinates Over Geoaxes
    %   -------------------------------------------------------------
    %      % Define uavCoverageSpace with polygons defined with vertices in
    %      % latitude and longitude format. Convert local coordinates to
    %      % LLA if you don't have coordinate in LLA format already.
    %
    %      polygon = 1000*[[0 0 1 3]', [0 3 3 0]'];
    %      convpoly = enu2lla([polygon,[100;100;100;100]],[0 0 0],"flat");
    %      LLAPolygon = convpoly(:,1:2);
    %      space2 = uavCoverageSpace(Polygons=LLAPolygon, ...
    %      UseLocalCoordinates=false); 
    %
    %      % Visualize polygon over a geoaxes.
    %      fig = figure;
    %      g = geoaxes(fig);
    %      show(space2,Parent=g);
    %
    %   See also uavCoveragePlanner


    %   Copyright 2021-2022 The MathWorks, Inc.
    %

    %#codegen
    properties (SetAccess= {?uavCoveragePlanner},GetAccess=public)

        %Polygons Polygons to survey with UAV
        %   Polygons to survey with UAV, stored as a two-element structure
        %   array containing the these fields:
        %      Vertices   - Polygon vertices stored as an M-element array
        %                   of N-by-2 matrices. M is the total number of
        %                   polygons and N is the total number of vertices
        %                   for each polygon.
        %      SweepAngle - Sweep angles for each polygon, stored as an
        %                   M-element row vector, where M is the total
        %                   number of polygons.
        %
        %   The property is read-only and can be specified at construction.
        %   The SweepAngle can be set using the setCoveragePattern method.
        %
        %   Default: [1x0 struct]
        Polygons

    end
    properties
        %Sidelap Normalized sidelap between sensor footprints of two rows
        %   Normalized sidelap between sensor footprints of two rows,
        %   specified as a nonnegative numeric scalar in the range [0, 1).
        %   A Sidelap value of 0 corresponds to no overlap between
        %   footprints and increasing Sidelap towards a value of 1 results
        %   in more overlap between footprints.
        %
        %   Default: 0
        Sidelap

        %Overlap Normalized overlap between sensor footprints in single row
        %   Normalized overlap between sensor footprints in single row,
        %   specified as a nonnegative numeric scalar in the range [0, 1].
        %   An overlap value of 0 corresponds to no overlap between
        %   footprints and increasing Overlap towards a value of 1 results
        %   in more overlap between consecutive footprints.
        %
        %   Default: 0
        Overlap

        %UnitWidth Width of sensor footprint
        %   Width of sensor footprint, specified as a positive numeric
        %   scalar, in meters. This measurement is perpendicular to the
        %   sweep line direction.
        %
        %   Default: 1
        UnitWidth

        %UnitLength Length of sensor footprint
        %  Length of sensor footprint, specified as a positive numeric
        %  scalar, in meters. This measurement is along the sweep line
        %  direction.
        %
        %   Default: 1
        UnitLength

    end

    properties (GetAccess=public,SetAccess= {?uav.internal.InternalAccess,?uavCoveragePlanner})
        %ReferenceHeight Flight altitude of UAV
        % Flight altitude of UAV, specified as a numeric scalar.
        %
        % Default: 100
        ReferenceHeight

        %UseLocalCoordinates Compute path in local or geodetic format
        %   Compute path in local or geodetic format, specified as a
        %   logical 1 (true) for local coordinate format or logical 0
        %   (false) for geodetic format. If true, the polygon vertices,
        %   takeoff, landing positions, and planned waypoints are defined
        %   in local coordinates. If false, they are instead defined in
        %   geodetic coordinates.
        %   
        %   Note that UseLocalCoordinates does not affect the
        %   exportWaypointsPlan function output format.
        %
        %   Default: true
        UseLocalCoordinates

        %ReferenceLocation Georeference for geodetic waypoint coordinates
        %   Georeference for geodetic waypoint coordinates, specified in
        %   the form [latitude longitude altitude], in degrees. This
        %   reference location is used to convert local coordinates to
        %   geodetic coordinates and compute geodetic coordinates in this
        %   reference frame.
        %
        %   Default: [0 0 0]
        ReferenceLocation
    end

    properties (Access= {?uav.internal.InternalAccess})
        %PolygonsInternal store internal representation of polygons in X-Y
        %frame. This is used for path planning.
        PolygonsInternal

    end

    methods
        function obj=uavCoverageSpace(varargin)
            %UAVCOVERAGESPACE Constructor.
            parser = robotics.core.internal.NameValueParser(...
                {'Polygons','UnitWidth', 'UnitLength', 'Sidelap', 'Overlap','ReferenceHeight','UseLocalCoordinates','ReferenceLocation'}, {{},1, 1, 0,0,100,true,[0,0,0]});
            parse(parser, varargin{:});

            obj.UnitWidth = parameterValue(parser, 'UnitWidth');
            obj.UnitLength = parameterValue(parser, 'UnitLength');
            obj.Sidelap = parameterValue(parser, 'Sidelap');
            obj.Overlap = parameterValue(parser, 'Overlap');
            obj.ReferenceHeight = parameterValue(parser, 'ReferenceHeight');
            obj.UseLocalCoordinates = parameterValue(parser, 'UseLocalCoordinates');
            obj.ReferenceLocation = parameterValue(parser, 'ReferenceLocation');
            polygons = parameterValue(parser, 'Polygons');
            coder.varsize('polygons', [1 Inf], [0 1]);
            coder.varsize('polygons{:}', [Inf 2], [1 0]);
            setPolygons(obj,polygons);
        end

        function set.ReferenceLocation(obj,lla0)
            %set.ReferenceLocation Setter for ReferenceLocation property
            validateattributes(lla0, {'double'}, {"real", "nonempty", "vector", "ncols", 3}, "uavCoverageSpace", 'ReferenceLocation');
            %?Same validation for Lat Lon inputs
            %Verify that the inputs are within the range
            matlabshared.internal.latlon.validateLat(lla0(:,1),0);
            matlabshared.internal.latlon.validateLat(lla0(:,1),1);
            matlabshared.internal.latlon.validateLon(lla0(:,2),0);
            matlabshared.internal.latlon.validateLon(lla0(:,2),1);
            obj.ReferenceLocation=lla0;
        end

        function set.UnitWidth(this, unitwidth)
            %set.UnitWidth Setter for UnitWidth property
            validateattributes(unitwidth,{'double', 'single'},{'nonempty', 'nonnan', 'real', 'scalar', ...
                'positive', 'finite'},'uavCoverageSpace','UnitWidth');
            this.UnitWidth = unitwidth;
        end

        function set.ReferenceHeight(this, height)
            %set.UnitWidth Setter for UnitWidth property
            validateattributes(height,{'double', 'single'},{'nonempty', 'nonnan', 'real', 'scalar', ...
                'nonnegative', 'finite'},'uavCoverageSpace','Height');
            this.ReferenceHeight = height;
        end

        function set.UnitLength(this, unitlength)
            %set.UnitLength Setter for UnitLength property
            validateattributes(unitlength,{'double', 'single'},{'nonempty', 'nonnan', 'real', 'scalar', ...
                'positive', 'finite'},'uavCoverageSpace','UnitLength');
            this.UnitLength = unitlength;
        end

        function set.Sidelap(this, sidelap)
            %set.Sidelap Setter for Sidelap property
            validateattributes(sidelap,{'double', 'single'},{'nonempty', 'nonnan', 'real', 'scalar', ...
                'nonnegative', 'finite', "<", 1},'uavCoverageSpace','Sidelap');
            this.Sidelap = sidelap;
        end

        function set.Overlap(this, overlap)
            %set.Overlap Setter for Overlap property
            validateattributes(overlap,{'double', 'single'},{'nonempty', 'nonnan', 'real', 'scalar', ...
                'nonnegative', 'finite', "<", 1},'uavCoverageSpace','Overlap');
            this.Overlap = overlap;
        end

        function set.UseLocalCoordinates(this, flag)
            %set.UseLocalCoordinates Setter for UseLocalCoordinates property
            validateattributes(flag,{'logical'},{'nonempty','scalar'},'UseLocalCoordinates');
            this.UseLocalCoordinates = flag;
        end

        function setCoveragePattern(obj,index,varargin)
            %setCoveragePattern set the SweepAngle for the coverage area.
            %   setCoveragePattern(CS ,INDEX,SweepAngle=ANGLE) sets the
            %   angle at which UAV sweeps the polygon with index INDEX
            %   to angle ANGLE, in degrees. for uavCoverageSpace CS.
            %   INDEX is same as the order in which the polygon was input
            %   to uavCoverageSpace at object creation.
            validateattributes(index,{'double', 'single'},{'nonempty', 'nonnan', 'real', 'scalar', ...
                'positive', 'finite'},'uavCoverageSpace','index');
            coder.internal.errorIf(index>numel(obj.Polygons), ...
                'uav:robotuav:coverageplanner:IndexExceedsPolygons');
            if(uavCoverageSpace.containsName(varargin{:},'SweepAngle'))
                parser = robotics.core.internal.NameValueParser({'SweepAngle'},{{}});
                parse(parser, varargin{:});
                sweepangle = parameterValue(parser, 'SweepAngle');
                validateattributes(sweepangle,{'double', 'single'},{'nonempty', 'nonnan', 'real', 'scalar', 'finite', "<=", 180, ">=", -180},'uavCoverageSpace','SweepAngle');
                %Validate if the path is interruptible.
                obj.Polygons(index).SweepAngle=sweepangle;
            end
        end


        function ax=show(obj,varargin)
            %SHOW Visualize the coverage space in 2D
            %   AX = SHOW(SS) visualizes the polygons in SS with numbering
            %   corresponding to the order of polygons specified at input.
            %
            %   AX = show(___, Name=Value) additionally takes in the
            %   following name value pairs:
            %      Parent           - Parent axis/geoaxes for plotting.
            %
            %                         Default: axes
            %
            %      FontSize         - Font size of numbering.
            %
            %                         Default: 20
            %
            %      LineWidth        - Width of lines for plotting polygons.
            %
            %                         Default: 2
            p = inputParser();
            p.addParameter("LineWidth", 2, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive", "real"], "show", "LineWidth"));
            p.addParameter("FontSize", 20, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive", "real"], "show", "LineWidth"));
            if(obj.UseLocalCoordinates)
                %Validate a simple axes as input.
                p.addParameter("Parent", [], ...
                    @(x)validateattributes(x, "matlab.graphics.axis.Axes", "scalar", "show", "Parent"));
                p.parse(varargin{:});
                if isempty(p.Results.Parent)
                    ax =newplot();
                else
                    ax = p.Results.Parent;
                end
                showLocal(obj,ax,p.Results.LineWidth,p.Results.FontSize);
            else
                p.addParameter("Parent", [], ...
                    @(x)validateattributes(x, "matlab.graphics.axis.GeographicAxes", "scalar", "show", "Parent"));
                p.parse(varargin{:});
                if isempty(p.Results.Parent)
                    ax = geoaxes();
                else
                    ax = p.Results.Parent;
                end
                showLatLon(obj,ax,p.Results.LineWidth,p.Results.FontSize);
            end
        end
    end


    methods(Access=private)
        function setPolygons(obj, polygons)
            %set.Polygons setter for polygons input into the coverage space.
            if ~isempty(polygons)

                % Ensure that  input is always a cell double
                if isnumeric(polygons)
                    polygons ={polygons};
                end

                % keep the numeric validation for an informative error
                % message even if single polygon
                validateattributes(polygons, {'cell', 'double'}, {'vector'}, 'uavCoverageSpace', 'Polygons');

                for n = 1 : numel(polygons)
                    validateattributes(polygons{n}, {'double'}, ...
                        {'real', 'ncols', 2, 'finite'}, 'uavCoverageSpace', 'Polygons');
                end
            end
            %Setup Polygons as a struct.
            %Set Polygon Vertices size
            polyVertex=[];
            s = struct('Vertices',polyVertex,'SweepAngle',NaN);
            %Declare field Vertices as variable sized.
            coder.varsize('s(:).Vertices',[inf,2]);

            numPolys=numel(polygons);
            polygonsStruct = repmat(s,1,numPolys);
            %Declare structure array as variable sized.
            coder.varsize('polygonsStruct',[1 inf]);
            %Internal representation is always in X-Y frame
            setPolygonsInternal(obj,polygons);
            for i=1:1:length(polygons)
                polygonsStruct(i).Vertices=polygons{i};
            end
            obj.Polygons=polygonsStruct;
        end

        function setPolygonsInternal(obj,polygons)
            %setPolygonsInternal setter for polygons internally to local
            %ENU  coordinates to allow computation of plan.
            obj.PolygonsInternal=polygons;
            for i=1:1:numel(polygons)
                if(~obj.UseLocalCoordinates)
                    tempPolygon= lla2enu([polygons{i},obj.ReferenceHeight*ones(size(polygons{i},1),1)],obj.ReferenceLocation,'flat');
                    obj.PolygonsInternal{i}=tempPolygon(:,1:2);
                    %Validate the polygon after LLA to ENU conversion.
                else
                    obj.PolygonsInternal{i}=polygons{i};
                end
                uav.internal.coveragePlanner.validatePolygon(obj.PolygonsInternal{i}(:,1:2));
            end
            uav.internal.coveragePlanner.validatePolygonRange(obj.PolygonsInternal);
        end

        function showLocal(obj,ax,linewidth,fontsize)
            %showLatLon
            %Cache hold state.
            previousSetting = ishold(ax);
            hold(ax, 'on');
            %Reset hold state on clean up
            reset = onCleanup(@()hold(ax, matlab.lang.OnOffSwitchState(previousSetting)));
            for i=1:1:length(obj.Polygons)
                plot(ax,polyshape(obj.Polygons(i).Vertices),'LineWidth',linewidth);
                centroid=mean(obj.Polygons(i).Vertices,1);
                text(ax, centroid(1, 1), centroid(1, 2), num2str(i),'FontSize',fontsize);
            end
        end


        function showLatLon(obj,ax,linewidth,fontsize)
            %showLatLon
            %Cache hold state.
            previousSetting = ishold(ax);
            hold(ax, 'on');
            %Reset hold state on clean up
            reset = onCleanup(@()hold(ax, matlab.lang.OnOffSwitchState(previousSetting)));
            for i=1:1:numel(obj.Polygons)
                geoplot(ax, [obj.Polygons(i).Vertices(:,1);obj.Polygons(i).Vertices(1,1)] ,[obj.Polygons(i).Vertices(:,2); ...
                    obj.Polygons(i).Vertices(1,2)],'LineWidth',linewidth);
                centroid=mean(obj.Polygons(i).Vertices,1);
                text(ax, centroid(1, 1), centroid(1, 2), num2str(i),'FontSize',fontsize);
            end
        end
    end

    methods
        function copyObj = copy(obj)
            %COPY Create deep copy of state space object
            %   COPYSPACE = COPY(SPACE) creates a deep copy of the state space
            %   object SPACE and return the new object in COPYSPACE. All
            %   data of SPACE is also present in COPYSPACE.
            %
            %   Example:
            %      % Create SE2 object and set custom weight
            %      space = uavCoverageSpace;
            %      space.UnitWidth = 5;
            %
            %      % Make a deep copy
            %      space2 = COPY(space)
            %
            %      % Verify that property values are the same
            %      isequal(space.UnitWidth, space2.UnitWidth)
            polygons= {obj.Polygons(:).Vertices};

            copyObj = uavCoverageSpace('Polygons',polygons,'ReferenceLocation',obj.ReferenceLocation,'UseLocalCoordinates',obj.UseLocalCoordinates);
            %Copy Name-Value pairs.
            copyObj.UnitWidth = obj.UnitWidth;
            copyObj.UnitLength = obj.UnitLength;
            copyObj.Sidelap = obj.Sidelap;
            copyObj.Overlap = obj.Overlap;
            copyObj.ReferenceHeight = obj.ReferenceHeight;
            %Set sweep angles.
            [copyObj.Polygons.SweepAngle]=obj.Polygons.SweepAngle;
        end
    end


    methods(Static,Hidden)
        function hasName = containsName(argName, varargin)
            %containsName During codegen, check if the given Name-Value pairs contain a
            %   certain ARGNAME. To be called before SetProperties.
            hasName = false;
            for i = 1:length(varargin)
                if strcmp(varargin{i}, argName)
                    hasName = true;
                    return;
                end
            end
        end
    end
end

% LocalWords:  Sidelap sidelap Georeference coverageplanner enu ARGNAME
