classdef multirotorMissionParser < handle
    %multirotorMissionParser Generate trajectory for multirotor from mission
    %
    %   multirotorMissionParser parses mission defined in uavMission object
    %   and generate a flight trajectory that follows the given mission as
    %   a multirotorFlightTrajectory object.
    %
    %   P = multirotorMissionParser creates a multirotor UAV mission parser
    %   P.
    %
    %   P = multirotorMissionParser(Name=Value) specifies properties using
    %   one or more name-value arguments.
    %      TransitionRadius     - Transition radius in meters
    %                             Transition radius used to compute
    %                             trajectory waypoints based on mission
    %                             definition, specified as a positive
    %                             scalar, in meters.
    %
    %      TakeoffSpeed         - Takeoff speed in m/s
    %                             Take off speed used to calculate how much
    %                             time UAV should take to reach desired
    %                             altitude during takeoff and land,
    %                             specified as a positive scalar, in
    %                             meters.
    %
    %   multirotorMissionParser properties:
    %      TransitionRadius     - Transition radius in meters
    %      TakeoffSpeed         - Takeoff speed in m/s
    %
    %   multirotorMissionParser methods:
    %      parse        - Generate reference trajectory in local NED frame
    %      copy         - Copy mission parser
    %
    %   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 uavMission, multirotorFlightTrajectory

    %   Copyright 2022 The MathWorks, Inc.

    properties (Dependent)
        %TransitionRadius Transition radius in meters
        %    Transition radius used to compute trajectory waypoints based
        %    on mission definition, specified as a positive scalar, in
        %    meters.
        %
        %    Default: 2
        TransitionRadius

        %TakeoffSpeed Takeoff speed in m/s
        %   Take off speed used to calculate how much time UAV should take
        %   to reach desired altitude during takeoff and land, specified as
        %   a positive scalar, in meters.
        %
        %   Default: 2
        TakeoffSpeed
    end

    properties (Access=private)
        %MultirotorBehavior Internal implementation of multirotor behavior
        MultirotorBehavior
    end
    
    methods
        function obj = multirotorMissionParser(varargin)
            %multirotorMissionParser Constructor

            parser = inputParser;
            parser.addParameter("TransitionRadius", 2, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive", "finite"], "multirotorMissionParser", "TransitionRadius"));
            parser.addParameter("TakeoffSpeed", 2, ...
                @(x)validateattributes(x, "numeric", ["scalar", "positive", "finite"], "multirotorMissionParser", "TakeoffSpeed"));
            parse(parser, varargin{:});

            obj.MultirotorBehavior = uav.internal.mission.behavior.MultirotorMinSnapBehavior;
            obj.MultirotorBehavior.TransitionRadius = double(parser.Results.TransitionRadius);
            obj.MultirotorBehavior.TakeoffSpeed = double(parser.Results.TakeoffSpeed);
        end

        function out = get.TransitionRadius(obj)
            %getter for TransitionRadius
            out = obj.MultirotorBehavior.TransitionRadius;
        end

        function set.TransitionRadius(obj, x)
            %setter for TransitionRadius
            validateattributes(x, "numeric", ["scalar", "positive", "finite"], "multirotorMissionParser", "TransitionRadius");
            obj.MultirotorBehavior.TransitionRadius(:) = x;
        end

        function out = get.TakeoffSpeed(obj)
            %getter for TakeoffPitch
            out = obj.MultirotorBehavior.TakeoffSpeed;
        end

        function set.TakeoffSpeed(obj, x)
            %setter for TakeoffPitch
            validateattributes(x, "numeric", ["scalar", "positive", "finite"], "multirotorMissionParser", "TakeoffSpeed");
            obj.MultirotorBehavior.TakeoffSpeed(:) = x;
        end

        function cobj = copy(obj)
            %copy Copy mission parser
            %   COBJ = copy(OBJ) creates a deep copy of the
            %   multirotorMissionParser object with the same properties.
            
            cobj = multirotorMissionParser;
            cobj.TransitionRadius = obj.TransitionRadius;
            cobj.TakeoffSpeed = obj.TakeoffSpeed;
        end
        
        function traj = parse(obj, mission, varargin)
            %parse Generate reference trajectory in local NED frame
            %    TRAJ = parse(OBJ,MISSION) generates reference trajectory
            %    TRAJ for a multirotor following UAV mission MISSION as a
            %    multirotorFlightTrajectory object. The trajectory is
            %    defined in a local NED frame with the origin defined with
            %    the same global coordinates as the HomeLocation of
            %    MISSION.
            %
            %    TRAJ = parse(OBJ,MISSION,REFERENCELOCATION) generates the
            %    reference trajectory in a local NED frame, with the origin
            %    defined by global coordinates REFERENCELOCATION.
            %    REFERENCELOCATION is a 1x3 vector [latitude longitude
            %    altitude]. Latitude and longitude are in degrees. Altitude
            %    is height in meters above WGS84 ellipsoid.

            narginchk(2,3);
            trajImpl = obj.MultirotorBehavior.parse(mission.MissionData, varargin{:});
            traj = multirotorFlightTrajectory(zeros(2,3),zeros(2,3),zeros(2,3),zeros(2,3),zeros(2,3),zeros(2,1),[0 1]);
            traj.loadData(trajImpl);

        end
    end
end

