classdef uavCoveragePlanner<handle & uav.internal.InternalAccess
    %UAVCOVERAGEPLANNER Path planner for coverage of regions of interest
    %   UAVCOVERAGEPLANNER is a planner object that plans an optimal path
    %   that a UAV can follow to cover a region of interest with a sensor,
    %   such as a camera. It does this by finding a sweep angle that
    %   minimizes the number of turns in each polygon. Then the planner
    %   uses one of two specified solver algorithms to optimize the
    %   connecting path between regions with a specified sweep pattern.
    %   See [1] and [2] for more details.
    %
    %   PLANNER = uavCoveragePlanner(COVERAGESPACE) creates a
    %   coverage planner PLANNER from a uavCoverageSpace object,
    %   COVERAGESPACE.
    %
    %   PLANNER = uavCoveragePlanner(...,Name=Value)
    %   sets properties using one or more name-value arguments
    %   in addition to the input arguments in the previous syntax.
    %
    %   uavCoveragePlanner properties:
    %      CoverageSpace            - State space for planner
    %      SolverAlgorithm          - Solver algorithm name
    %      SolverParameters         - Parameters for specific algorithm
    %
    %   uavCoveragePlanner methods:
    %      plan                     - Plan path through coverage space
    %      exportWaypointsPlan      - Export waypoints to file format
    %
    %   Example:
    %      % Define the vertices for three rectanges.
    %      rectA = [0 0; 0 100; 200 100; 200 0];
    %      rectB = [200 0; 200 100; 450 100; 450 0];
    %      rectC = [450 0; 450 100; 650 100; 650 0];
    %
    %      % Create uavCoverageSpace object using the three polygons.
    %      cs = uavCoverageSpace(Polygons={rectA,rectB,rectC});
    %
    %      % Create coverage planner with the exhaustive solver algorithm.
    %      cp = uavCoveragePlanner(cs,Solver="Exhaustive");
    %
    %      % Plan a path using the exhaustive solver with the takeoff
    %      % position at [0 0 0] and the landing position at [600 0 0].
    %      [wp,soln] = plan(cp,[0 0 0],[600 0 0]);
    %
    %      % Create coverage planner with the minimum traversal algorithm.
    %      cp = uavCoveragePlanner(cs,Solver="MinTraversal");
    %
    %      % Plan a path with the minimum traversal algorithm and the same
    %      % takeoff and landing positions.
    %      [wp,soln] = plan(cp,[0 0 0],[600 0 0]);
    %
    %      % Export waypoints to a ".waypoints" file format in ENU format.
    %      exportWaypointsPlan(cp,soln,"custommission.waypoints");
    %
    %      % Visualize the polygons and plot the planned path.
    %      ax = axes;
    %      show(cs,Parent=ax);
    %      hold on
    %      plot(wp(:,1),wp(:,2));
    %      hold off
    %
    %   References:
    %   [1] Marina Torres, David A. Pelta, Jose L. Verdegay, Juan C. Torres
    %   Coverage path planning with unmanned aerial vehicles
    %   for 3D terrain reconstruction,
    %   Expert Systems with Applications,Volume 55,2016.
    %   [2] Yan Li, Hai Chen, Meng Joo Er, Xinmin Wang,
    %   Coverage path planning for UAVs
    %   based on enhanced exact cellular decomposition method,
    %   Mechatronics,Volume 21, Issue 5,2011.
    %
    %      See also uavCoverageSpace

    %   Copyright 2022 The MathWorks, Inc.
    %

    %#codegen
    properties
        %CoverageSpace
        CoverageSpace

    end


    properties (Dependent)
        %SolverParameters Parameters for specific algorithm
        %   SolverParameters for a specific algorithm, specified as a
        %   structure containing these fields depending on the value
        %   of SolverAlgorithm:
        %      - "Exhaustive"
        %         - MinAdjacencyCount -  Minimum number of
        %           adjacent polygons a sequence must have for the
        %           planner to permute, specified as a nonnegative
        %           integer.
        %           Default is 1.
        %         - VisitingSequence - Order of traversal of polygons,
        %           specified as an N-element row vector, where N is the
        %           total number of polygons in the coverage space. Default
        %           is [], which specifies no visiting sequence and enables
        %           the solver algorithm to determine the visiting
        %           sequence.
        %      - "MinTraversal"
        %         - StartingArea - Index of polygon where the UAV starts
        %           coverage specified as an integer scalar in the range
        %           [1, N]. N is the total number of polygons in the
        %           coverage space.".
        %           Default is 1.
        %         - VisitingSequence - Order of traversal of polygons,
        %           specified as an N-element row vector, where N is the
        %           total number of polygons in the coverage space.
        %           Default is [], which specifies no visiting sequence and
        %           enables the solver algorithm to determine the visiting
        %           sequence.
        SolverParameters

        %SolverAlgorithm Solver algorithm for path finding
        %   Solver algorithm for path finding, specified as either
        %   "Exhaustive or "MinTraversal":
        %      - "Exhaustive" - The planner iterates through
        %        all the coverage options in each polygon
        %        to find the path that minimizes the connection
        %        distance. See [1] for more details.
        %      - "MinTraversal" - The planner uses a recursive
        %        minimum traversal approach through a graph of adjacent
        %        polygons to minimize the connection distance between the
        %        polygons from takeoff to landing. When a coverage path has
        %        no more immediate neighbours, the UAV flies to the nearest
        %        node in the graph and continues the optimality criteria.
        %        See [2] for more details.
        %
        %   Default: "MinTraversal"
        %
        %   References:
        %   1. Marina Torres, David A. Pelta, Jose L. Verdegay, Juan C.
        %   Torres Coverage path planning with unmanned aerial vehicles
        %   for 3D terrain reconstruction,
        %   Expert Systems with Applications,Volume 55,2016.
        %   2. Yan Li, Hai Chen, Meng Joo Er, Xinmin Wang,
        %   Coverage path planning for UAVs
        %   based on enhanced exact cellular decomposition method,
        %   Mechatronics,Volume 21, Issue 5,2011.
        SolverAlgorithm
    end


    properties(Access={?uav.internal.InternalAccess})
        %SolverAlgorithmInternal
        SolverAlgorithmInternal

        %SolverParametersInternal
        SolverParametersInternal

        %CoveragePolygons Store path information for each polygon.
        CoveragePolygons

        %OptimalSweepAngleInternal
        OptimalSweepAngleInternal

    end


    properties(Access={?uav.internal.InternalAccess},Constant)
        %ExhaustivePlannerRegionLimit
        ExhaustivePlannerRegionLimit=12
    end

    methods
        function obj=uavCoveragePlanner(coverageSpace,varargin)
            %UAVCOVERAGEPLANNER Constructor.
            obj.validateConstructorInput(coverageSpace);
            %Put Exhaustive Solver as the default.
            %Set coverage parameters
            parser = robotics.core.internal.NameValueParser(...
                {'SolverParameters'}, {struct});
            [hasAlg,i,invalid] = containsName('Solver', varargin{:});
            if(hasAlg)&&(~invalid)
                obj.SolverAlgorithm=varargin{i+1};
            else
                obj.SolverAlgorithm="MinTraversal";
            end
            parse(parser, varargin{:});
            obj.SolverParameters = parameterValue(parser, 'SolverParameters');
            obj.OptimalSweepAngleInternal=zeros(1,numel(coverageSpace.Polygons));
        end

        function set.SolverAlgorithm(obj, salgorithm)
            %set.SolverAlgorithm

            obj.SolverAlgorithmInternal=validatestring(salgorithm, ...
                {'Exhaustive','MinTraversal'}, ...
                'uavCoveragePlanner', 'Solver');
            obj.SolverParametersInternal=obj.getDefaultSolverParameters;
        end


        function salgorithm = get.SolverAlgorithm(obj)
            %get.SolverAlgorithm
            salgorithm = obj.SolverAlgorithmInternal;
        end

        function solverparams=getDefaultSolverParameters(obj)
            %getDefaultSolverParameters
            coder.varsize('sequence',[1,inf]);
            switch (obj.SolverAlgorithmInternal)
                case('Exhaustive')
                    sequence=[];
                    exparams=struct('MinAdjacencyCount',0,'VisitingSequence',sequence);
                    solverparams=exparams;
                case('MinTraversal')
                    sequence=[];
                    minparams=struct('StartingArea',1,'VisitingSequence',sequence);
                    solverparams=minparams;
            end
            coder.varsize('solverparams(:).VisitingSequence',[1,inf]);
        end


        function set.SolverParameters(obj,solverparams)
            %set.SolverParameters
            params = obj.getDefaultSolverParameters;

            if isfield(solverparams, 'VisitingSequence')&&(~isempty(solverparams.VisitingSequence))
                validateattributes(solverparams.VisitingSequence, {'double'}, ...
                    {'nonempty','vector', 'nonnan', 'finite', 'positive'});
                %Sanity checks on visiting sequence if specified
                coder.internal.errorIf((numel(solverparams.VisitingSequence)~=numel(obj.CoverageSpace.Polygons)), ...
                    'uav:robotuav:coverageplanner:SequenceSameSize');
                expectedSequence=1:1:length(obj.CoverageSpace.Polygons);
                coder.internal.errorIf(any(sort(solverparams.VisitingSequence)~=expectedSequence), ...
                    'uav:robotuav:coverageplanner:SequenceNumbering');
                params.VisitingSequence = solverparams.VisitingSequence;
            end

            switch (obj.SolverAlgorithm)
                case 'Exhaustive'
                    if isfield(solverparams, 'MinAdjacencyCount')
                        validateattributes(solverparams.MinAdjacencyCount,{'double', 'single'},{'nonempty', 'nonnan', 'integer', 'scalar', ...
                            'nonnegative', 'finite'},'SolverParameters','MinAdjacencyCount');
                        coder.internal.errorIf(solverparams.MinAdjacencyCount>numel(obj.CoverageSpace.Polygons), ...
                            'uav:robotuav:coverageplanner:MinAdjExceedsPolygons');
                        params.MinAdjacencyCount=solverparams.MinAdjacencyCount;
                    end
                case 'MinTraversal'
                    if isfield(solverparams, 'StartingArea')
                        validateattributes(solverparams.StartingArea,{'double', 'single'},{'nonempty', 'nonnan', 'integer', 'scalar', ...
                            'positive', 'finite'},'SolverParameters','StartingArea');
                        coder.internal.errorIf(solverparams.StartingArea>numel(obj.CoverageSpace.Polygons), ...
                            'uav:robotuav:coverageplanner:IndexExceedsPolygons');
                        params.StartingArea=solverparams.StartingArea;
                    end
            end
            obj.SolverParametersInternal=params;
        end

        function params=get.SolverParameters(obj)
            params=obj.SolverParametersInternal;
        end

        function set.CoverageSpace(obj,cs)
            validateattributes(cs, {'uavCoverageSpace'}, {'scalar', 'nonempty'}, 'uavCoverageSpace');
            obj.CoverageSpace=cs;
        end

        function [waypoints,pathSolnObj]=plan(obj,varargin)
            %   plan Plan coverage path
            %      PATH = plan(PLANNER,TAKEOFF) returns a path PATH
            %      starting from the UAV takeoff position TAKEOFF. PATH is
            %      an N-by-2 matrix containing N waypoints and the landing
            %      position is the same as the takeoff position. If the
            %      CoverageSpace of PLANNER has UseLocalCoordinates=true,
            %      then TAKEOFF and the waypoints in PATH are defined in
            %      [x y z] local coordinates, in meters. If
            %      UseLocalCoordinates=false, then the waypoints are
            %      defined in [latitude longitude altitude], where latitude
            %      ,longitude is in degrees and altitude is in metres.
            %
            %      [PATH,SOLNINFO] = plan(planner,TAKEOFF,LANDING)
            %      specifies a landing position in addition to the
            %      take off position.
            %
            %      [PATH,SOLNINFO] = plan(PLANNER,_) returns information,
            %      SOLNINFO, about the order of visitations of polygons and
            %      sweep permutations. SOLNINFO is a structure containing
            %      these fields:
            %
            %         IsPathFound      - Status of path planning, returned
            %                            as a logical 1 (true) if the
            %                            solver successfully found a path,
            %                            and logical 0 if the solver was
            %                            unsuccessful in finding a path.
            %
            %         VisitingSequence - Order in which polygons are
            %                            visited in the planned path,
            %                            returned as an N-element row
            %                            vector of integers in the range
            %                            [1, N]. N is the total number of
            %                            polygons in the coverage space.
            %
            %         SweepPattern     - Sweep pattern used to cover
            %                            polygons, specified a scalar
            %                            integer in the range [1, 4]:
            %                            1 - Forward
            %                            2 - Counter-clockwise
            %                            3 - Reverse
            %                            4 - Clockwise
            %
            %         TransitionCost   - Euclidean distance cost in metres
            %                            for transitioning between polygons
            %                            including between the transitions
            %                            between the polygons to the
            %                            takeoff and landing positions.
            %
            %         DistanceCost     - Euclidean distance cost in metres
            %                            for total path traversed by UAV
            %                            in coverage path including
            %                            Transition Cost.
            %
            %         Takeoff          - Takeoff position as a three-
            %                            element row vector. If
            %                            UseLocalCoordinates property of
            %                            the coverage space is true,
            %                            Takeoff is in the form,
            %                            [latitude longitude altitude].
            %                            If UseLocalCoordinates is false,
            %                            then Takeoff is in the form
            %                            [x y z], in meters.
            %
            %         Landing          - Landing position as a three-
            %                            element row vector. If
            %                            UseLocalCoordinates property of
            %                            the coverage space is true,
            %                            Landing is in the form,
            %                            [latitude longitude altitude].
            %                            If UseLocalCoordinates is false,
            %                            then Landing is in the form
            %                            [x y z], in meters.
            %
            %       VerboseMode        - Output diagnostic information
            %                            about planning algorithm.
            %                            Default: false
            %
            %      [_] = plan(_,VerboseMode=true)
            %      displays additional information, such as solver
            %      computation time, in the Command Window while planning.
            %

            %Safeguard no empty space to underlying planner.
            coder.internal.errorIf((isempty(obj.CoverageSpace.Polygons)), ...
                'uav:robotuav:coverageplanner:EmptySpace');
            computeCoveragePolygons(obj);
            [waypoints,pathSolnObj]=planCoverage(obj,varargin{:});
        end

        function exportWaypointsPlan(obj,solnInfo,filename,varargin)
            %   EXPORTWAYPOINTSPLAN Export waypoints to file
            %      exportWaypointsPlan(CP,SOLNINFO,FILENAME) exports
            %      waypoints and plan data SOLNINFO to a file in ".txt" or
            %      ".waypoints" format depending on the file extension
            %      specified in the file name FILENAME. The exported
            %      waypoints are in geodetic coordinates. If the waypoints
            %      are local coordinates, they are converted to geodetic
            %      coordinates during the export process. SOLNINFO is a
            %      structure that is output by the plan function.
            %
            %      exportWaypointsPlan(___, ReferenceFrame=FRAME) exports
            %      the waypoints in the reference frame FRAME. FRAME is
            %      either "NED" (north-east-down) or "ENU" (east-north-up).
            %      The default reference frame is "ENU".
            %
            validateattributes(solnInfo, {'struct'}, {'scalar', 'nonempty'}, 'exportWaypointsPlan', 'solninfo');
            %Harder checks for solnInfo validity.
            validateSolnInfo(obj,solnInfo);
            [filepath,name,extension]=uavCoveragePlanner.validateGetFilename(filename);

            p = inputParser();
            p.addParameter("ReferenceFrame","ENU", ...
                @(x)validateattributes(x, ["char", "string"], "scalartext", "uavCoveragePlanner", "ReferenceFrame"));

            %Get waypoints in local frame.
            [waypoints,~]=computeLocalWaypoints(obj,solnInfo);
            if(obj.CoverageSpace.UseLocalCoordinates)
                p.parse(varargin{:});
                %Error if unsupported reference frame specified.
                refFrame=validatestring(p.Results.ReferenceFrame, ["ENU", "NED"], "exportWaypointsPlan", "ReferenceName");
                %Convert to LLA frame for writing to waypoints file.
                waypoints=getLLAOutputFormat(obj,waypoints,refFrame);
                %convertToLLA converts to LLA without offsetting by
                %ReferenceHeight.
                takeoffAndLanding=obj.convertToLLA([solnInfo.Takeoff;solnInfo.Landing],refFrame);
                waypoints=[takeoffAndLanding(1,:);waypoints;takeoffAndLanding(2,:)];
            else
                waypoints=getLLAOutputFormat(obj,waypoints);
                waypoints=[solnInfo.Takeoff;waypoints;solnInfo.Landing];
            end

            % Number of waypoints
            missionWP=waypoints(2:end-1,1:3);
            numWaypoints = size(missionWP,1);

            % Open file
            fileID = fopen(fullfile(filepath,name)+extension,'w');
            % resource release when the execution is interrupted
            cleanup = onCleanup(@()fclose(fileID));

            %MAVLINK EXPORT FORMAT:
            %<INDEX> <CURRENT WP> <COORD FRAME> <COMMAND> <PARAM1> <PARAM2> <PARAM3> <PARAM4> <PARAM5/X/LATITUDE> <PARAM6/Y/LONGITUDE> <PARAM7/Z/ALTITUDE> <AUTOCONTINUE>
            % Write header
            fprintf(fileID,'QGC WPL 110\n');
            % % Write home location for RTL as takeoff. This becomes the
            % home location in Mission Planner and is not a waypoint.
            fprintf(fileID,'0\t1\t0\t16\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t1\n',[0,0,0,0,waypoints(1,1:3)]);
            % % Write takeoff as takeoff mission mode. Adjust third
            % coordinate to ReferenceHeight (Altitude UAV climbs to in
            % Takeoff Mission Mode).
            fprintf(fileID,'1\t0\t0\t22\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t1\n',[0,0,0,0,waypoints(1,1:2),obj.CoverageSpace.ReferenceHeight]);
            % Compose file content
            % The file is in the following format:
            %
            % QGC WPL <VERSION>
            % <INDEX> <CURRENT WP> <COORD FRAME> <COMMAND> <PARAM1> <PARAM2> <PARAM3> <PARAM4> <PARAM5/Y/LATITUDE> <PARAM6/X/LONGITUDE> <PARAM7/Z/ALTITUDE> <AUTOCONTINUE>
            %<COMMAND> is set to 16 which denotes waypoint.
            fileContent = [(2:numWaypoints+1)',zeros(numWaypoints,1),zeros(numWaypoints,1),repmat(16,numWaypoints,1),zeros(numWaypoints,1),zeros(numWaypoints,1),zeros(numWaypoints,1),zeros(numWaypoints,1),missionWP(:,1:2),missionWP(:,3),ones(numWaypoints,1)];

            % Write the content to the file.
            % Transpose file content so that it is column order
            fprintf(fileID,'%u\t%u\t%u\t%u\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%u\n',fileContent');
            % % Write landing waypoint
            landContent = [numWaypoints+1,0,0,21,0,0,0,0,waypoints(end,1:2),waypoints(end,3),1];
            fprintf(fileID,'%u\t%u\t%u\t%u\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%.9f\t%u\n',landContent');
        end
    end

    methods (Access =?uav.internal.InternalAccess)

        function validateSolnInfo(obj,solnInfo)
            %validateSolnInfo validate solution info for validate
            %properties.
            %No polygons in coverage space.
            if (numel(obj.CoverageSpace.Polygons)==0)
                error(message("uav:robotuav:coverageplanner:EmptySpace"));
            end
            validateattributes(solnInfo, {'struct'}, {'scalar', 'nonempty'}, 'solnInfo');
            %Check if required fields exist in structure.
            if (~(isfield(solnInfo,'Takeoff'))||(~isfield(solnInfo,'Landing'))||(~isfield(solnInfo,'VisitingSequence'))||(~isfield(solnInfo,'SweepPattern')))
                error(message('uav:robotuav:coverageplanner:InvalidSolnInfo'));
            end

            %Validate takeoff and landing fields.
            validateattributes(solnInfo.Takeoff, {'double'}, {"real", "nonempty","finite", "vector", "ncols", 3}, "uavCoveragePlanner", 'Takeoff');
            validateattributes(solnInfo.Landing, {'double'}, {"real", "nonempty","finite", "vector", "ncols", 3}, "uavCoveragePlanner", 'Landing');

            %Basic validation for visiting sequence and sweep pattern
            validateattributes(solnInfo.VisitingSequence, {'int32'}, {"real", "nonempty","finite", "vector","numel", numel(obj.CoveragePolygons)}, "uavCoveragePlanner", 'VisitingSequence');
            validateattributes(solnInfo.SweepPattern, {'int32'}, {"real", "nonempty","finite", "vector",">=", 1, "<=", 4, "numel", numel(obj.CoveragePolygons)}, "uavCoveragePlanner", 'SweepPattern');
            %Visiting Sequence is non unique.
            expectedSequence=int32(1:1:length(obj.CoverageSpace.Polygons));
            if any(sort(solnInfo.VisitingSequence)~=expectedSequence)
                error(message('uav:robotuav:coverageplanner:SequenceNumbering'));
            end
        end


        function validateConstructorInput(obj,cs)
            %validateConstructorInput
            validateattributes(cs, {'uavCoverageSpace'}, {'scalar', 'nonempty'}, 'uavCoverageSpace');
            obj.CoverageSpace=cs;
        end

        function [waypoints,sweepCost]=computeLocalWaypoints(obj,solnInfo)
            %computeLocalWaypoints compute intermediate coordinates of
            %coverage excluding takeoff and landing in local coordinate
            %frame. In case of LLA based planning, the waypoints are in ENU
            %frame. In local coordinates based polygon planning, the
            %waypoints are agnostic to frame of reference.
            sweepCost=0;

            waypoints=[];
            for i=1:1:numel(solnInfo.VisitingSequence)
                polygonIndex=solnInfo.VisitingSequence(i);
                sweepIndex=solnInfo.SweepPattern(i);
                switch sweepIndex
                    case 1
                        polygonpath=obj.CoveragePolygons(polygonIndex).Normal;
                    case 2
                        polygonpath=obj.CoveragePolygons(polygonIndex).Reverse;
                    case 3
                        polygonpath=obj.CoveragePolygons(polygonIndex).CCWNormal;
                    case 4
                        polygonpath=obj.CoveragePolygons(polygonIndex).CCWReverse;
                    otherwise
                        polygonpath=[];
                end
                waypoints=[waypoints;polygonpath]; %#ok<AGROW>
                %Find euclidean distance of polygon path
                sweepCost=sweepCost+uav.internal.coveragePlanner.sweepDistance(polygonpath);
            end
        end

        function out=getLLAOutputFormat(obj,waypoints,frame)
            %getLLAOutputFormat Convert to LLA coordinates and adjust to
            %global reference altitude. Used to displace coverage mission
            %points excluding takeoff and landing to referenceHeight.
            if(nargin<3)
                frame="ENU";
            end
            out=convertToLLA(obj,waypoints,frame);
            %Adjust to global altitude.
            out(:,3)=obj.CoverageSpace.ReferenceHeight;
        end

        function out=convertToLLA(obj,waypoints,frame)
            %convertToLLA convert to LLA coordinates without adjusting
            %coordinates to reference height. This is useful when
            %converting takeoff and landing to LLA reference frame without
            %altering takeoff-landing altitude.
            if(strcmp(frame,"ENU"))
                out= enu2lla(waypoints,obj.CoverageSpace.ReferenceLocation,'flat');
            else
                out= ned2lla(waypoints,obj.CoverageSpace.ReferenceLocation,'flat');
            end
        end

        function  [waypoints,pathSolnObj]=planCoverage(obj,varargin)
            %planCoverage
            [Takeoff,Landing,TakOffFormatOutput,LandingFormatOutput,optionalArguments]=getTakeoffLanding(obj,varargin{:});
            %Check if VisitOrderSpecified
            VisitOrderSpecified=(~isempty(obj.SolverParameters.VisitingSequence));
            if(~VisitOrderSpecified)
                %Pre-fill for C interface array if sequence is not
                %specified to lock in size.
                SpecifiedSequence=ones(1,numel(obj.CoveragePolygons));
            else
                %reduce by 1 for indexing in C++
                SpecifiedSequence=obj.SolverParameters.VisitingSequence-1;
            end
            %Assemble start and end poses.
            startPoses=zeros(1,0);
            endPoses=zeros(1,0);
            coder.varsize('startPoses');
            coder.varsize('endPoses');
            for i=1:1:length(obj.CoveragePolygons)
                startPoses=[startPoses,reshape(obj.CoveragePolygons(i).Start',1,[])]; %#ok<AGROW>
                endPoses=[endPoses,reshape(obj.CoveragePolygons(i).Exit',1,[])]; %#ok<AGROW>
            end
            %Assemble Adjacency
            adjacencyInfo=zeros(1,0);
            AdjacencyMatrix=uav.internal.coveragePlanner.computeAdjacency(obj.CoverageSpace.PolygonsInternal);
            coder.varsize('adjacencyInfo');
            adjacencyInfo=[adjacencyInfo,reshape(AdjacencyMatrix',1,[])];
            switch obj.SolverAlgorithmInternal
                case 'Exhaustive'
                    exhaustivePlannerVerboseMode(obj,optionalArguments{:});
                    coder.internal.errorIf(numel(obj.CoveragePolygons)>=obj.ExhaustivePlannerRegionLimit, ...
                        'uav:robotuav:coverageplanner:LargeProblemSubset');
                    pathSolnObj = uav.internal.coveragePlanner.coverageBuiltins.connectExhaustivePath...
                        (VisitOrderSpecified,SpecifiedSequence,obj.SolverParametersInternal.MinAdjacencyCount,numel(obj.CoverageSpace.PolygonsInternal),adjacencyInfo,startPoses,endPoses,Takeoff,Landing);
                case 'MinTraversal'
                    pathSolnObj=planMinTraversalPathWrapper(obj,VisitOrderSpecified,SpecifiedSequence,adjacencyInfo,startPoses,endPoses,Takeoff,Landing);
                otherwise
                    pathSolnObj=uav.internal.coveragePlanner.coverageBuiltins.getStructParameters(int32(0),int32(0),0,0);
            end
            if(pathSolnObj.IsPathFound)
                %Assemble Path
                [waypoints,sweepCost]=computeLocalWaypoints(obj,pathSolnObj);
                if(~obj.CoverageSpace.UseLocalCoordinates)
                    waypoints=getLLAOutputFormat(obj,waypoints);
                end
                %Add sweep cost to distance cost.
                pathSolnObj.DistanceCost=pathSolnObj.DistanceCost+sweepCost;
                %Append Takeoff and Landing to the waypoints.
                waypoints=[TakOffFormatOutput;waypoints;LandingFormatOutput];
            else
                %Revert to no solution values.
                pathSolnObj.DistanceCost=0;
                pathSolnObj.TransitionCost=0;
                waypoints=[];
                pathSolnObj.VisitingSequence=int32(zeros(1,numel(obj.CoveragePolygons)));
                pathSolnObj.SweepPattern=int32(zeros(1,numel(obj.CoveragePolygons)));
            end
            %Add Takeoff and Landing to pathSolnObj.
            pathSolnObj.Takeoff=TakOffFormatOutput;
            pathSolnObj.Landing=LandingFormatOutput;
        end

        function exhaustivePlannerVerboseMode(obj,varargin)
            %exhaustivePlannerVerboseMode notify user if time to compute is
            %larger and provide solutions.
            parser = robotics.core.internal.NameValueParser(...
                {'VerboseMode'}, {true});
            parse(parser, varargin{:});
            isVerbose = parameterValue(parser, 'VerboseMode');
                if((numel(obj.CoveragePolygons)>7)&&(isVerbose))&&(coder.target('MATLAB'))
                    disp(message("uav:robotuav:coverageplanner:ExhaustiveComputation").getString);
                end
        end

        function [takeoffLocal,landingLocal,takeoffOutput,landingOutput,optionalArguments]=getTakeoffLanding(obj,takeoff,varargin)
            %computeLocalCoordinates
            narginchk(2,5);
            if(nargin==2)
                optionalArguments=cell.empty; %#ok
                landing=takeoff;
                optionalArguments={"VerboseMode",false};
            elseif ~isnumeric(varargin{1})
                landing=takeoff;
                optionalArguments={"VerboseMode",false};
            else
                landing=varargin{1};
                optionalArguments={"VerboseMode",false};
                for i=2:1:numel(varargin)
                    optionalArguments{i}=varargin{i};
                end
            end

            validateattributes(takeoff, {'double'}, {"real", "nonempty","finite", "vector", "ncols", 3}, "uavCoveragePlanner", 'Takeoff');
            validateattributes(landing, {'double'}, {"real", "nonempty","finite", "vector", "ncols", 3}, "uavCoveragePlanner", 'Landing');
            if(~obj.CoverageSpace.UseLocalCoordinates)
                %Convert to LLA.
                takeoffLocal= lla2enu(takeoff,obj.CoverageSpace.ReferenceLocation,'flat');
                landingLocal= lla2enu(landing,obj.CoverageSpace.ReferenceLocation,'flat');
                takeoffOutput=takeoff;
                landingOutput=landing;
            else
                takeoffLocal=takeoff;
                landingLocal=landing;
                takeoffOutput=takeoff;
                landingOutput=landing;
            end
        end

    end

    methods (Access =?uav.internal.InternalAccess)
        function computeCoveragePolygons(obj)
            %computeCoveragePolygons
            %Step 1 Compute sweep angles for given polygons.
            computeOptimalSweepAngles(obj)
            %Step 2 Assemble Coverage Alternatives.
            obj.CoveragePolygons=uav.internal.coveragePlanner.coveragePattern(obj.CoverageSpace.PolygonsInternal,obj.OptimalSweepAngleInternal,obj.CoverageSpace.UnitWidth,obj.CoverageSpace.Sidelap,obj.CoverageSpace.UnitLength,obj.CoverageSpace.Overlap,obj.CoverageSpace.ReferenceHeight);
        end

        function computeOptimalSweepAngles(obj)
            %computeOptimalSweepAngles compute sweep angles for
            %coverageSpace polygons as a step prior to planning.
            %This function must be executed prior to planning as certain
            %combinations of sidespacing can lead to UAV exiting the path
            %for concave configurations.
            for index=1:1:numel(obj.CoverageSpace.Polygons)
                swp=obj.CoverageSpace.Polygons(index).SweepAngle;
                if(isnan(swp))
                    %Unspecified sweep angle. Compute it for the given
                    %Polygon.Error if no such angle found. (for concave
                    %polygon)
                    [~,min_angle] = uav.internal.coveragePlanner.polygonSweepDirection(obj.CoverageSpace.PolygonsInternal{index},obj.CoverageSpace.UnitWidth,obj.CoverageSpace.Sidelap);
                    %Convert to degrees.
                    obj.OptimalSweepAngleInternal(index)=rad2deg(min_angle);
                else
                    %Specified angle. Validate if the path is interruptible and then assign.
                    rotatedPolygon=uav.internal.coveragePlanner.rotatePoints(obj.CoverageSpace.PolygonsInternal{index},swp,false);
                    sweepLines = uav.internal.coveragePlanner.horizontalSweepLines(rotatedPolygon,obj.CoverageSpace.UnitWidth,obj.CoverageSpace.Sidelap);
                    [~,isInterruptible]=uav.internal.coveragePlanner.coverageIntersections(rotatedPolygon,sweepLines);
                    coder.internal.errorIf(isInterruptible, ...
                        'uav:robotuav:coverageplanner:InterceptedSweepDir');
                    obj.OptimalSweepAngleInternal(index)=swp;

                end
            end
        end

        function pathSolnObj=planExhaustivePathWrapper(obj,visitOrderSpecified,specifiedSequence,startPoses,endPoses,adjacencyInfo,takeoff,landing)
            %planExhaustivePathWrapper
            pathSolnObj = uav.internal.coveragePlanner.coverageBuiltins.connectExhaustivePath...
                (visitOrderSpecified,specifiedSequence,obj.SolverParametersInternal.MinAdjacencyCount,numel(obj.CoverageSpace.PolygonsInternal),adjacencyInfo,startPoses,endPoses,takeoff,landing);

        end

        function pathSolnObj=planMinTraversalPathWrapper(obj,visitOrderSpecified,specifiedSequence,adjacencyInfo,startPoses,endPoses,takeoff,landing)
            %planMinTraversalPathWrapper
            startingArea_0=(obj.SolverParametersInternal.StartingArea-1);
            pathSolnObj = uav.internal.coveragePlanner.coverageBuiltins.connectMinTraversalPath...
                (visitOrderSpecified,specifiedSequence,startingArea_0,numel(obj.CoverageSpace.PolygonsInternal),adjacencyInfo,startPoses,endPoses,takeoff,landing);
        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)
            copySpace = copy(obj.CoverageSpace);
            copyObj=uavCoveragePlanner(copySpace,'Solver',obj.SolverAlgorithm,'SolverParameters',obj.SolverParameters);
        end
    end

    methods (Static, Hidden)

        function [filepath,name,ext]=validateGetFilename(filename)
            %validateFilename
            validateattributes(filename, ["char", "string"], "scalartext", "uavCoveragePlanner", "FileName");
            [filepath,name,ext] = fileparts(filename);
            validatestring(ext, [".txt",".waypoints"], "exportWaypointsPlan", "File extension");
        end

        function props = matlabCodegenNontunableProperties(~)
            % In codegen, explicitly set the algorithm to non tunable to
            % prevent errors.
            props = {'SolverAlgorithmInternal'};
        end
    end
end

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

% LocalWords:  solverparams coverageplanner sidespacing COVERAGESPACE cp wp soln custommission
% LocalWords:  Mechatronics repeat:plan repeat:Takeoff repeat:Landing EXPORTWAYPOINTSPLAN QGC WPL
% LocalWords:  AUTOCONTINUE
