classdef multirotorFlightTrajectory < uav.internal.mission.trajectory.UserInterface
    %multirotorFlightTrajectory Generate UAV motion vectors along trajectory
    %
    %   multirotorFlightTrajectory generates a trajectory based on user
    %   provided waypoints, velocities, accelerations, jerks, snaps, yaws
    %   and time of arrivals using piecewise 9th degree polynomial to
    %   interpolate linear positions, and cubicpolytraj to interpolate yaw
    %   angles.
    %
    %   T = multirotorFlightTrajectory(WPTS,VELS,ACCS,JERKS,SNAPS,YAWS,
    %   TOAS) creates a reference trajectory based on user provided
    %   waypoints WPTS, velocities VELS, accelerations ACCS, jerks JERKS,
    %   snaps SNAPS, yaw angles YAWS and time of arrivals TOAS. WPTS is a
    %   NX3 matrix, where N is the total number of waypoints, and each row
    %   contains one waypoint that the trajectory will pass through. VELS
    %   is a NX3 matrix, each row contains the desired velocity at given
    %   waypoint. ACCS is a NX3 matrix, each row contains the desired
    %   acceleration at given waypoint. JERKS is a NX3 matrix, each row
    %   contains the desired jerks at given waypoint. SNAPS is a NX3
    %   matrix, each row contains the desired snaps at given waypoint.
    %   WPTS, VELS, ACCS, JERKS, SNAPS are expressed in Local NED frame.
    %   YAWS is a NX1 matrix, each row contains the desired yaw angle at
    %   given waypoint. Yaw angle is the rotation along Z axis in euler
    %   angle using ZYX rotation sequence from NED frame to body frame.
    %   TOAS is a NX1 vector contain time of arrivals at each waypoint.
    %
    %   multirotorFlightTrajectory properties:
    %      Waypoints             - Positions (m) in local NED frame 
    %      Velocities            - Velocities (m/s) in local NED frame
    %      Accelerations         - Accelerations (m/s^2) in local NED frame
    %      Jerks                 - Jerks (m/s^3) in local NED frame
    %      Snaps                 - Snaps (m/s^4) in local NED frame
    %      Yaws                  - Yaw angles (radian) in local NED frame
    %      TimeOfArrival         - Time (s) at each waypoint
    %      StartTime             - Start time (s) of the trajectory
    %      EndTime               - End time (s) of the trajectory
    %
    %   multirotorFlightTrajectory methods:
    %      query                 - Get UAV motion vectors at given timestamps.
    %      show                  - Visualize UAV trajectory
    %      copy                  - Copy flight trajectory
    %
    %   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, multirotorMissionParser

    %   Copyright 2022 The MathWorks, Inc.

    %#codegen

    properties (Dependent, SetAccess = private)
        %StartTime Start time (s) of the trajectory
        StartTime

        %EndTime End time (s) of the trajectory
        EndTime

        %Waypoints Positions (m) in local NED frame 
        Waypoints

        %Velocities Velocities (m/s) in local NED frame
        Velocities

        %Accelerations Accelerations (m/s^2) in local NED frame
        Accelerations

        %Jerks Jerks (m/s^3) in local NED frame
        Jerks

        %Snaps Snaps (m/s^4) in local NED frame
        Snaps

        %Yaws Yaws (degree) in local NED frame
        Yaws

        %TimeOfArrival Time (s) at each waypoint
        TimeOfArrival
    end
    
    methods
        function obj = multirotorFlightTrajectory(pos, vel, acc, jerk, snap, yaw, timeOfArrival)
            %multirotorFlightTrajectory Constructor

            validateattributes(pos, "numeric", {'2d', 'ncols', 3, 'finite'}, ...
                'multirotorFlightTrajectory', 'Waypoints');
            validateattributes(vel, "numeric", {'2d', 'ncols', 3, 'finite'}, ...
                'multirotorFlightTrajectory', 'Velocities');
            validateattributes(acc, "numeric", {'2d', 'ncols', 3, 'finite'}, ...
                'multirotorFlightTrajectory', 'Accelerations');
            validateattributes(jerk, "numeric", {'2d', 'ncols', 3, 'finite'}, ...
                'multirotorFlightTrajectory', 'Jerks');
            validateattributes(snap, "numeric", {'2d', 'ncols', 3, 'finite'}, ...
                'multirotorFlightTrajectory', 'Snaps');
            validateattributes(yaw, "numeric", {'vector', 'finite'}, ...
                'multirotorFlightTrajectory', 'Yaws');
            validateattributes(timeOfArrival, "numeric", {'vector', 'finite', 'increasing'}, ...
                'multirotorFlightTrajectory', 'TimeOfArrivals');

            obj.TrajectoryData = ...
                uav.internal.mission.trajectory.MultirotorFixedBoundaryTrajectory(...
                double(pos), double(vel), double(acc), double(jerk), double(snap), ...
                double(yaw), double(timeOfArrival));
        end

        function out = get.StartTime(obj)
            %getter for StartTime
            out = obj.TrajectoryData.StartTime;
        end

        function out = get.EndTime(obj)
            %getter for EndTime
            out = obj.TrajectoryData.EndTime;
        end
        
        function out = get.Waypoints(obj)
            %getter for Waypoints
            out = obj.TrajectoryData.Waypoints;
        end

        function out = get.Velocities(obj)
            %getter for Velocities
            out = obj.TrajectoryData.Velocities;
        end

        function out = get.Accelerations(obj)
            %getter for Accelerations
            out = obj.TrajectoryData.Accelerations;
        end

        function out = get.Jerks(obj)
            %getter for Jerks
            out = obj.TrajectoryData.Jerks;
        end

        function out = get.Snaps(obj)
            %getter for Snaps
            out = obj.TrajectoryData.Snaps;
        end

        function out = get.Yaws(obj)
            %getter for Yaws
            out = obj.TrajectoryData.Yaws;
        end

        function out = get.TimeOfArrival(obj)
            %getter for TimeOfArrivals
            out = obj.TrajectoryData.TimeOfArrivals;
        end


        function cobj = copy(obj)
            %copy Copy flight trajectory

            wpts = [0 0 0; 1 0 0];
            vels = zeros(2,3);
            accs = zeros(2,3);
            jerks = zeros(2,3);
            snaps = zeros(2,3);
            yaws = [0 0];
            toa = [0 1];
            cobj = multirotorFlightTrajectory(wpts, vels, accs, jerks, snaps, yaws, toa);
            cobj.TrajectoryData = obj.TrajectoryData.copy;
        end

    end

    methods (Access = {?multirotorFlightTrajectory, ?multirotorMissionParser})
        function loadData(obj, traj)
            %loadData load data from TRAJ
            %   loadData(OBJ, TRAJ) loads data from TRAJ, which is a
            %   uav.internal.mission.trajectory.MultirotorMinSnapTrajectory
            %   object
            obj.TrajectoryData = traj;
        end
    end
end

