classdef uavScenario < handle
    %uavScenario Generate UAV simulation scenario
    %
    %   uavScenario is used to generate a simulation scenario consisting of
    %     static meshes, UAV platforms and sensors in a 3D environment.
    %
    %   SCENE = uavScenario creates an empty scenario with NED and ENU
    %   inertial frames.
    %
    %   SCENE = uavScenario(___, Name, Value) additionally take in the
    %   following name value pairs:
    %      UpdateRate            - Simulation update rate (Hz)
    %      StopTime              - Simulation stop time (s)
    %      HistoryBufferSize     - Max number of steps stored in the scenario
    %      ReferenceLocation     - Scenario origin in geodetic coordinates
    %      MaxNumFrames          - Max number of inertial frames, platforms
    %                              and sensors
    %
    %   uavScenario properties:
    %      UpdateRate            - Simulation update rate (Hz)
    %      StopTime              - Simulation stop time (s)
    %      HistoryBufferSize     - Max number of steps stored in the scenario
    %      ReferenceLocation     - Scenario origin in geodetic coordinates
    %      MaxNumFrames          - Max number of inertial frames, platforms
    %                              and sensors
    %      CurrentTime           - Current simulation time
    %      IsRunning             - Indicates whether the scenario is running
    %                              or not
    %      TransformTree         - Storage for the transformation between
    %                              frames
    %      InertialFrames        - Names for the inertial frames in the
    %                              scenario
    %      Platforms             - A collection of uavPlatforms added to the
    %                              scenario
    %
    %   uavScenario methods:
    %      setup                 - Prepare the scenario for simulation
    %      advance               - Increment simulation time and move all
    %                              platforms
    %      updateSensors         - Update sensor readings
    %      restart               - Reset simulation scenario
    %      addInertialFrame      - Define new inertial frame in the scenario
    %      addMesh               - Define new static mesh in the scenario
    %      terrainHeight         - Return the terrain height at given location
    %      show                  - Visualize the scenario in 2D
    %      show3D                - Visualize the scenario in 3D
    %      copy                  - Creates uninitialized copy of scene
    %
    %   Example:
    %      % Create scenario
    %      scenario = uavScenario(StopTime=10);
    %
    %      % Add floor and buildings
    %      addMesh(scenario, "polygon", { ...
    %              [-100 -100; 100 -100; 100 100; -100 100], [-3 0]}, ...
    %              [0.3 0.3 0.3]);
    %      addMesh(scenario, "polygon", { ...
    %              [0 0; 30 0; 30 30; 0 30], [0 15]}, [0 0.9 0]);
    %
    %      % Add a UAV platform
    %      traj = waypointTrajectory([-75 -75 -10; 75 -75 -10; 75 75 -10], ...
    %      TimeOfArrival=[0 5 10], AutoBank=true);
    %      plat = uavPlatform("quadrotor", scenario, Trajectory=traj);
    %      updateMesh(plat, "quadrotor", {10}, [1 0 0], [0 0 0], ...
    %      eul2quat([0 0 pi]));
    %
    %      % Visualize the scenario
    %      ax = show3D(scenario);
    %
    %      % Setup simulation
    %      setup(scenario);
    %
    %      % Start simulation
    %      while (scenario.IsRunning)
    %           advance(scenario);
    %           show3D(scenario, Parent=ax, FastUpdate=true);
    %           pause(0.1);
    %      end
    %
    %      % Restart
    %      restart(scenario);
    %
    %   See also uavPlatform, uavSensor

    %   Copyright 2020-2023 The MathWorks, Inc.

    properties (Dependent, SetAccess = private)
        %UpdateRate Simulation update rate (Hz)
        %   UpdateRate is a positive scalar that determines the step size
        %   the scenario time in each advance call, which equals to
        %   1/UpdateRate.
        %
        %   Default: 10
        UpdateRate

        %StopTime Simulation stop time
        %   StopTime is a positive scalar that determines when the scenario
        %   would stop advancing.
        %
        %   Default: inf
        StopTime

        %HistoryBufferSize Max number of steps stored in the scenario
        %   HistoryBufferSize is a positive scalar that  determines how
        %   many frames of platform poses are stored within the scenario.
        %
        %   Default: 100
        HistoryBufferSize

        %ReferenceLocation Scenario origin in geodetic coordinates
        %   Specify the origin of the local coordinate system as a
        %   3-element row vector in geodetic coordinates (latitude,
        %   longitude, and altitude). Altitude is the height above the
        %   reference ellipsoid model, WGS84. The reference location is in
        %   [degrees degrees meters].
        %
        %   Default: [0 0 0]
        ReferenceLocation

        %MaxNumFrames Max number of inertial frames, platforms and sensors
        %   MaxNumFrames is a positive integer scalar that defines the
        %   upper limit on the total number of inertial frames, platforms
        %   and sensors that can be added to the scenario.
        %
        %   Default: 10
        MaxNumFrames

        %CurrentTime Current simulation time
        %   CurrentTime is a positive scalar that indicates the current
        %   simulation time.
        CurrentTime

        %IsRunning Indicates whether the scenario is running or not
        %   IsRunning is a logical scalar that indicates whether scenario
        %   is running or not. Scenario is running after it is setup, until
        %   it advance pass its stop time.
        IsRunning

        %TransformTree Storage for the transformation between frames
        %   TransformTree is a transformTree objects that stores all the
        %   transformation information between inertial frames, platform
        %   frames and sensor frames over time.
        TransformTree

        %InertialFrames Names for the inertial frames in the scenario
        %   InertialFrames is a string array that stores the names for all
        %   inertial frames defined in the scenario.
        InertialFrames

        %Meshes Cell array of static meshes in the scenario
        %   Meshes is a 1xn cell array of extendedObjectMesh representing
        %   static meshes in the scenario.
        Meshes
    end

    properties (SetAccess = {?uavScenario, ?uavPlatform})
        %Platforms A collection of uavPlatforms added to the scenario
        %   Platforms is a 1xN vector of uavPlatform objects
        %
        %   Default: uavPlatform
        Platforms
    end

    properties (Access = {?uavScenaro, ?uavPlatform, ...
            ?uav.sluav.internal.scenario.util.Scope, ...
            ?matlab.unittest.TestCase, ...
            ?uav.scenarioapp.internal.model.DataModel, ...
            ?uav.scenarioapp.internal.utility.ScenarioImportHandler, ...
            ?uav.internal.scenario.sensor.GNSSAdaptor})
        %ScenarioImpl Implementation of the scenario
        ScenarioImpl
    end

    properties (Access = {?uavScenario, ?matlab.unittest.TestCase})
        %TerrainName Name of the terrain source
        TerrainName = ""
    end

    methods
        function r = get.UpdateRate(obj)
            r = obj.ScenarioImpl.UpdateRate;
        end

        function t = get.StopTime(obj)
            t = obj.ScenarioImpl.StopTime;
        end

        function s = get.HistoryBufferSize(obj)
            s = obj.TransformTree.MaxNumTransforms;
        end

        function r = get.ReferenceLocation(obj)
            r = obj.ScenarioImpl.GeoFrame.GeoOrigin;
        end

        function n = get.MaxNumFrames(obj)
            n = obj.TransformTree.MaxNumFrames;
        end

        function t = get.CurrentTime(obj)
            t = obj.ScenarioImpl.CurrentTime;
        end

        function isRunning = get.IsRunning(obj)
            isRunning = obj.ScenarioImpl.IsRunning;
        end

        function t = get.TransformTree(obj)
            t = obj.ScenarioImpl.TFTree;
        end

        function names = get.InertialFrames(obj)
            names = obj.ScenarioImpl.InertialFrameNames;
        end

        function meshes = get.Meshes(obj)
            meshes = cell(size(obj.ScenarioImpl.Meshes));
            for idx = 1:numel(meshes)
                meshes{idx} = obj.ScenarioImpl.Meshes{idx}.Mesh;
            end
        end
    end

    methods
        function obj = uavScenario(varargin)
            %uavScenario

            % parse inputs
            p = inputParser;
            p.addParameter("StopTime", inf, ...
                @(x)validateattributes(x, "numeric", {"scalar", "real", ">=", uav.internal.scenario.Scene.StartTimeConstant}, "uavScenario", "StopTime"));
            p.addParameter("UpdateRate", 10, ...
                @(x)validateattributes(x, "numeric", ["scalar", "real", "positive", "finite"], "uavScenario", "UpdateRate"));
            p.addParameter("HistoryBufferSize", 100, ...
                @(x)validateattributes(x, "numeric", {"scalar", "real", "positive", "integer", ">=", 2}, "uavScenario", "HistoryBufferSize"));
            p.addParameter("ReferenceLocation", [0 0 0], ...
                @(x)validateattributes(x, "numeric", {"2d", "nrows", 1, "ncols", 3, "real", "finite"}, "uavScenario", "ReferenceLocation"));
            p.addParameter("MaxNumFrames", 10, ...
                @(x)validateattributes(x, "numeric", {"scalar", "real", "positive", "integer", ">=", 3}, "uavScenario", "MaxNumFrames"));
            p.parse(varargin{:});

            validateattributes(p.Results.ReferenceLocation(1), "numeric", {">=", -90, "<=", 90}, "uavScenario", "ReferenceLocationLatitude");
            validateattributes(p.Results.ReferenceLocation(2), "numeric", {">=", -180, "<=", 180}, "uavScenario", "ReferenceLocationLongitude");

            obj.ScenarioImpl = uav.internal.scenario.Scene(...
                double(p.Results.UpdateRate), double(p.Results.StopTime), ...
                double(p.Results.HistoryBufferSize), double(p.Results.ReferenceLocation), ...
                double(p.Results.MaxNumFrames));
            obj.Platforms = uavPlatform.empty;
            obj.ScenarioImpl.Parent = obj;
        end

        function addInertialFrame(obj, base, name, varargin)
            %addInertialFrame Define new inertial frame in the scenario
            %   addInertialFrame(SCENE, BASE, NAME, POSITION, ORIENTATION)
            %   adds a new inertial frame with scalar string NAME to the
            %   scenario. The new frame is defined by its POSITION and
            %   ORIENTATION relative to the inertial frame identified by
            %   BASE, which is a string scalar referring to the base
            %   inertial frame name. The POSITION is a 1x3 vector. The
            %   ORIENTATION is a 1x4 vector or a scalar quaternion.
            %
            %   addInertialFrame(SCENE, BASE, NAME, TFORM) adds a new
            %   inertial frame relative to the inertial frame based on
            %   TFORM, which is a 4x4 transform matrix that maps points in
            %   the new inertial frame to BASE.

            base = validatestring(base, obj.InertialFrames, "addInertialFrame", "base");
            name = convertCharsToStrings(name);
            validateattributes(name, "string", "scalartext", "addInertialFrame", "name");
            if any(strcmp(name, [obj.TransformTree.info.FrameNames]))
                error(message("uav:robotuav:scenario:InertialFrameAlreadyExist", name));
            end
            if nargin == 4
                tform = varargin{1};
                validateattributes(tform, "numeric", {"2d", "nrows", 4, "ncols", 4, "finite"}, "addInertialFrame", "tform");
            else
                position = varargin{1};
                validateattributes(position, "numeric", {"nrows", 1, "ncols", 3, "finite"}, "addInertialFrame", "position");
                validateattributes(varargin{2}, ["quaternion", "numeric"], {'finite'}, "addInertialFrame", "orientation");
                validateattributes(norm(varargin{2}), ["quaternion", "numeric"], {'nonzero'}, "addInertialFrame", "orientation norm");
                orientation = robotics.internal.validation.validateQuaternion(varargin{2}, "addInertialFrame", "orientation");
                tform = trvec2tform(double(position))*quat2tform(double(orientation));
            end

            obj.ScenarioImpl.addInertialFrame(base, name, double(tform));
        end

        function varargout = addMesh(obj, type, geometries, color, varargin)
            %addMesh Define new static mesh in the scenario
            %   addMesh(OBJ, TYPE, GEOMETRIES, COLOR) adds meshes of
            %   different types into the uavScenario OBJ. The supported
            %   mesh TYPE and corresponding GEOMETRIES are shown in the
            %   additional syntaxes listed below.
            %
            %   addMesh(OBJ, "cylinder", {[CENTERX, CENTERY, RADIUS], [ZMIN,
            %   ZMAX]}, COLOR) adds a static cylinder mesh. CENTERX and
            %   CENTERY specify the center of the cylinder. RADIUS
            %   specifies the cylinder radius. ZMIN and ZMAX specify the
            %   lower and higher boundary of the cylinder in Z direction.
            %   Set mesh COLOR as a 1x3 vector [r,g,b].
            %
            %   addMesh(OBJ, "polygon", {[ENDPTSX ENDPTSY], [ZMIN, ZMAX]},
            %   COLOR) adds a static extruded polygon mesh. [ENDPTSX
            %   ENDPTSY] is a Nx2 matrix, of which each row specifies the
            %   end points of a polygon and they should be specified in
            %   either clockwise or counter clockwise order.  ZMIN and ZMAX
            %   specify the lower and higher boundary of the cylinder in Z
            %   direction. Set mesh COLOR as a 1x3 vector [r,g,b].
            %
            %   addMesh(OBJ, "terrain", {TERRAINNAME, [XMIN, XMAX], [YMIN,
            %   YMAX]}, COLOR) adds a static terrain mesh. Terrain can only
            %   be added once to a uavScenario object. TERRAINNAME is a
            %   string that specifies the terrain data. The terrain data
            %   specified by TERRAINNAME must be either "gmted2010", or a
            %   terrain name previously added using the addCustomTerrain
            %   function. The imported terrain must cover the area bounded
            %   by XMIN, XMAX in x direction and YMIN, YMAX in y direction.
            %   Set mesh COLOR as a 1x3 vector [r,g,b].
            %
            %   "gmted2010" is a tiled global terrain derived from the
            %   GMTED2010 model by the U.S. Geological Survey (USGS) and
            %   National Geospatial-Intelligence Agency (NGA) and hosted by
            %   MathWorks. Internet access is required to use "gmted2010".
            %
            %   INFO = addMesh(OBJ, "terrain", {TERRAINNAME, [XMIN, XMAX],
            %   [YMIN, YMAX]}, COLOR, Verbose=true) outputs the information
            %   related to terrain as a struct INFO that contains the
            %   terrain name, latitude range and longitude range.
            %
            %   addMesh(OBJ, "surface", {MESHGRIDX, MESHGRIDY, Z}, COLOR)
            %   adds a static surface mesh. MESHGRIDX, MESHGRIDY and Z are
            %   2D matrices of same size that defines a surface mesh. Set
            %   mesh COLOR as a 1x3 vector [r,g,b].
            %
            %   addMesh(OBJ, "custom", {VERTICES, FACES}, COLOR) adds a
            %   static custom mesh. VERTICES is a NX3 matrix that contains
            %   vertices of the mesh. FACES is a NX3 matrix, each row
            %   containing the indices of the 3 points in VERTICES that
            %   forms a triangle face. Set mesh COLOR as a 1x3 vector
            %   [r,g,b].
            %
            %   addMesh(OBJ, "buildings", {OSMFILE, XBOUND, YBOUND,
            %   ALTITUDE}, COLOR) adds building meshes defined in OSM file
            %   to uavScenario. OSMFILE can either be the file name of OSM
            %   file in current folder or MATLAB path, or the full or
            %   relative path to the OSM file. XBOUND and YBOUND are 1x2
            %   vectors that define the boundary inside which OSM buildings
            %   are imported. ALTITUDE can be either a scalar that defines
            %   the height of the bases for all OSM buildings, or a string
            %   "auto". If ALTITUDE is set to "auto", mesh base heights are
            %   defined by terrain heights from the terrain associated with
            %   scenario OBJ. If there is no terrain already associated
            %   with OBJ, building mesh base heights are set to 0.
            %
            %   INFO = addMesh(OBJ, "buildings", {OSMFILE, XBOUND, YBOUND,
            %   ALTITUDE}, COLOR, Verbose=true) outputs the information
            %   related to OSMFILE as a struct INFO that contains the
            %   OSM file name, latitude range and longitude range, total
            %   number of buildings in the OSM file and number of buildings
            %   imported from the OSM file.
            %
            %   addMesh(___, Name, Value) sets additional options specified
            %   by one or more Name-Value pair arguments:
            %      UseLatLon       - When set to true, x-y coordinates in
            %                        the GEOMETRIES are in lat-lon
            %                        coordinates for mesh types other than
            %                        "custom". Vertices for custom mesh use
            %                        "ENU" as reference frame.
            %
            %                        Default: false
            %
            %      ReferenceFrame  - Define GEOMETRIES relative to
            %                        ReferenceFrame, which must be one of
            %                        the scenario's inertial frames.
            %                        Reference frame for terrain and
            %                        buildings import must be ENU.
            %
            %                        Default: "ENU"
            %
            %      Verbose         - Output information about terrain or
            %                        building import process as either
            %                        display or struct output when set to
            %                        true.
            %
            %                        Default: false
            %
            %
            %   Example:
            %
            %      % Create a scenario centered at New York City
            %      scene = uavScenario("ReferenceLocation",[40.707088 -74.012146 0]);
            %
            %      % Add terrain based on gmted2010 data set
            %      addMesh(scene,"terrain",{"gmted2010",[-1000 1000],[-1000 1000]},[0.6 0.6 0.6]);
            %
            %      % Add buildings from OSM file manhattan.osm.
            %      % The file was downloaded from https://www.openstreetmap.org/,
            %      % which provides access to crowd-sourced map data all
            %      % over the world. The data is licensed under the Open
            %      % Data Commons Open Database License (ODbL),
            %      % https://opendatacommons.org/licenses/odbl/
            %      addMesh(scene,"buildings",{"manhattan.osm",[-800 800],[-800 800],"auto"},[0 1 0]);
            %
            %      % Visualize the scenario
            %      show3D(scene);

            type = validatestring(type, uav.internal.scenario.Constants.ScenarioMeshShapeType, "addMesh", "type");

            switch type
                case "cylinder"
                    validateattributes(geometries, "cell", {"numel", 2}, "addMesh", "geometries");
                    validateattributes(geometries{1}, "numeric", {"2d", "nrows", 1, "ncols", 3, "finite"}, "addMesh", "center and radius (geometries{1})");
                    validateattributes(geometries{1}(3), "numeric", "positive", "addMesh", "radius");
                    validateattributes(geometries{2}, "numeric", {"2d", "nrows", 1, "ncols", 2, "finite", "increasing"}, "addMesh", "z (geometries{2})");
                    geometries = cellfun(@double, geometries, "UniformOutput", false);
                case "polygon"
                    validateattributes(geometries, "cell", {"numel", 2}, "addMesh", "geometries");
                    validateattributes(geometries{1}, "numeric", {"2d", "ncols", 2, "finite"}, "addMesh", "vertices (geometries{1})");
                    validateattributes(geometries{2}, "numeric", {"2d", "nrows", 1, "ncols", 2, "finite", "increasing"}, "addMesh", "z (geometries{2})");
                    geometries = cellfun(@double, geometries, "UniformOutput", false);
                case "terrain"
                    if strlength(obj.TerrainName) > 0
                        error(message('uav:robotuav:scenario:TerrainMeshAlreadyAdded', obj.TerrainName));
                    end
                    validateattributes(geometries, "cell", {"numel", 3}, "addMesh", "geometries");
                    geometries{1} = validatestring(geometries{1}, uav.internal.scenario.geometry.listTerrain('terrain'), "addMesh", "terrain name (geometries{1})");
                    validateattributes(geometries{2}, "numeric", {"vector", "numel", 2, "finite", "increasing"}, "addMesh", "x range (geometries{2})");
                    validateattributes(geometries{3}, "numeric", {"vector", "numel", 2, "finite", "increasing"}, "addMesh", "y range (geometries{3})");
                case "surface"
                    validateattributes(geometries, "cell", {"numel", 3}, "addMesh", "geometries");
                    validateattributes(geometries{1}, "numeric", ["2d", "real", "finite"], "addMesh", "x grid (geometries{1})");
                    validateattributes(geometries{2}, "numeric", ["2d", "real", "finite"], "addMesh", "y grid (geometries{2})");
                    validateattributes(geometries{3}, "numeric", ["2d", "real", "finite"], "addMesh", "z (geometries{3})");
                    if ~all(size(geometries{1}) == size(geometries{2}) & size(geometries{2}) == size(geometries{3}))
                        error(message('uav:robotuav:scenario:SurfaceMeshSizeMismatch'));
                    end
                    if any(size(geometries{1}) < [2 2])
                        error(message('uav:robotuav:scenario:SurfaceMeshDimensionLow'));
                    end
                case "buildings"
                    validateattributes(geometries, "cell", {"numel", 4}, "addMesh", "geometries");

                    validateattributes(geometries{1}, {'string', 'char'}, ...
                        {'scalartext', 'nonempty'}, 'uavScenario', 'addMesh');
                    geometries{1} = robotics.internal.validation.findFilePath(convertStringsToChars(geometries{1}), 'uavScenario', 'addMesh');
                    validateattributes(geometries{2}, "numeric", {"vector", "numel", 2, "finite", "increasing"}, "addMesh", "x range (geometries{2})");
                    validateattributes(geometries{3}, "numeric", {"vector", "numel", 2, "finite", "increasing"}, "addMesh", "y range (geometries{3})");
                    if isnumeric(geometries{4})
                        validateattributes(geometries{4}, "numeric", ["scalar", "nonempty", "finite"], "addMesh", "altitude");
                    else
                        geometries{4} = validatestring(geometries{4}, "auto", "addMesh", "altitude");
                    end
                    geometries{1} = uav.internal.scenario.geometry.OSMInfo(geometries{1});
                    geometries{5} = obj.TerrainName;
                case "custom"
                    validateattributes(geometries, "cell", {"numel", 2}, "addMesh", "geometries");
                    validateattributes(geometries{1}, "numeric", {"2d", "finite", "ncols", 3}, "addMesh", "vertices (geometries{1})");
                    validateattributes(geometries{2}, "numeric", {"2d", "finite", "integer", "ncols", 3}, "addMesh", "faces (geometries{2})");
            end

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

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

            % check number of output for verbose mode
            if ~logical(p.Results.Verbose)
                nargoutchk(0,0);
            else
                % only support output for building and terrain type
                switch type
                    case "buildings"
                        nargoutchk(0,1);
                    case "terrain"
                        nargoutchk(0,1);
                    otherwise
                        nargoutchk(0,0);
                end
            end

            if logical(p.Results.UseLatLon)
                obj.ScenarioImpl.addMesh(type, geometries, color);
            else
                refFrame = validatestring(p.Results.ReferenceFrame, obj.InertialFrames, "addMesh", "ReferenceFrame");
                if any(strcmp(type, ["terrain", "buildings"])) && ~strcmp(refFrame, uav.internal.scenario.Scene.ENUFrameName)
                    error(message("uav:robotuav:scenario:TerrainBuildingReferenceFrameNotSupported", refFrame));
                end
                obj.ScenarioImpl.addMesh(type, geometries, color, refFrame);
            end

            if strcmp(type, "terrain")
                obj.TerrainName = geometries{1};
            end

            if logical(p.Results.Verbose) && any(strcmp(type, ["terrain", "buildings"])) 
                if strcmp(type, "buildings")
                    info = geometries{1};
                else
                    info = uav.internal.scenario.geometry.TerrainInfo(geometries{1});
                end
                if nargout == 1
                    varargout{1} = toStruct(info);
                else
                    print(info);
                    disp(message("shared_uav_rst:robotuav:scenario:ScenarioReferenceInfo", ...
                        num2str(obj.ReferenceLocation(1)), ...
                        num2str(obj.ReferenceLocation(2))).getString);
                end
            end
        end

        function h = terrainHeight(obj, x, y, varargin)
            %terrainHeight Returns the terrain height at given location
            %   H = terrainHeight(OBJ, X, Y) returns terrain height H at
            %   coordinate given by X and Y. X, Y, and H are numeric
            %   matrices with same size. H is the terrain height in meters
            %   above the WGS84 reference ellipsoid.
            %
            %   H = terrainHeight(___, Name, Value) sets additional options
            %   specified by one or more Name-Value pair arguments:
            %      UseLatLon       - To specify the X and Y inputs as
            %                        latitude and longitude, set this
            %                        argument to true.
            %
            %                        Default: false
            %
            %      ReferenceFrame  - Reference frame for the XY-coordinates,
            %                        specified as a string, which should
            %                        match a reference frame in the UAV
            %                        scenario.
            %
            %                        Default: "ENU"

            if strlength(obj.TerrainName) == 0
                error(message("uav:robotuav:scenario:NoTerrainData"));
            end

            inputSize = size(x);
            if any(size(y) ~= inputSize)
                error(message('uav:robotuav:scenario:TerrainHeightXYSizeMismatch'));
            end
            validateattributes(x, "numeric", ["nonempty", "real", "finite"], "terrainHeight", "x");
            validateattributes(y, "numeric", ["nonempty", "real", "finite"], "terrainHeight", "y");

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

            if ~logical(p.Results.UseLatLon)
                refFrame = validatestring(p.Results.ReferenceFrame, obj.InertialFrames, "terrainHeight", "ReferenceFrame");
                h = uav.internal.scenario.geometry.terrainHeight(obj.TerrainName, x, y, obj.ScenarioImpl.GeoFrame, refFrame);
            else
                h = uav.internal.scenario.geometry.terrainHeight(obj.TerrainName, x, y);
            end
        end

        function setup(obj)
            %SETUP prepares the scenario for simulation
            %   SETUP(SCENE) updates the SCENE to running state, sets
            %   platforms' poses to their initial values, and generates
            %   initial sensor readings.

            obj.ScenarioImpl.setup();
        end

        function advanced = advance(obj)
            %ADVANCE Increment simulation time and move all platforms
            %   ADVANCED = ADVANCE(SCENE) SCENE Simulation time is
            %   increment according to SCENE's update rate. Platforms in
            %   SCENE are moved if they are associated with a
            %   trajectory. ADVANCED is a scalar logical that indicates
            %   whether SCENE is able to advance 1 step. It will be
            %   false when trying to advance SCENE beyond its stop time.

            if obj.ScenarioImpl.IsSetup
                advanced = obj.ScenarioImpl.advance();
            else
                error(message("uav:robotuav:scenario:AdvanceScenarioBeforeSetup"));
            end
        end

        function updateSensors(obj)
            %updateSensors Update sensor readings
            %   updateSensors(SCENE) updates all sensor readings based on
            %   latest states of all platforms in the SCENE.

            if obj.ScenarioImpl.IsSetup
                obj.ScenarioImpl.updateSensors();
            else
                error(message("uav:robotuav:scenario:UpdateSensorBeforeSetup"));
            end
        end

        function poses = targetPoses(obj, platform)
            %targetPoses Target positions and orientations as seen from platform.
            %   POSES=targetPoses(SCENE, PLATFORM) returns the POSES of
            %   all targets with respect to the PLATFORM. PLATFORM is
            %   either a uavPlatform object or a string scalar representing
            %   the name of the platform. Targets are platforms or meshes
            %   in the SCENE that are not the specified PLATFORM. POSES
            %   is a 1 by N-1+M structure array containing the fields:
            %       - PlatformID
            %       - ClassID = 0
            %       - Position
            %       - Velocity
            %       - Acceleration = [0 0 0]
            %       - Orientation
            %       - AngularVelocity
            %   N is the number of platforms in SCENE. M is the number
            %   of meshes in SCENE. The position, velocity, orientation,
            %   and angular velocity of the targets are with respect to
            %   PLATFORM in the body frame of PLATFORM. The Orientation
            %   field is a quaternion representing the frame rotation from
            %   the body frame of PLATFORM to body frame of the target. The
            %   first N-1 elements in POSES correspond to all the target
            %   platforms. The PlatformID for these elements correspond to
            %   the position of the target platforms in the
            %   SCENE.Platforms array. The next M elements in POSES
            %   correspond to all the target meshes. The PlatformID for
            %   these elements are Idx + N, where Idx corresponds to the
            %   position of the target mesh in the SCENE.Meshes array.
            %   POSES also contains default ClassID as 0 and default
            %   Acceleration as [0 0 0]. The targets must already exist in
            %   the UAV scenario.

            poses = targetPoses(obj.ScenarioImpl, platform);
        end

        function restart(obj)
            %RESTART Reset simulation scenario
            %   RESTART(SCENE) resets the SCENE, which resets the time,
            %   platform poses, and sensor readings. The IsRunning property
            %   is set to false.
            obj.ScenarioImpl.restart();
        end

        function cobj = copy(obj)
            %COPY Creates uninitialized copy of scene
            %   COBJ = copy(OBJ) returns a uavScenario COBJ that contains
            %   the same platforms, sensors and meshes settings as OBJ.
            %   COBJ doesn't copy over simulation states such as
            %   CurrentTime from OBJ.

            data = obj.ScenarioImpl.SceneData;
            g = uav.internal.scenario.ScenarioGenerator;
            cobj = g.generate(data);
            if strlength(obj.TerrainName) > 0 && strlength(cobj.TerrainName) == 0
                % This condition only happens when copying a mat file saved
                % in 21b that contains a terrain
                cobj.TerrainName = obj.TerrainName;
            end
        end

        function ax = show(obj, varargin)
            %SHOW Visualize the scenario in 2D
            %   AX = SHOW(SCENE) visualizes the latest state of the
            %   platforms in a geographic axes
            %
            %   AX = show(SCENE, T) visualizes the trajectory of
            %   platforms at timestamps in vector T.
            %
            %   AX = show(___, Name, Value) additionally takes in the
            %   following name value pairs:
            %      Parent           - Parent axis for plotting.
            %
            %                         Default: geoaxes
            %
            %      MarkerSize       - Marker size in points squared
            %
            %                         Default: 36
            %
            %      ShowPlatformName - Set to false to hide UAV name on the
            %                         plot.
            %
            %                         Default: true

            p = inputParser();
            p.addOptional("Time", obj.ScenarioImpl.CurrentTime, ...
                @(x)validateattributes(x, "numeric", {"real", "vector", ...
                ">=", obj.ScenarioImpl.CurrentTime - obj.HistoryBufferSize/obj.UpdateRate}, ...
                "show", "time"));
            p.addParameter("Parent", [], ...
                @(x)validateattributes(x, "matlab.graphics.axis.GeographicAxes", "scalar", "show", "Parent"));
            p.addParameter("MarkerSize", [], ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive", "real"], "show", "MarkerSize"));
            p.addParameter("ShowPlatformName", true, ...
                @(x)validateattributes(x, ["logical", "numeric"], ["scalar", "binary"], "show", "ShowPlatformName"));

            p.parse(varargin{:});
            if isempty(p.Results.Parent)
                ax = geoaxes();
            else
                ax = p.Results.Parent;
            end
            plotTime = double(p.Results.Time);

            if ~ishold(ax)
                cla(ax);
                hold(ax, "on");
                cleanup = onCleanup(@()hold(ax, "off"));
            end

            if isscalar(plotTime)
                % Loop through each platform and draw them on map
                latlon = zeros(numel(obj.Platforms), 2);
                latlonLimits = zeros(2*numel(obj.Platforms), 2);
                for idx = 1:numel(obj.Platforms)
                    % Needs to add a 100 meter margin around the platform
                    % so they don't show up on the edge of map
                    marginSize = 100;
                    lla = obj.ScenarioImpl.GeoFrame.local2lla(obj.Platforms(idx).Name,...
                        [0 0 0; -marginSize -marginSize 0; marginSize marginSize 0], plotTime);
                    latlon(idx, :) = lla(1, 1:2);
                    latlonLimits(2*idx-1:2*idx, :) = lla(2:3, 1:2);
                    if p.Results.ShowPlatformName
                        text(ax, lla(1, 1), lla(1, 2), obj.Platforms(idx).Name);
                    end
                end
                geoscatter(ax, latlon(:,1), latlon(:,2), p.Results.MarkerSize, 'Marker', 'x');

                if ~isempty(latlonLimits) && ~any(isnan(latlonLimits(:)))
                    geolimits(ax, [min(latlonLimits(:,1)), max(latlonLimits(:,1))], ...
                        [min(latlonLimits(:,2)), max(latlonLimits(:,2))]);
                end
            else
                % Loop through each platform and draw them on map
                for idx = 1:numel(obj.Platforms)
                    lla = zeros(numel(plotTime), 3);
                    for tIdx = 1:numel(plotTime)
                        lla(tIdx, :) = obj.ScenarioImpl.GeoFrame.local2lla(obj.Platforms(idx).Name, [0 0 0], plotTime(tIdx));
                    end
                    geoplot(ax, lla(:,1), lla(:,2));
                end
                if p.Results.ShowPlatformName
                    legend(ax, [obj.Platforms.Name]);
                end
            end
        end

        function [ax, plotFrames] = show3D(obj, varargin)
            %show3D Visualize the scenario in 3D
            %   [AX, PLOTFRAMES] = show3D(SCENE) visualizes the latest
            %   states of the platforms and sensor frames in the SCENE,
            %   along with all static meshes. AX is the handle to the
            %   figure handle. PLOTFRAMES is a nested struct to query
            %   hgtransforms for each visualization frames created by
            %   show3D method. For example, PLOTFRAMES.ENU is the
            %   hgtransform you can use as parent to plot in the ENU frame.
            %
            %   [AX, PLOTFRAMES] = show3D(SCENE, T) visualizes the
            %   SCENE at time T.
            %
            %   [AX, PLOTFRAMES] = show3D(___, Name, Value) additionally
            %   take in the following name value pairs:
            %      Parent           - Parent axis for plotting.
            %
            %                         Default: newplot
            %
            %      FastUpdate       - Set to true to fast update plot in an
            %                         existing plot. Parent must be
            %                         provided to perform fast update.
            %
            %                         Default: false

            p = inputParser();
            p.addOptional("Time", obj.ScenarioImpl.CurrentTime, ...
                @(x)validateattributes(x, "numeric", {"scalar", ">=", obj.ScenarioImpl.StartTime}, "show3D", "time"));
            p.addParameter("Parent", [], ...
                @(x)validateattributes(x, "matlab.graphics.axis.Axes", "scalar", "show3D", "Parent"));
            p.addParameter("FastUpdate", false, ...
                @(x)validateattributes(x, ["logical", "numeric"], ["scalar", "binary"], "show3D", "FastUpdate"));
            p.parse(varargin{:});

            if p.Results.FastUpdate
                % Update the existing transforms if fast update is true

                if isempty(p.Results.Parent)
                    error(message("uav:robotuav:scenario:ShowFastUpdateEmptyAx"));
                end

                ax = p.Results.Parent;
                plotFrames = struct;

                for idx = 1:numel(obj.InertialFrames)
                    htform = findobj(ax, "-depth", 2, "Tag", obj.getHGTransformTag(obj.InertialFrames(idx)));
                    if isempty(htform)
                        error(message("uav:robotuav:scenario:MissingTransformInPlot", "Inertial Frame", obj.InertialFrames(idx)));
                    end
                    plotFrames.(obj.InertialFrames(idx)) = htform(1);
                end

                for idx = 1:numel(obj.ScenarioImpl.Platforms)
                    plat = obj.ScenarioImpl.Platforms(idx);
                    htform = findobj(ax, "-depth", 2, "Tag", obj.getHGTransformTag(plat.Name+".BodyFrame"));
                    if isempty(htform)
                        error(message("uav:robotuav:scenario:MissingTransformInPlot", "Platform", plat.Name));
                    end

                    platTform = obj.getNormalizedTform(plat.ReferenceFrame, plat.Name, double(p.Results.Time));
                    if ~any(isnan(platTform))
                        htform(1).Matrix = platTform;
                        htform(1).Visible = true;
                    else
                        htform(1).Visible = false;
                    end
                    plotFrames.(plat.Name).BodyFrame = htform(1);

                    htform = findobj(ax, "-depth", 3, "Tag", obj.getHGTransformTag(plat.Name+".MeshFrame"));
                    if isempty(htform)
                        error(message("uav:robotuav:scenario:MissingTransformInPlot", "Platform", plat.Name));
                    end
                    plotFrames.(plat.Name).MeshFrame = htform(1);

                    for sIdx = 1:numel(plat.Sensors)
                        sensor = plat.Sensors{sIdx};
                        sensorName = extractAfter(sensor.Name, plat.Name+"/");

                        htform = findobj(ax, "-depth", 3, "Tag", obj.getHGTransformTag(sensor.Name));
                        if isempty(htform)
                            error(message("uav:robotuav:scenario:MissingTransformInPlot", "Sensor", sensor.Name));
                        end
                        plotFrames.(plat.Name).(sensorName) = htform(1);
                    end
                end
            else
                % Create new plot if not fast update
                if isempty(p.Results.Parent)
                    ax = newplot;
                    axis(ax, "equal");
                else
                    ax = p.Results.Parent;
                end
                plotTime = double(p.Results.Time);

                if ~ishold(ax)
                    cla(ax);
                    hold(ax, "on");
                    cleanup = onCleanup(@()hold(ax, "off"));
                end

                plotFrames = struct;

                % plot all obstacle meshes
                for idx = 1:numel(obj.ScenarioImpl.Meshes)
                    show3D(obj.ScenarioImpl.Meshes{idx}, ax);
                end
                xlabel(ax, message("uav:robotuav:scenario:Show3DXLabel").getString);
                ylabel(ax, message("uav:robotuav:scenario:Show3DYLabel").getString);
                zlabel(ax, message("uav:robotuav:scenario:Show3DZLabel").getString);
                view(ax, 3);
                % create hgtransform for each inertial frames
                enuFrameName = obj.ScenarioImpl.ENUFrameName;
                for idx = 1:numel(obj.InertialFrames)
                    plotFrames.(obj.InertialFrames(idx)) = hgtransform("Parent", ax, ...
                        "Matrix", obj.getNormalizedTform(enuFrameName, obj.InertialFrames(idx), obj.ScenarioImpl.StartTime), ...
                        "Tag", obj.getHGTransformTag(obj.InertialFrames(idx)));
                end

                % create hgtransform for each platform and their sensors
                for idx = 1:numel(obj.ScenarioImpl.Platforms)
                    plat = obj.ScenarioImpl.Platforms(idx);
                    platTform = obj.getNormalizedTform(plat.ReferenceFrame, plat.Name, plotTime);
                    if ~any(isnan(platTform))
                        plotFrames.(plat.Name).BodyFrame = hgtransform(...
                            "Parent", plotFrames.(plat.ReferenceFrame), ...
                            "Matrix", obj.getNormalizedTform(plat.ReferenceFrame, plat.Name, plotTime), ...
                            "Tag", obj.getHGTransformTag(plat.Name+".BodyFrame"));
                    else
                        plotFrames.(plat.Name).BodyFrame = hgtransform(...
                            "Parent", plotFrames.(plat.ReferenceFrame), ...
                            "Matrix", eye(4), ...
                            "Tag", obj.getHGTransformTag(plat.Name+".BodyFrame"));
                        plotFrames.(plat.Name).BodyFrame.Visible = false;
                    end

                    plotFrames.(plat.Name).MeshFrame = hgtransform(...
                        "Parent", plotFrames.(plat.Name).BodyFrame, ...
                        "Matrix", plat.Mesh.Transform, ...
                        "Tag", obj.getHGTransformTag(plat.Name+".MeshFrame"));

                    patch("Faces", plat.Mesh.Mesh.Faces, ...
                        "Vertices", plat.Mesh.Mesh.Vertices, ...
                        "Parent", plotFrames.(plat.Name).MeshFrame, ...
                        "FaceColor", plat.Mesh.Color, ...
                        "LineStyle", "none");

                    % create hgtransform for its sensors
                    for sIdx = 1:numel(plat.Sensors)
                        sensor = plat.Sensors{sIdx};
                        sensorName = extractAfter(sensor.Name, plat.Name+"/");
                        plotFrames.(plat.Name).(sensorName) = hgtransform(...
                            "Parent", plotFrames.(plat.Name).BodyFrame, ...
                            "Matrix", obj.getNormalizedTform(plat.Name, sensor.Name, plotTime), ...
                            "Tag", obj.getHGTransformTag(sensor.Name));
                    end
                end
            end
        end
    end

    methods (Access = private)
        function tform = getNormalizedTform(obj, tgt, base, t)
            %getNormalizedTfrom get transform with normalized and
            %orthogonal rotation matrix

            % Due to floating point computation accuracy, the
            % rotation matrix part in the transform is slightly
            % off.
            % Normalize the rotation matrix's determinant to 1 and
            % make the rotation matrix orthogonal
            tform = obj.TransformTree.getTransform(tgt, base, t);
            tform = uav.internal.util.getNormalizedTform(tform);
        end
    end

    methods (Static)
        function obj = loadobj(obj)
            %loadobj customize load process

            % Assign parent when loading mat file saved before 21a
            if isempty(obj.ScenarioImpl.Parent)
                obj.ScenarioImpl.Parent = obj;
            end
        end
    end

    methods (Static, Access = private)
        function tag = getHGTransformTag(tag)
            tag = "UAV_Scenario_Frame_"+tag;
        end
    end
end

