classdef uavPlatform < handle
%uavPlatform Create UAV platform to mount sensors in scenario
%   The uavPlatform object represents a UAV platform in a given UAV
%   scenario. Use the platform to define and track the trajectory of an
%   object in the scenario. Mount sensors like the gpsSensor,
%   insSensor, or uavLidarPointCloudGeneration to simulate sensor
%   readings for the platform. Add a body mesh for visualization or set
%   geo-fencing limitations and check permissions.
%
%   PLATFORM = uavPlatform(NAME, SCENARIO) creates a platform with a
%   specified string NAME and adds it to the SCENARIO given as a
%   uavScenario object.
%
%   PLATFORM = uavPlatform(___, Name, Value) specifies additional
%   parameters as name-value pairs in any order. You can specify
%   property names as name-value pairs, and additional name-value pairs
%   are:
%      ReferenceFrame              - Decides which inertial frame
%                                    platform's motion is relative to.
%                                    Default: "NED"
%      StartTime                   - Initial time that platform starts
%                                    trajectory.
%                                    Default: 0
%      Trajectory                  - waypointTrajectory or 
%                                    polynomialTrajectory for the platform.
%                                    If not provided, UAV must be moved at 
%                                    each simulation step using move 
%                                    method.
%                                    Default: []
%      InitialPosition             - Initial platform position for the
%                                    UAV if it is not associated with a
%                                    trajectory.
%                                    Default: [0 0 0]
%      InitialOrientation          - Initial platform orientation for
%                                    the UAV if it is not associated
%                                    with a trajectory.
%                                    Default: [1 0 0 0]
%      InitialVelocity             - Initial platform velocity if it is
%                                    not associated with a trajectory.
%                                    Default: [0 0 0]
%      InitialAcceleration         - Initial platform acceleration if
%                                    it is not associated with a
%                                    trajectory.
%                                    Default: [0 0 0]
%      InitialAngularVelocity      - Initial platform angular velocity
%                                    if it is not associated with a
%                                    trajectory.
%                                    Default: [0 0 0]
%
%   uavPlatform properties:
%      Name                - Identifier for UAV platform
%      Trajectory          - Trajectory for UAV platform
%      ReferenceFrame      - Reference frame for computing UAV platform motion
%      Mesh                - UAV platform body mesh
%      MeshColor           - UAV platform body mesh color
%      MeshTransform       - Transform between UAV platform body and mesh frames
%      Sensors             - Sensors mounted on UAV platform
%      GeoFences           - Geo-fence restrictions for UAV platform
%
%   uavPlatform methods:
%      move                - Move UAV platform in scenario at current simulation step.
%      read                - Read UAV platform motion vector.
%      updateMesh          - Update UAV platform body mesh.
%      addGeoFence         - Add geographical fencing to UAV platform.
%      checkPermission     - Check permission based on geo-fencing.
%      addWind             - Add wind model to platform.
%      removeWind          - Remove wind model from platform.
%      windDetails         - Return details of wind model objects added to UAV platform.
%      windVelocity        - Return wind translational velocity on UAV platform.
%      windAngularRate     - Return wind angular velocity on UAV platform.
%
%   Example:
%      % Create scenario.
%      scenario = uavScenario;
%
%      % Create platform with trajectory.
%      traj = waypointTrajectory("SampleRate", 10, ...
%         "TimeOfArrival", [0 0.5], "Waypoints", [1 0 0; 2 0 0]);
%      platform = uavPlatform("UAV", scenario, "Trajectory", traj);
%
%      % Setup the scenario and gather initial platform pose.
%      setup(scenario);
%      motion = read(platform);
%
%      % Advance the scene and gather motion of the platform.
%      advance(scenario);
%      motion = read(platform);
%
%   See also uavScenario, uavSensor

