classdef uavMission < handle
    %uavMission Stores mission data for a UAV flight
    %
    %   uavMission stores UAV mission data that can be used to generate
    %   flight trajectory for use in UAV scenario simulation.
    %
    %   M = uavMission() creates an empty mission.
    %
    %   M = uavMission(Name=Value) specifies properties using one or more
    %   name-value arguments.
    %
    %       PlanFile       - Import mission data from file
    %                        PlanFile is a string containing absolute
    %                        or relative file path to a *.waypoints or
    %                        *.plan file. When specified, uavMission
    %                        reads mission data from given PLAN file in PX4
    %                        mission json format as a *.plan file or
    %                        MAVLink waypoints format as a
    %                        *.waypoints file.
    %
    %       HomeLocation   - Home location [latitude longitude altitude]
    %                        Geodetic start location, specified as a 1x3
    %                        vector in the form [latitude longitude
    %                        altitude]. Latitude and longitude specified in
    %                        degrees and altitude is specified in meters.
    %                        This will be ignored when importing from file.
    %
    %       InitialYaw     - Initial yaw angle in degrees
    %                        Initial yaw angle between the x-axis of the
    %                        UAV body and the north direction, specified as
    %                        a scalar, in degrees. Clockwise is positive
    %                        direction when looking downwards.
    %
    %       Frame          - Reference frame used by mission waypoints,
    %                        specified as "Global", "GlobalRelativeAlt",
    %                        "LocalENU", or "LocalNED". This will be
    %                        ignored when importing from file.
    %
    %       Speed          - Reference speed used by the mission,
    %                        specified as a scalar, in m/s. This will be
    %                        ignored when importing from *.plan file.
    %
    %   uavMission properties:
    %       HomeLocation      - Home location [latitude longitude altitude]
    %       InitialYaw        - Initial yaw angle in degrees
    %       Speed             - Reference speed used by mission waypoints
    %       Frame             - Reference frame used by mission waypoints
    %       NumMissionItems   - Number of mission items
    %
    %   uavMission methods:
    %       addTakeoff              - Add takeoff mission item
    %       addWaypoint             - Add waypoint mission item
    %       addLoiter               - Add loiter mission item
    %       addHover                - Add hover mission item
    %       addLand                 - Add land mission item
    %       copy                    - Copy UAV mission
    %       removeItem              - Remove mission items
    %       show                    - Visualize UAV mission
    %       showdetails             - Show UAV mission data as a table
    %
    %   Example:
    %
    %      %Create mission
    %      mission = uavMission(PlanFile="flight.plan");
    %
    %      %Create parser
    %      parser = multirotorMissionParser();
    %
    %      %Generate flight trajectory
    %      traj = parse(parser, mission);
    %
    %      %Visualize
    %      figure()
    %      show(mission);
    %      hold on;
    %      show(traj);
    %
    %   See also fixedwingMissionParser, multirotorMissionParser


    %   Copyright 2022 The MathWorks, Inc.

    properties (Dependent, SetAccess = private)
        %Home location as [latitude longitude altitude]
        %   Geodetic start location, specified as a 1x3 vector in the form
        %   [latitude longitude altitude]. Latitude and longitude specified
        %   in degrees and altitude is specified in meters.
        %
        %   Default: [0 0 0]
        HomeLocation

        %InitialYaw Initial yaw angle in degrees
        %   Initial yaw angle between the x-axis of the UAV body and the
        %   north direction, specified as a scalar, in degrees. Clockwise
        %   is positive direction when looking downwards.
        %
        %   Default: 0
        InitialYaw

        %Frame Reference frame used by mission waypoints
        %   Reference frame used by mission waypoints, specified as
        %   "Global", "GlobalRelativeAlt", "LocalENU", or "LocalNED".
        %
        %   Default: "Global"
        Frame

        %Speed Reference speed in m/s
        %   Reference speed used by the mission, specified as a scalar, in
        %   m/s.
        %
        %   Default: 8
        Speed

        %NumMissionItems Number of mission items
        NumMissionItems
    end

    properties (Access = {?uavMission, ?fixedwingMissionParser, ?multirotorMissionParser})
        %MissionData Internal implementation of the mission data storage
        MissionData
    end

    properties (Constant, Access = {?uavMission, ?matlab.unittest.TestCase})
        %Dialect MAVLink dialect used to parse MAVLink command messages
        Dialect = mavlinkdialect("common.xml");
        
        %ParameterNameDict Map mission type to its parameter names
        ParameterNameDict = dictionary(enumeration("uav.internal.mission.data.MissionType"), ...
            {strings(1,0), ["Altitude" "Pitch" "Yaw"], ["X" "Y" "Z" "Yaw" "AcceptanceRadius"], ...
            ["X" "Y" "Z" "Radius" "Duration"], ["X" "Y" "Z" "Radius" "Duration"],...
            ["X" "Y" "Z" "Yaw"], "Speed"}');
    end

    methods
        function obj = uavMission(varargin)
            %uavMission constructor

            parser = inputParser;
            parser.addParameter("PlanFile", "", ...
                @(x)validateattributes(x, ["string", "char"], ["scalartext", "nonempty"], "uavMission", "PlanFile"));
            parser.addParameter("HomeLocation", [0 0 0], ...
                @(x)validateattributes(x, "numeric", {"row", "numel", 3, "finite"}, "uavMission", "HomeLocation"));
            parser.addParameter("InitialYaw", 0, ...
                @(x)validateattributes(x, "numeric", ["scalar", "finite"], "uavMission", "InitialYaw"));
            parser.addParameter("Speed", 8, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive"], "uavMission", "Speed"));
            parser.addParameter("Frame", string(uav.internal.mission.data.MissionFrame.Global), ...
                @(x)validateattributes(x, ["string", "char"], ["scalartext", "nonempty"], "uavMission", "Frame"));
            parser.parse(varargin{:});

            if strlength(parser.Results.PlanFile)~=0
                robotics.internal.validation.findFilePath(convertStringsToChars(parser.Results.PlanFile), 'uavMission', 'PlanFile');
                [~, ~, extension] = fileparts(convertCharsToStrings(parser.Results.PlanFile));
                switch extension
                    case ".waypoints"
                        planParser = uav.internal.mission.MAVLinkWaypointsParser(obj.Dialect);
                        obj.MissionData = planParser.parse(convertCharsToStrings(parser.Results.PlanFile));
                        obj.MissionData.InitialHeading = double(parser.Results.InitialYaw);
                    case ".plan"
                        planParser = uav.internal.mission.MissionPlanParser(obj.Dialect);
                        obj.MissionData = planParser.parse(convertCharsToStrings(parser.Results.PlanFile));
                        obj.MissionData.InitialHeading = double(parser.Results.InitialYaw);
                        obj.MissionData.Speed = double(parser.Results.Speed);
                    otherwise
                        error(message('uav:robotuav:mission:WrongMissionDefinitionFileExt'));
                end
            else
                obj.MissionData = uav.internal.mission.Mission;
                obj.MissionData.changeHomeLocation(double(parser.Results.HomeLocation));
                frame = validatestring(parser.Results.Frame, string(enumeration("uav.internal.mission.data.MissionFrame")), "uavMission", "Frame");
                obj.MissionData.changeFrame(uav.internal.mission.data.MissionFrame(frame));
                obj.MissionData.Speed = double(parser.Results.Speed);
                obj.MissionData.InitialHeading = double(parser.Results.InitialYaw);
            end
        end

        function out = get.Frame(obj)
            % getter for Frame property
            out = string(obj.MissionData.Frame);
        end
        function out = get.HomeLocation(obj)
            % getter for HomeLocation property
            out = obj.MissionData.HomeLocation;
        end

        function out = get.Speed(obj)
            % getter for Speed property
            out = obj.MissionData.Speed;
        end

        function out = get.InitialYaw(obj)
            % getter for InitialYaw property
            out = obj.MissionData.InitialHeading;
        end

        function out = get.NumMissionItems(obj)
            % getter for NumMissionItems property
            out = obj.MissionData.NumMissionItems;
        end

        function cobj = copy(obj)
            %copy Copy UAV mission
            %   COBJ = copy(OBJ) creates a deep copy of the uavMission
            %   object with the same mission items.

            cobj = uavMission;
            cobj.MissionData = obj.MissionData.copy();
        end
        
        function addTakeoff(obj, altitude, varargin)
            %addTakeoff Add takeoff mission item.
            %   addTakeoff(OBJ,ALTITUDE) adds a takeoff mission item that
            %   take UAV to desired ALTITUDE. ALTITUDE is a scalar numeric
            %   value. When represented in Global frame, ALTITUDE is the
            %   height in meters above WGS84 reference ellipsoid. When
            %   represented in Global frame with relative altitude or local
            %   frames, ALTITUDE is the height in meters above the
            %   HomeLocation of the mission.
            %
            %   addTakeoff(___, Name=Value) sets additional options
            %   specified by one or more name-value pair arguments:
            %      Pitch           - Desired takeoff pitch angle in degrees
            %                        Default: nan
            %
            %      Yaw             - Desired yaw angle in degrees
            %                        Default: nan
            %
            %      Frame           - Frame in which altitude is specified
            %                        Default: Same as frame of the mission
            %
            %      Timestamp       - Timestamp for this item in seconds
            %                        Default: Set automatically based on
            %                        the reference speed of the mission
            %
            %      InsertAtRow     - Desired location in the mission
            %                        Default: Insert at the end of mission

            validateattributes(altitude, "numeric", ["scalar", "finite", "nonnegative"], "addTakeoff", "altitude");
            parser = obj.missionItemParser("addTakeoff");
            parser.addParameter("Pitch", nan, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive"], "addTakeoff", "Pitch"));
            parser.addParameter("Yaw", nan, ...
                @(x)validateattributes(x, "numeric",  ["scalar", "finite"], "addTakeoff", "Yaw"));
            parser.parse(varargin{:});
            
            obj.MissionData.takeoff(...
                uav.internal.mission.data.MissionFrame(parser.Results.Frame), ...
                double(altitude), double(parser.Results.Pitch), double(parser.Results.Yaw), ...
                double(parser.Results.Timestamp), double(parser.Results.InsertAtRow));

        end

        function addWaypoint(obj, waypoint, varargin)
            %addWaypoint Add waypoint mission item.
            %   addWaypoint(OBJ,WAYPOINT) adds a waypoint mission item that
            %   take the UAV to the desired WAYPOINT. WAYPOINT is a 1x3
            %   numeric value represented in the mission's frame.
            %
            %   addWaypoint(___, Name=Value) sets additional options
            %   specified by one or more name-value pair arguments:
            %      AcceptanceRadius- Acceptance radius in meters
            %                        Default: NaN
            %
            %      Yaw             - Desired yaw angle in degrees
            %                        Default: NaN
            %
            %      Frame           - Frame in which altitude is specified
            %                        Default: Same as frame of the mission
            %
            %      Timestamp       - Timestamp for this item in seconds
            %                        Default: Set automatically based on
            %                        the reference speed of the mission
            %
            %      InsertAtRow     - Desired location in the mission
            %                        Default: Insert at the end of mission

            validateattributes(waypoint, "numeric", {"row", "numel", 3, "finite"}, "addWaypoint", "waypoint");
            parser = obj.missionItemParser("addWaypoint");
            parser.addParameter("AcceptanceRadius", nan, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive"], "addWaypoint", "AcceptanceRadius"));
            parser.addParameter("Yaw", nan, ...
                @(x)validateattributes(x, "numeric",  ["scalar", "finite"], "addWaypoint", "Yaw"));
            parser.parse(varargin{:});
            
            obj.MissionData.waypoint(...
                uav.internal.mission.data.MissionFrame(parser.Results.Frame), ...
                double(waypoint), double(parser.Results.Yaw), double(parser.Results.AcceptanceRadius), ...
                double(parser.Results.Timestamp), double(parser.Results.InsertAtRow));

        end

        function addLoiter(obj, center, radius, duration, varargin)
            %addLoiter Add loiter mission item.
            %   addLoiter(OBJ,CENTER,RADIUS,DURATION) adds a loiter mission
            %   item that take UAV to loiter around a center coordinate
            %   CENTER with a radius RADIUS for a duration DURATION. CENTER
            %   is a 1x3 numeric value represented in the mission's frame.
            %   RADIUS is a positive numeric scalar representing loiter
            %   radius in meters. DURATION is a positive numeric scalar for
            %   the time spent in loiter motion in seconds.
            %
            %   addLoiter(___, Name=Value) sets additional options
            %   specified by one or more name-value pair arguments:
            %      Frame           - Frame in which altitude is specified
            %                        Default: Same as frame of the mission
            %
            %      Timestamp       - Timestamp for this item in seconds
            %                        Default: Set automatically based on
            %                        the reference speed of the mission
            %
            %      InsertAtRow     - Desired location in the mission
            %                        Default: Insert at the end of mission

            validateattributes(center, "numeric", {"row", "numel", 3, "finite"}, "addLoiter", "center");
            validateattributes(radius, "numeric", ["scalar", "positive"], "addLoiter", "radius");
            validateattributes(duration, "numeric", ["scalar", "positive"], "addLoiter", "duration");
            parser = obj.missionItemParser("addLoiter");
            parser.parse(varargin{:});
            
            obj.MissionData.loiter(...
                uav.internal.mission.data.MissionFrame(parser.Results.Frame), ...
                double(center), double(radius), double(duration), ...
                double(parser.Results.Timestamp), double(parser.Results.InsertAtRow));

        end

        function addHover(obj, center, radius, duration, varargin)
            %addHover Add hover mission item.
            %   addHover(OBJ,CENTER,RADIUS,DURATION) adds a hover mission
            %   item that take UAV to hover around a center coordinate
            %   CENTER with a radius RADIUS for a duration DURATION. CENTER
            %   is a 1x3 numeric value represented in the mission's frame.
            %   RADIUS is a positive numeric scalar representing hover
            %   radius in meters. DURATION is a positive numeric scalar for
            %   the time spent in hover motion in seconds.
            %
            %   addHover(___, Name=Value) sets additional options
            %   specified by one or more name-value pair arguments:
            %      Frame           - Frame in which altitude is specified
            %                        Default: Same as frame of the mission
            %
            %      Timestamp       - Timestamp for this item in seconds
            %                        Default: Set automatically based on
            %                        the reference speed of the mission
            %
            %      InsertAtRow     - Desired location in the mission
            %                        Default: Insert at the end of mission

            validateattributes(center, "numeric", {"row", "numel", 3, "finite"}, "addHover", "center");
            validateattributes(radius, "numeric", ["scalar", "positive"], "addHover", "radius");
            validateattributes(duration, "numeric", ["scalar", "positive"], "addHover", "duration");
            parser = obj.missionItemParser("addHover");
            parser.parse(varargin{:});
            
            obj.MissionData.hover(...
                uav.internal.mission.data.MissionFrame(parser.Results.Frame), ...
                double(center), double(radius), double(duration), ...
                double(parser.Results.Timestamp), double(parser.Results.InsertAtRow));

        end

        function addLand(obj, waypoint, varargin)
            %addLand Add land mission item.
            %   addLand(OBJ,WAYPOINT) adds a land mission item that take
            %   UAV to land at desired WAYPOINT. WAYPOINT is a 1x3 numeric
            %   value represented in the mission's frame.
            %
            %   addLand(___, Name=Value) sets additional options specified
            %   by one or more name-value pair arguments:
            %      Yaw             - Desired yaw angle in degrees
            %                        Default: NaN
            %
            %      Frame           - Frame in which altitude is specified
            %                        Default: Same as frame of the mission
            %
            %      Timestamp       - Timestamp for this item in seconds
            %                        Default: Set automatically based on
            %                        the reference speed of the mission
            %
            %      InsertAtRow     - Desired location in the mission
            %                        Default: Insert at the end of mission

            validateattributes(waypoint, "numeric", {"row", "numel", 3, "finite"}, "addLand", "waypoint");
            parser = obj.missionItemParser("addLand");
            parser.addParameter("Yaw", nan, ...
                @(x)validateattributes(x, "numeric",  ["scalar", "finite"], "addLand", "Yaw"));
            parser.parse(varargin{:});
            
            obj.MissionData.land(...
                uav.internal.mission.data.MissionFrame(parser.Results.Frame), ...
                double(waypoint), double(parser.Results.Yaw), ...
                double(parser.Results.Timestamp), double(parser.Results.InsertAtRow));

        end

        function addChangeSpeed(obj, speed, varargin)
            %addChangeSpeed Add change speed mission item.
            %   addChangeSpeed(OBJ,SPEED) adds a change speed mission item
            %   that changes UAV's reference speed to SPEED. SPEED is a
            %   scalar numeric value for UAV's reference speed for
            %   subsequent mission items in m/s.
            %
            %   addChangeSpeed(___, Name=Value) sets additional options
            %   specified by one or more name-value pair arguments:
            %      Timestamp       - Timestamp for this item in seconds
            %                        Default: Set automatically based on
            %                        the reference speed of the mission
            %
            %      InsertAtRow     - Desired location in the mission
            %                        Default: Insert at the end of mission

            validateattributes(speed, "numeric", ["scalar", "positive"], "addChangeSpeed", "speed");

            parser = inputParser;
            parser.addParameter("InsertAtRow", obj.NumMissionItems+1, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive", "integer"], ...
                "addChangeSpeed", "InsertAtRow"));
            parser.addParameter("Timestamp", nan, ...
                @(x)validateattributes(x, "numeric",  "scalar", "addChangeSpeed", "Timestamp"));
            parser.parse(varargin{:});
            
            obj.MissionData.changeSpeed(...
                uav.internal.mission.data.MissionFrame.Global, ...
                double(speed), double(parser.Results.Timestamp), double(parser.Results.InsertAtRow));

        end

        function removeItem(obj, idx)
            %removeItem Remove mission items at specified indices
            %   removeItem(OBJ,IDX) removes mission items at specified
            %   indices IDX, which is a vector of integers between 1 and
            %   the number of mission items in OBJ.

            validateattributes(idx, "numeric", ...
                {"vector", "nonempty", "integer", ">=", 1, "<=", obj.NumMissionItems}, "removeItem", "idx");
            obj.MissionData.removeItems(idx);
        end

        function tbl = showdetails(obj)
            %showdetails Show UAV mission data as a table
            %   TBL = showdetails(OBJ) returns a MATLAB table TBL that
            %   contains mission details.

            % Generate table header
            variableNameHeaders = ["ParameterName" + (1:7);"ParameterValue"+(1:7)];
            variableNameHeaders = variableNameHeaders(:)';
            variableNameHeaders =  ["Idx", "Timestamp", "MissionType" variableNameHeaders];

            % Fill parameter names according to mission item types
            parameterNames = strings(obj.NumMissionItems, 7);
            for idx = 1:obj.NumMissionItems
                parameterName = obj.ParameterNameDict(obj.MissionData.MissionType(idx));
                parameterNames(idx, 1:numel(parameterName{1})) = parameterName{1};
            end

            % Generate table
            tbl = table((1:obj.NumMissionItems)', obj.MissionData.UserTimestamp, ...
                string(obj.MissionData.MissionType), ...
                parameterNames(:,1), ...
                obj.MissionData.MissionParameter(:,1), ...
                parameterNames(:,2), ...
                obj.MissionData.MissionParameter(:,2), ...
                parameterNames(:,3), ...
                obj.MissionData.MissionParameter(:,3), ...
                parameterNames(:,4), ...
                obj.MissionData.MissionParameter(:,4), ...
                parameterNames(:,5), ...
                obj.MissionData.MissionParameter(:,5), ...
                parameterNames(:,6), ...
                obj.MissionData.MissionParameter(:,6), ...
                parameterNames(:,7), ...
                obj.MissionData.MissionParameter(:,7), ...
                'VariableNames', variableNameHeaders);
        end

        function ax = show(obj, varargin)
            %show Visualize UAV mission
            %   AX = show(OBJ) visualizes the UAV mission and returns the
            %   axis handle AX that contains the plot. The visualization is
            %   rendered in local ENU frame.
            %
            %   Ax = show(___, Name=Value) sets additional options
            %   specified by one or more name-value pair arguments:
            %      Parent              - Parent axis to store the plot
            %
            %      ReferenceLocation   - Reference location of plot origin
            %                            ReferenceLocation is a 1x3 vector
            %                            for [latitude, longitude,
            %                            altitude].
            %                            Default: same as mission's home
            %                            location

            parser = inputParser;
            parser.addParameter("Parent", [], ...
                @(x)validateattributes(x, "matlab.graphics.axis.Axes", "scalar", "show", "Parent"));
            parser.addParameter("ReferenceLocation", obj.HomeLocation, ...
                @(x)validateattributes(x, "numeric", {"row", "numel", 3, "finite"}, "show", "ReferenceLocation"));
            parser.parse(varargin{:});

            if isempty(parser.Results.Parent)
                ax = newplot;
            else
                ax = parser.Results.Parent;
            end

            obj.MissionData.show3D(ax, parser.Results.ReferenceLocation);
        end
    end

    methods (Access=private)
        function parser = missionItemParser(obj, fcnName)
            %missionItemParser creates input parser for mission item functions

            parser = inputParser;
            parser.addParameter("InsertAtRow", obj.NumMissionItems+1, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive", "integer"], fcnName, "InsertAtRow"));
            parser.addParameter("Timestamp", nan, ...
                @(x)validateattributes(x, "numeric",  "scalar", fcnName, "Timestamp"));
            parser.addParameter("Frame", obj.Frame, ...
                @(x)validateattributes(x, ["string", "char"], ["scalartext", "nonempty"], fcnName, "Frame"));
        end
    end
end