%   Copyright 2020-2023 The MathWorks, Inc.

    properties (Dependent, SetAccess = private)
        %Name Identifier for UAV platform
        %   Name is a scalar string that identifies the platform in the
        %   scenario. It must be unique within the scenario.
        Name

        %ReferenceFrame Reference frame for computing UAV platform motion
        %   ReferenceFrame is a scalar string that matches one of the
        %   scenario's inertial frame names. All platform motion vector is
        %   computed relative to this inertial frame.
        %
        %   Default: "NED"
        ReferenceFrame

        %Trajectory Trajectory for UAV platform
        %   Trajectory can either be a waypointTrajectory object, 
        %   polynomialTrajectory object or empty. When Trajectory is a 
        %   waypointTrajectory or polynomialTrajectory, Platform is moved
        %   along the trajectory during the scenario simulation. Otherwise,
        %   platform must be moved by user using move method.
        %
        %   Default: []
        Trajectory

        %Mesh UAV platform body mesh
        %   Mesh is an extendedObjectMesh object that describes the 3D
        %   model of the platform. It is used to generate 3D point cloud.
        %
        %   Default: a box with [1 0.5 0.3] as [length width height]
        Mesh

        %MeshColor UAV platform body mesh color
        %   MeshColor is a 1x3 vector defining platform body mesh color.
        %
        %   Default: [1 0 0]
        MeshColor

        %MeshTransform Transform between UAV platform body and mesh frames
        %   MeshTransform is a 4x4 homogeneous transform that maps points
        %   in platform body mesh frame to points in platform body frame.
        %
        %   Default: eye(4)
        MeshTransform

        %GeoFences Geo-fence restrictions for platform
        %   GeoFences is a 1xn struct array. Each element has two fields:
        %   Geometry and Permission. Geometry is an extendedObjectMesh
        %   object representing the shape of the geofence, relative to ENU
        %   frame. Permission is a logical scalar that indicates platform
        %   is permitted or not inside the geofence.
        GeoFences
    end

    properties (SetAccess = {?uavPlatform, ?uavSensor})
        %Sensors Sensors mounted on UAV platform
        %   Sensors is a 1xN vector of uavSensor objects
        Sensors
    end

    properties (Access={?uavPlatform, ...
                        ?uavSensor, ...
                        ?uav.internal.tabcompletion.scenario.TabUtils, ...
                        ?uav.SensorAdaptor, ...
                        ?uav.internal.scenario.Sensor , ...
                        ?uav.internal.scenario.SensorInterface , ...
                        ?uav.internal.scenario.sensor.SensorAdaptor, ...
                        ?uav.sluav.internal.scenario.Platform, ...
                        ?matlab.unittest.TestCase})
        %ScenarioImpl Implementation of the UAV scenario
        ScenarioImpl

        %PlatformImpl Implementation of the UAV platform
        PlatformImpl
    end

    methods
        function traj = get.Trajectory(obj)
            traj = obj.PlatformImpl.Trajectory;
        end

        function name = get.ReferenceFrame(obj)
            name = obj.PlatformImpl.ReferenceFrame;
        end

        function name = get.Name(obj)
            name = obj.PlatformImpl.Name;
        end

        function mesh = get.Mesh(obj)
            mesh = obj.PlatformImpl.Mesh.Mesh;
        end

        function color = get.MeshColor(obj)
            color = obj.PlatformImpl.Mesh.Color;
        end

        function transform = get.MeshTransform(obj)
            transform = obj.PlatformImpl.Mesh.Transform;
        end

        function fences = get.GeoFences(obj)
            fences = struct("Geometry", {}, "Permission", {});
            for idx = 1:numel(obj.PlatformImpl.GeoFences)
                fences(idx) = struct("Geometry", obj.PlatformImpl.GeoFences{idx}.Shape.Mesh, ...
                                     "Permission", obj.PlatformImpl.GeoFences{idx}.Permission);
            end

        end

    end

    methods
        function obj = uavPlatform(name, scenario, varargin)
        %uavPlatform

        % validate inputs
            validateattributes(scenario, "uavScenario", "scalar", "uavPlatform", "scenario");
            name = convertCharsToStrings(name);
            validateattributes(name, ["char", "string"], "scalartext", "uavPlatform", "name");
            if any(strcmp(name, [scenario.Platforms.Name]))
                error(message("uav:robotuav:scenario:PlatformNameNotUnique", name));
            end

            % parse inputs
            p = inputParser;
            p.addParameter("ReferenceFrame", uav.internal.scenario.Scene.NEDFrameName, ...
                           @(x)validateattributes(x, ["char", "string"], "scalartext", "uavPlatform", "ReferenceFrame"));
            p.addParameter("Trajectory", [],...
                           @(x)validateattributes(x, ["waypointTrajectory","polynomialTrajectory"], "scalar", "uavPlatform", "Trajectory"));
            p.addParameter("InitialPosition", [0 0 0], ...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "uavPlatform", "InitialPosition"));
            p.addParameter("InitialOrientation", [1 0 0 0]);
            p.addParameter("InitialVelocity", [0 0 0], ...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "uavPlatform", "InitialVelocity"));
            p.addParameter("InitialAcceleration", [0 0 0], ...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "uavPlatform", "InitialAcceleration"));
            p.addParameter("InitialAngularVelocity", [0 0 0], ...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "uavPlatform", "InitialAngularVelocity"));
            p.addParameter("StartTime", scenario.ScenarioImpl.StartTime, ...
                           @(x)validateattributes(x, "numeric", {">=", scenario.ScenarioImpl.StartTime}, "uavPlatform", "StartTime"));
            p.parse(varargin{:});

            if scenario.IsRunning
                error(message("uav:robotuav:scenario:CannotAddPlatformToRunningScenario"));
            end

            obj.ScenarioImpl = scenario.ScenarioImpl;
            traj = p.Results.Trajectory;

            if isempty(traj)
                q = robotics.internal.validation.validateQuaternion(p.Results.InitialOrientation, "uavPlatform", "InitialOrientation");
                motion = [double(p.Results.InitialPosition), double(p.Results.InitialVelocity), ...
                          double(p.Results.InitialAcceleration), double(q), ...
                          double(p.Results.InitialAngularVelocity)];
                obj.PlatformImpl = uav.internal.scenario.Platform(...
                    obj.ScenarioImpl, p.Results.ReferenceFrame, name, double(p.Results.StartTime), motion);
            else
                if ~strcmp(p.Results.ReferenceFrame, traj.ReferenceFrame)
                    error(message("uav:robotuav:scenario:MismatchTrajectoryReferenceFrame", ...
                                  traj.ReferenceFrame, p.Results.ReferenceFrame));
                end
                obj.PlatformImpl = uav.internal.scenario.Platform(...
                    obj.ScenarioImpl, p.Results.ReferenceFrame, name, double(p.Results.StartTime), traj, varargin);
            end
            obj.ScenarioImpl.addPlatform(obj.PlatformImpl);
            obj.Sensors = uavSensor.empty;
            scenario.Platforms(end+1) = obj;
        end

        function addWind(obj, winds)
            % addWind adds wind models to the uavPlatform
            %   addWind(PLAT,  WindData) Add wind model based on WindData
            %   to uavPlatform PLAT. The WindData is used during scenario
            %   simulation to generate wind translational and angular
            %   velocities. WindData can be a scalar or cell array of
            %   uavWindSteady, uavWindGust or uavWindTurbulence objects.

            if obj.ScenarioImpl.IsRunning
                error(message("uav:robotuav:scenario:CannotAddWindWhileScenarioRunning"));
            end

            windClassValidator = @(wind)validateattributes(wind, ...
                    ["uavWindSteady", "uavWindGust", "uavWindTurbulence"], ...
                    "scalar", ...
                    "addWind", "winds");
            if iscell(winds)
                cellfun(windClassValidator, winds);
            else
                windClassValidator(winds);
            end

            obj.PlatformImpl.addWind(winds);
        end

        function removeWind(obj, ID)
            % removeWind Remove wind models from platform
            %   removeWind(PLAT, ID) removes the wind model identified by
            %   ID. ID is a vector containing integers corresponds to wind
            %   models added to the platform.

            arguments
                obj uavPlatform
                ID (1,:) {mustBeInteger, mustBeNonempty, mustBePositive} 
            end

            if obj.ScenarioImpl.IsRunning
                error(message("uav:robotuav:scenario:CannotRemoveWindWhileScenarioRunning"));
            end

            validateattributes(ID, "numeric", {'<=', numel(obj.PlatformImpl.WindData)}, "removeWind", "ID");
            obj.PlatformImpl.removeWind(ID);
        end

        function v = windVelocity(obj, ID)
            % windVelocity Returns wind velocity on platform at current time
            %   VWind = windVelocity(PLAT) returns the wind velocity VWind
            %   (m/s) for the given platform PLAT at current time in
            %   scenario simulation.  VWind is a 1x3 vector for the wind
            %   translational velocity in the platform's body frame. 
            %
            %   VWind = windVelocity(PLAT, ID) returns the wind velocity
            %   VWind (m/s) for the given platform PLAT at current time in
            %   scenario simulation generated by the wind model identified
            %   by ID. ID is a N element vector and VWind is a Nx3 vector
            %   for the wind velocities in the platform's body frame. Each
            %   row in VWind is generated by the wind model identified by
            %   the corresponding element in ID.

            if ~obj.ScenarioImpl.IsRunning
                error(message("uav:robotuav:scenario:CannotGetWindVelocityWhenScenarioNotRunning"));
            end

            if nargin == 1
                v = obj.PlatformImpl.windVelocity();
            else
                validateattributes(ID, "double", ...
                    {"vector","integer","positive","<=",numel(obj.PlatformImpl.WindData)}, ...
                    "windVelocity", "ID");
                v = obj.PlatformImpl.windVelocity(ID);
            end
        end

        function omegaWind = windAngularRate(obj, ID)
            % windAngularRate Returns wind angular rate on platform at current time
            %   omegaWind = windAngularRate(OBJ) returns the resultant wind
            %   angular velocity omegaWind (rad/s) on the platform OBJ
            %   generated by all wind models on the platform. omegaWind is
            %   a 1x3 vector for the wind angular velocity in the
            %   platform's body frame.
            %
            %   omegaWind = windAngularRate(OBJ, ID) returns the wind
            %   angular velocity omegaWind (rad/s) on the platform OBJ
            %   generated by the wind model identified by ID. ID is a N
            %   element vector and omegaWind is a Nx3 vector for the wind
            %   angular velocities in the platform's body frame. Each row
            %   in omegaWind is generated by the wind model identified by
            %   the corresponding element in ID.

            if ~obj.ScenarioImpl.IsRunning
                error(message("uav:robotuav:scenario:CannotGetWindAngularRateWhenScenarioNotRunning"));
            end

            if nargin == 1
                omegaWind = obj.PlatformImpl.windAngularRate();
            else
                validateattributes(ID, "double", ...
                    {"vector","integer","positive","<=",numel(obj.PlatformImpl.WindData)}, ...
                    "windAngularRate", "ID");
                omegaWind = obj.PlatformImpl.windAngularRate(ID);
            end
        end

        function tbl = windDetails(obj)
            %windDetails Returns wind model details as a table
            %    TBL = windDetails(OBJ) returns the details of the wind
            %    model added to OBJ as a table TBL. The first column is ID
            %    starting from 1 to N, where N is the number of wind models
            %    added to PLAT. The second column contains cell array of
            %    wind model data objects.

            WindProperties = obj.PlatformImpl.WindData';
            ID = transpose(1:numel(WindProperties));

            tbl = table(ID, WindProperties);

        end

        function updateMesh(obj, type, geometries, color, varargin)
        %updateMesh Update the body mesh of the platform
        %   updateMesh(PLATFORM, TYPE, GEOMETRIES, COLOR, POSITION,
        %   ORIENTATION) updates the UAV platform's body mesh. Specify
        %   the Type as a string with corresponding GEOMETRIES
        %   parameters:
        %      - Type:"fixedwing", GEOMETRIES:{scale}. This sets a
        %      fixedwing mesh to the platform body frame. The size of
        %      the mesh is determined by a scalar scale. The center of
        %      the mesh frame is at the center of the fixedwing mesh. X
        %      axis forward, y Axis to the left, z Axis up.
        %      - Type:"quadrotor", GEOMETRIES:{scale}. This sets a
        %      quadrotor mesh to the platform body frame. The size of
        %      the mesh is determined by a scalar scale. The center of
        %      the mesh frame is at the center of the quadrotor mesh. X
        %      axis forward, y Axis to the left, z Axis up.
        %      - Type:"cuboid", GEOMETRIES:{[x, y, height]}. This sets
        %      a cuboid mesh to the platform body frame. The size of
        %      the mesh is determined by x, y, height. The center of
        %      the mesh frame is at the center of the cuboid mesh. X
        %      axis forward, y Axis to the left, z Axis up.
        %      - Type:"custom", GEOMETRIES:{vertices, faces}. This sets
        %      a mesh defined by vertices and faces.
        %   COLOR describes the mesh color. Mesh is offset from the
        %   platform's body frame linearly by POSITION and rotated by
        %   ORIENTATION. POSITION is a 1x3 vector. ORIENTATION is a 1x4
        %   vector or a scalar quaternion.
        %   updateMesh(PLATFORM, TYPE, GEOMETRIES, COLOR, OFFSET) set
        %   the mesh relative to the platform's body frame using 4x4
        %   transform matrix OFFSET, which describes the transform that
        %   transforms points in the mesh frame to body frame

            type = validatestring(type, uav.internal.scenario.Constants.BodyMeshType, "updateMesh", "type");
            switch type
              case "fixedwing"
                validateattributes(geometries, "cell", {"numel", 1}, "updateMesh", "geometries");
                validateattributes(geometries{1}, "numeric", ["scalar", "positive", "finite"], "updateMesh", "geometries");

              case "quadrotor"
                validateattributes(geometries, "cell", {"numel", 1}, "updateMesh", "geometries");
                validateattributes(geometries{1}, "numeric", ["scalar", "positive", "finite"], "updateMesh", "geometries");

              case "cuboid"
                validateattributes(geometries, "cell", {"numel", 1}, "updateMesh", "geometries");
                validateattributes(geometries{1}, "numeric", {"2d", "nrows", 1, "ncols", 3, "positive", "finite"}, "updateMesh", "geometries");

              case "custom"
                validateattributes(geometries, "cell", {"numel", 2}, "updateMesh", "geometries");
                validateattributes(geometries{1}, "numeric", {"2d", "ncols", 3, "real", "finite"}, "updateMesh", "geometries");
                validateattributes(geometries{2}, "numeric", {"2d", "ncols", 3, "integer", "positive"}, "updateMesh", "geometries");
            end
            geometries = cellfun(@double, geometries, "UniformOutput", false);

            validateattributes(color, "numeric", {"2d", "nrows", 1, "ncols", 3, "real", "nonnegative", ">=", 0, "<=", 1}, "updateMesh", "color");
            color = double(color);

            if nargin == 5
                offset = varargin{1};
                validateattributes(offset, "numeric", {"2d", "nrows", 4, "ncols", 4, "real", "finite"}, "updateMesh", "offset");
                offset = double(offset);
            else
                position = varargin{1};
                validateattributes(position, "numeric", {"nrows", 1, "ncols", 3, "real", "finite"}, "updateMesh", "position");
                validateattributes(varargin{2}, ["quaternion", "numeric"], {'finite'}, "updateMesh", "orientation");
                validateattributes(norm(varargin{2}), ["quaternion", "numeric"], {'nonzero'}, "updateMesh", "orientation norm");
                orientation = robotics.internal.validation.validateQuaternion(varargin{2}, "updateMesh", "orientation");
                offset = trvec2tform(double(position))*quat2tform(double(orientation));
            end

            obj.PlatformImpl.setMesh(type, geometries, color, offset);
        end

        function addGeoFence(obj, type, geometries, permission, varargin)
        %addGeoFence Add geographical fencing to UAV platform
        %   addGeoFence(OBJ, TYPE, GEOMETRIES, PERMISSION) adds a
        %   geofence with coordinates defined in ENU coordinates.
        %   Specify the Type as a string with corresponding GEOMETRIES
        %   parameters:
        %      - Type:"cylinder", GEOMETRIES:{[centerx, centery,
        %      radius], [zmin, zmax]}
        %      - Type:"polygon", GEOMETRIES:{[endptsX endptsY],
        %      [zmin, zmax]}. End points of the polygon should be
        %      specified in either clockwise or counter clockwise
        %      order.
        %   Set PERMISSION to true to only allow the platform to
        %   operate inside the geofence when calling checkPermission.
        %   Set PERMISSION to false to only allow the platform to
        %   operate outside the geofence when calling checkPermission.
        %
        %   addGeoFence(___, Name, Value) additionally take in the
        %   following name value pairs:
        %      UseLatLon       - When set to true, x-y coordinates in
        %                        the GEOMETRIES are in lat-lon
        %                        coordinates.
        %
        %                        Default: false
        %
        %      ReferenceFrame  - Define GEOMETRIES relative to
        %                        ReferenceFrame, which must be one of
        %                        the scenario's inertial frames.
        %
        %                        Default: "ENU"


            type = validatestring(type, uav.internal.scenario.Constants.GeometryShapeType, "addGeoFence", "type");
            switch type
              case "cylinder"
                validateattributes(geometries, "cell", {"numel", 2}, "addGeoFence", "geometries");
                validateattributes(geometries{1}, "numeric", {"2d", "nrows", 1, "ncols", 3}, "addGeoFence", "geometries{1}");
                validateattributes(geometries{2}, "numeric", {"2d", "nrows", 1, "ncols", 2}, "addGeoFence", "geometries{2}");

              case "polygon"
                validateattributes(geometries, "cell", {"numel", 2}, "addGeoFence", "geometries");
                validateattributes(geometries{1}, "numeric", {"2d", "ncols", 2}, "addGeoFence", "geometries{1}");
                validateattributes(geometries{2}, "numeric", {"2d", "nrows", 1, "ncols", 2}, "addGeoFence", "geometries{2}");
            end
            geometries = cellfun(@double, geometries, "UniformOutput", false);
            permission = robotics.internal.validation.validateLogical(...
                permission, "addGeoFence", "permission");

            p = inputParser();
            p.addParameter("ReferenceFrame", uav.internal.scenario.Scene.ENUFrameName, ...
                           @(x)validateattributes(x, ["char", "string"], "scalartext", "addGeoFence", "ReferenceFrame"));
            p.addParameter("UseLatLon", false, ...
                           @(x)validateattributes(x, ["logical", "numeric"], "binary", "addGeoFence", "UseLatLon"));
            p.parse(varargin{:});

            if logical(p.Results.UseLatLon)
                obj.PlatformImpl.addGeoFence(type, geometries, logical(permission));
            else
                obj.PlatformImpl.addGeoFence(type, geometries, logical(permission), p.Results.ReferenceFrame);
            end
        end

        function [motion, lla] = read(obj)
        %READ Read the UAV platform's motion vector
        %   [MOTION, LLA] = READ(OBJ) reads the latest motion of the
        %   UAV as a vector and the latitude-longitude-altitude
        %   coordinates, LLA. MOTION is a 1x16 vector including
        %   position, velocity, acceleration, orientation, angular
        %   velocity as [x,y,z,vx,vy,vz,ax,ay,az,qw,qx,qy,qz,wx,wy,wz].

            [motion, lla] = obj.PlatformImpl.read(obj.ScenarioImpl.CurrentTime);
        end

        function move(obj, motion)
        %MOVE Move the UAV platform in scenario at current simulation step
        %   MOVE(UAV, MOTION) move the UAV platform in the scenario
        %   according to MOTION, which is a 1x16 vector including
        %   position, velocity, acceleration, orientation, angular
        %   velocity as [x,y,z,vx,vy,vz,ax,ay,az,qw,qx,qy,qz,wx,wy,wz].

            narginchk(2,2);
            validateattributes(motion, "numeric", {"vector", "numel", 16, "finite", "real"}, "move", "motion");
            validateattributes(norm(motion(10:13)), "numeric", "nonzero", "move", "orientation norm");
            robotics.internal.validation.validateQuaternion(motion(10:13), "move", "orientation");
            obj.PlatformImpl.move(double(reshape(motion, 1, 16)), obj.ScenarioImpl.CurrentTime);
        end

        function permitted = checkPermission(obj, varargin)
        %checkPermission Check permission based on geo-fencing
        %   PERMITTED = checkPermission(UAV) checks the current UAV
        %   position is permitted or not according to the geofences.
        %
        %   PERMITTED = checkPermission(UAV, POSITION) checks a
        %   POSITION as [x,y,z] in ENU frame is permitted or not
        %   according to the geofences.
        %
        %   PERMITTED = checkPermission(___, Name, Value) additionally
        %   take in the following name value pairs:
        %      UseLatLon       - When set to true, POSITION is given in
        %                        lat-lon-altitude coordinates.
        %
        %                        Default: false
        %
        %      ReferenceFrame  - Define POSITION relative to
        %                        ReferenceFrame, which must be one of
        %                        the scenario's inertial frames.
        %
        %                        Default: "ENU"

            p = inputParser();
            p.addOptional("Position", [], ...
                          @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "addGeoFence", "Positions"));
            p.addParameter("ReferenceFrame", uav.internal.scenario.Scene.ENUFrameName, ...
                           @(x)validateattributes(x, ["char", "string"], "scalartext", "addGeoFence", "ReferenceFrame"));
            p.addParameter("UseLatLon", false, ...
                           @(x)validateattributes(x, ["logical", "numeric"], "binary", "addGeoFence", "UseLatLon"));
            p.parse(varargin{:});

            if logical(p.Results.UseLatLon)
                position = double(p.Results.Position);
                if isempty(position)
                    % if position is not provided, use lla of the UAV's
                    % current position
                    [~, position] = obj.read();
                end

                for idx = 1:numel(obj.PlatformImpl.GeoFences)
                    if ~obj.PlatformImpl.GeoFences{idx}.checkPermission(position)
                        permitted = false;
                        return;
                    end
                end
            else
                position = double(p.Results.Position);
                referenceFrame = p.Results.ReferenceFrame;
                if isempty(position)
                    % if position is not provided, use UAV's current motion
                    % and reference frame
                    motion = obj.read();
                    position = motion(1:3);
                    referenceFrame = obj.ReferenceFrame;
                end

                for idx = 1:numel(obj.GeoFences)
                    if ~obj.PlatformImpl.GeoFences{idx}.checkPermission(position, referenceFrame)
                        permitted = false;
                        return;
                    end
                end
            end

            permitted = true;
        end

    end

end
