"""
"""
from __future__ import print_function

from .utils import *
from .types import *

import msgpackrpc
import numpy as np
import msgpack
import time
import math
import logging

class MultirotorClient(object):

    """
    
    Attributes:
        client (TYPE): Description
    """
    
    def __init__(self, ip = "", port = 41451, timeout_value = 3600):
        """
        
        Args:
            ip (str, optional): Description
            port (int, optional): Description
            timeout_value (int, optional): Description
        """
        if (ip == ""):
            ip = "127.0.0.1"
        self.client = msgpackrpc.Client(msgpackrpc.Address(ip, port), timeout = timeout_value, pack_encoding = 'utf-8', unpack_encoding = 'utf-8')
        
    # -----------------------------------  Common vehicle APIs ---------------------------------------------
    def reset(self):
        """Resets the drone
        """
        self.client.call('reset')

    def ping(self):
        """
        Returns:
            TYPE: Description
        """
        return self.client.call('ping')

    def getClientVersion(self):
        """
        Returns:
            TYPE: Description
        """
        return 1 # sync with C++ client

    def getServerVersion(self):
        """
        Returns:
            TYPE: Description
        """
        return self.client.call('getServerVersion')

    def getMinRequiredServerVersion(self):
        """
        Returns:
            TYPE: Description
        """
        return 1 # sync with C++ client

    def getMinRequiredClientVersion(self):
        """
        Returns:
            TYPE: Description
        """
        return self.client.call('getMinRequiredClientVersion')

    def enableApiControl(self, vehicle_name = ''):
        """Enables API control for vehicle with name vehicle_name
        
        Args:
            vehicle_name (str, optional): Name of vehicle to enable API control for
        
        Returns:
            bool: Success
        """
        return self.client.call('enableApiControl', True, vehicle_name)

    def disableApiControl(self, vehicle_name = ''):
        """Disables API control for vehicle with name vehicle_name
        
        Args:
            vehicle_name (str, optional): Name of vehicle to disable API control for
        
        Returns:
            bool: Success
        """
        return self.client.call('enableApiControl', False, vehicle_name)

    def isApiControlEnabled(self, vehicle_name = ''):
        """
        
        Args:
            vehicle_name (str, optional): Name of vehicle
        
        Returns:
            bool: If API is enabled 
        """
        return self.client.call('isApiControlEnabled', vehicle_name)

    def arm(self, vehicle_name = ''):
        """Arms vehicle corresponding to vehicle_name
        
        Args:
            vehicle_name (str): Name of vehicle
        
        Returns:
            bool: Success
        
        """
        return self.client.call('armDisarm', True, vehicle_name)

    def disarm(self, vehicle_name = ''):
        """Disarms vehicle corresponding to vehicle_name.
        
        Args:
            vehicle_name (str): Name of vehicle
        
        Returns:
            bool: Success
        
        """
        return self.client.call('armDisarm', False, vehicle_name)

    def simPause(self, is_paused):
        """Pause sim
        
        Args:
            is_paused (TYPE): Description
        """
        self.client.call('simPause', is_paused)

    def simIsPause(self):
        """
        
        Returns:
            TYPE: Description
        """
        return self.client.call("simIsPaused")

    def simContinueForTime(self, seconds):
        """
        
        Args:
            seconds (TYPE): Description
        """
        self.client.call('simContinueForTime', seconds)

    def confirmConnection(self):
        """
        """
        if self.ping():
            print("Connected!")
        else:
             print("Ping returned false!")
        server_ver = self.getServerVersion()
        client_ver = self.getClientVersion()
        server_min_ver = self.getMinRequiredServerVersion()
        client_min_ver = self.getMinRequiredClientVersion()
    
        ver_info = "Client Ver:" + str(client_ver) + " (Min Req: " + str(client_min_ver) + \
              "), Server Ver:" + str(server_ver) + " (Min Req: " + str(server_min_ver) + ")"

        if server_ver < server_min_ver:
            print(ver_info, file=sys.stderr)
            print("AirSim server is of older version and not supported by this client. Please upgrade!")
        elif client_ver < client_min_ver:
            print(ver_info, file=sys.stderr)
            print("AirSim client is of older version and not supported by this server. Please upgrade!")
        else:
            print(ver_info)
        print('')

    # camera control
    # simGetImage returns compressed png in array of bytes
    # image_type uses one of the ImageType members
    def simGetImage(self, camera_name, image_type, vehicle_name = ''):
        """
        Args:
            camera_name (TYPE): Description
            image_type (TYPE): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            TYPE: Description
        """
        # todo: in future remove below, it's only for compatibility to pre v1.2
        camera_name = str(camera_name)

        # because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
        result = self.client.call('simGetImage', camera_name, image_type, vehicle_name)
        if (result == "" or result == "\0"):
            return None
        return result

    # camera control
    # simGetImage returns compressed png in array of bytes
    # image_type uses one of the ImageType members
    def simGetImages(self, requests, vehicle_name = ''):
        """
        Args:
            requests (TYPE): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            TYPE: Description
        """
        responses_raw = self.client.call('simGetImages', requests, vehicle_name)
        return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw]

    def simGetCollisionInfo(self, vehicle_name = ''):
        """
        
        Args:
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            TYPE: Description
        """
        return CollisionInfo.from_msgpack(self.client.call('simGetCollisionInfo', vehicle_name))

    def simSetVehiclePose(self, pose, ignore_collison, vehicle_name = ''):
        """
        
        Args:
            pose (TYPE): Description
            ignore_collison (TYPE): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        """
        self.client.call('simSetVehiclePose', pose, ignore_collison, vehicle_name)

    def simGetVehiclePose(self, vehicle_name = ''):
        """
        
        Args:
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            TYPE: Description
        """
        pose = self.client.call('simGetVehiclePose', vehicle_name)
        return Pose.from_msgpack(pose)

    def simGetObjectPose(self, object_name):
        """
        
        Args:
            object_name (TYPE): Description
        
        Returns:
            TYPE: Description
        """
        pose = self.client.call('simGetObjectPose', object_name)
        return Pose.from_msgpack(pose)

    def simSetObjectPose(self, object_name, pose, teleport = True):
        """
        
        Args:
            object_name (TYPE): Description
            pose (TYPE): Description
            teleport (bool, optional): Description
        
        Returns:
            TYPE: Description
        """
        return self.client.call('simSetObjectPose', object_name, pose, teleport)

    def simListSceneObjects(self, name_regex = '.*'):
        """Returns list of all objects in the scene 
        
        Args:
            name_regex (str, optional): Description
        
        Returns:
            List[strings]: List of all objects in the scene 
        """
        return self.client.call('simListSceneObjects', name_regex)

    def simLoadLevel(self, level_name):
        """Loads desired level
        
        Args:
            level_name (str): Description
        
        Returns:
            TYPE: Description
        """
        return self.client.call('simLoadLevel', level_name)

    def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False):
        """
        
        Args:
            mesh_name (str): Description
            object_id (int): Description
            is_name_regex (bool, optional): Description
        
        Returns:
            bool: Success
        """
        return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex)

    def simGetSegmentationObjectID(self, mesh_name):
        """
        
        Args:
            mesh_name (str): Description
        
        Returns:
            int: segmentation ID
        """
        return self.client.call('simGetSegmentationObjectID', mesh_name)

    def simPrintLogMessage(self, message, message_param = "", severity = 0):
        """
        
        Args:
            message (TYPE): Description
            message_param (str, optional): Description
            severity (int, optional): Description
        
        Returns:
            TYPE: Description
        """
        return self.client.call('simPrintLogMessage', message, message_param, severity)

    def simGetCameraInfo(self, camera_name, vehicle_name = ''):
        """
        
        Args:
            camera_name (TYPE): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            CameraInfo: Description
        """
        # TODO: below str() conversion is only needed for legacy reason and should be removed in future
        return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name))

    def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = ''):
        """Sets camera's orientation to desired quaternion
        
        Args:
            camera_name (str): Description
            orientation (Quaternionr): Desired orientation quaternion
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        """
        # TODO: below str() conversion is only needed for legacy reason and should be removed in future
        self.client.call('simSetCameraOrientation', str(camera_name), orientation, vehicle_name)

    def simGetGroundTruthKinematics(self, vehicle_name = ''):
        """
        
        Args:
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            TYPE: Description
        """
        kinematics_state = self.client.call('simGetGroundTruthKinematics', vehicle_name)
        return KinematicsState.from_msgpack(kinematics_state)
    simGetGroundTruthKinematics.__annotations__ = {'return': KinematicsState}

    def cancelLastTask():
        """
        """
        self.client.call('cancelLastTask')

    def waitOnLastTask(timeout_sec = float('nan')):
        """
        
        Args:
            timeout_sec (TYPE, optional): Description
        
        Returns:
            TYPE: Description
        """
        return self.client.call('waitOnLastTask', timeout_sec)

# -----------------------------------  Multirotor APIs ---------------------------------------------

    def takeoffAsync(self, timeout_sec = 20, vehicle_name = ''):
        """
        
        Args:
            timeout_sec (int, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('takeoff', timeout_sec, vehicle_name)  

    def landAsync(self, timeout_sec = 60, vehicle_name = ''):
        """
        
        Args:
            timeout_sec (int, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('land', timeout_sec, vehicle_name)   

    def goHomeAsync(self, timeout_sec = 3e+38, vehicle_name = ''):
        """
        
        Args:
            timeout_sec (float, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 

        Returns:
            TYPE: Description
        """
        return self.client.call_async('goHome', timeout_sec, vehicle_name)

    # APIs for control
    def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = ''):
        """
        z is given in local NED frame of the vehicle.  
        Roll (rate) / pitch (rate) / yaw (rate) set soints are given in **radians**, in the body frame. 
        The body frame follows the Front Left Up (FLU) convention, and right-handedness. 

        - X axis is along the "Front" direction of the quadrotor. 
            Clockwise rotation about this axis defines a positive **roll** angle.    
            Hence, rolling with a positive angle is equivalent to translating in the "right" direction, w.r.t. our FLU body frame. 
        - Y axis is along the "Left" direction of the quadrotor.  
            Clockwise rotation about this axis defines a positive **pitch** angle.    
            Hence, pitching with a positive angle is equivalent to translating in the "front" direction, w.r.t. our FLU body frame. 
        - Z axis is along the "Up" direction. 
            Clockwise rotation about this axis defines a positive **yaw** angle. 
            Hence, yawing with a positive angle is equivalent to rotated towards the "left" direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. 
        
        Args:
            roll (float): Desired roll angle, in radians.
            pitch (float): Desired pitch angle, in radians.
            yaw (float): Desired yaw angle, in radians.
            z (float): Desired Z value (in local NED frame of the vehicle)
            duration (float): Desired amount of time (seconds), to send this command for
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, vehicle_name)

    def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''):
        """
        Desired throttle is between 0.0 to 1.0
        Roll (rate) / pitch (rate) / yaw (rate) set soints are given in **radians**, in the body frame. 
        The body frame follows the Front Left Up (FLU) convention, and right-handedness. 

        - X axis is along the "Front" direction of the quadrotor. 
            Clockwise rotation about this axis defines a positive **roll** angle.    
            Hence, rolling with a positive angle is equivalent to translating in the "right" direction, w.r.t. our FLU body frame. 
        - Y axis is along the "Left" direction of the quadrotor.  
            Clockwise rotation about this axis defines a positive **pitch** angle.    
            Hence, pitching with a positive angle is equivalent to translating in the "front" direction, w.r.t. our FLU body frame. 
        - Z axis is along the "Up" direction. 
            Clockwise rotation about this axis defines a positive **yaw** angle. 
            Hence, yawing with a positive angle is equivalent to rotated towards the "left" direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. 
                
        Args:
            roll (float): Desired roll angle, in radians.
            pitch (float): Desired pitch angle, in radians.
            yaw (float): Desired yaw angle, in radians.
            throttle (float): Desired throttle (between 0.0 to 1.0)
            duration (float): Desired amount of time (seconds), to send this command for
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveByRollPitchYawThrottle', roll, -pitch, -yaw, throttle, duration, vehicle_name)

    def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name = ''):
        """
        Desired throttle is between 0.0 to 1.0
        Roll (rate) / pitch (rate) / yaw (rate) set soints are given in **radians**, in the body frame. 
        The body frame follows the Front Left Up (FLU) convention, and right-handedness. 

        - X axis is along the "Front" direction of the quadrotor. 
            Clockwise rotation about this axis defines a positive **roll** angle.    
            Hence, rolling with a positive angle is equivalent to translating in the "right" direction, w.r.t. our FLU body frame. 
        - Y axis is along the "Left" direction of the quadrotor.  
            Clockwise rotation about this axis defines a positive **pitch** angle.    
            Hence, pitching with a positive angle is equivalent to translating in the "front" direction, w.r.t. our FLU body frame. 
        - Z axis is along the "Up" direction. 
            Clockwise rotation about this axis defines a positive **yaw** angle. 
            Hence, yawing with a positive angle is equivalent to rotated towards the "left" direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. 
                
        Args:
            roll (float): Desired roll angle, in radians.
            pitch (float): Desired pitch angle, in radians.
            yaw_rate (float): Desired yaw rate, in radian per second.
            throttle (float): Desired throttle (between 0.0 to 1.0)
            duration (float): Desired amount of time (seconds), to send this command for
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, throttle, duration, vehicle_name)

    def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name = ''):
        """
        z is given in local NED frame of the vehicle.  
        Roll (rate) / pitch (rate) / yaw (rate) set soints are given in **radians**, in the body frame. 
        The body frame follows the Front Left Up (FLU) convention, and right-handedness. 

        - X axis is along the "Front" direction of the quadrotor. 
            Clockwise rotation about this axis defines a positive **roll** angle.    
            Hence, rolling with a positive angle is equivalent to translating in the "right" direction, w.r.t. our FLU body frame. 
        - Y axis is along the "Left" direction of the quadrotor.  
            Clockwise rotation about this axis defines a positive **pitch** angle.    
            Hence, pitching with a positive angle is equivalent to translating in the "front" direction, w.r.t. our FLU body frame. 
        - Z axis is along the "Up" direction. 
            Clockwise rotation about this axis defines a positive **yaw** angle. 
            Hence, yawing with a positive angle is equivalent to rotated towards the "left" direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. 
                
        Args:
            roll (float): Desired roll angle, in radians.
            pitch (float): Desired pitch angle, in radians.
            yaw_rate (float): Desired yaw rate, in radian per second.
            z (float): Desired Z value (in local NED frame of the vehicle)
            duration (float): Desired amount of time (seconds), to send this command for
            vehicle_name (str, optional): Name of the multirotor to send this command to 
       
        Returns:
            bool: Success
        """
        return self.client.call_async('moveByRollPitchYawrateZ', roll, -pitch, -yaw_rate, z, duration, vehicle_name)

    def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name = ''):
        """
        z is given in local NED frame of the vehicle.  
        Roll (rate) / pitch (rate) / yaw (rate) set soints are given in **radians**, in the body frame. 
        The body frame follows the Front Left Up (FLU) convention, and right-handedness. 

        - X axis is along the "Front" direction of the quadrotor. 
            Clockwise rotation about this axis defines a positive **roll** angle.    
            Hence, rolling with a positive angle is equivalent to translating in the "right" direction, w.r.t. our FLU body frame. 
        - Y axis is along the "Left" direction of the quadrotor.  
            Clockwise rotation about this axis defines a positive **pitch** angle.    
            Hence, pitching with a positive angle is equivalent to translating in the "front" direction, w.r.t. our FLU body frame. 
        - Z axis is along the "Up" direction. 
            Clockwise rotation about this axis defines a positive **yaw** angle. 
            Hence, yawing with a positive angle is equivalent to rotated towards the "left" direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. 
        
        Args:
            roll_rate (float): Desired roll rate, in radians / second
            pitch_rate (float): Desired pitch rate, in radians / second
            yaw_rate (float): Desired yaw rate, in radians / second
            z (float): Desired Z value (in local NED frame of the vehicle)
            duration (float): Desired amount of time (seconds), to send this command for
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveByAngleRatesZ', roll_rate, -pitch_rate, -yaw_rate, z, duration, vehicle_name)

    def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name = ''):
        """
        Desired throttle is between 0.0 to 1.0
        Roll (rate) / pitch (rate) / yaw (rate) set soints are given in **radians**, in the body frame. 
        The body frame follows the Front Left Up (FLU) convention, and right-handedness. 

        - X axis is along the "Front" direction of the quadrotor. 
            Clockwise rotation about this axis defines a positive **roll** angle.    
            Hence, rolling with a positive angle is equivalent to translating in the "right" direction, w.r.t. our FLU body frame. 
        - Y axis is along the "Left" direction of the quadrotor.  
            Clockwise rotation about this axis defines a positive **pitch** angle.    
            Hence, pitching with a positive angle is equivalent to translating in the "front" direction, w.r.t. our FLU body frame. 
        - Z axis is along the "Up" direction. 
            Clockwise rotation about this axis defines a positive **yaw** angle. 
            Hence, yawing with a positive angle is equivalent to rotated towards the "left" direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. 
        
        Args:
            roll_rate (float): Desired roll rate, in radians / second
            pitch_rate (float): Desired pitch rate, in radians / second
            yaw_rate (float): Desired yaw rate, in radians / second
            throttle (float): Desired throttle (between 0.0 to 1.0)
            duration (float): Desired amount of time (seconds), to send this command for
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveByAngleRatesThrottle', roll_rate, -pitch_rate, -yaw_rate, throttle, duration, vehicle_name)

    def moveByVelocityAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''):
        """
        
        Args:
            vx (float): Description
            vy (float): Description
            vz (float): Description
            duration (float): Description
            drivetrain (TYPE, optional): Description
            yaw_mode (TYPE, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name)

    def moveByVelocityZAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''):
        """
        
        Args:
            vx (TYPE): Description
            vy (TYPE): Description
            z (TYPE): Description
            duration (TYPE): Description
            drivetrain (DrivetrainType, optional): Description
            yaw_mode (YawMode, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveByVelocityZ', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name)

    def moveOnPathAsync(self, path, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), 
        lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''):
        """
        
        Args:
            path (TYPE): Description
            velocity (TYPE): Description
            timeout_sec (float, optional): Description
            drivetrain (TYPE, optional): Description
            yaw_mode (TYPE, optional): Description
            lookahead (TYPE, optional): Description
            adaptive_lookahead (int, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            TYPE: Description
        """
        return self.client.call_async('moveOnPath', path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name)

    def setTrajectoryTrackerGains(self, gains, vehicle_name = ''):
        """
        Sets trajectory tracker gains for moveOnSplineAsync, moveOnSplineVelConstraintsAsync. 
        Must be called once before either moveOnSpline* API is called
        
        Args:
            gains (list[float]): Pass TrajectoryTrackerGains().to_list() to set default values. Look at TrajectoryTrackerGains to set custom gains
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        """
        self.client.call('setTrajectoryTrackerGains', gains, vehicle_name)

    def moveOnSplineAsync(self, waypoints, vel_max=15.0, acc_max=7.5, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=True, vehicle_name = ''):
        """
        - Fits a minimum jerk trajectory to the list of given 3D waypoints (specified by the waypoints parameter).
        - Uses ETHZ-ASL's [mav_trajectory_generation](https://github.com/ethz-asl/mav_trajectory_generation) as the trajectory planning backend.
        - Tracks the references positions and velocities using a pure pursuit tracking controller. 
        The gains of the pure pursuit tracking controller are set by setTrajectoryTrackerGains.
        - Reference yaws are allocated along the tangent of the trajectory. Hence the drone will always look at the direction along which it is flying, behaving like a 3D car.
        - Note: setTrajectoryTrackerGains() must be called once before calling moveOnSpline()

        Args:
            waypoints (list[Vector3r]): List of 3D waypoints, defined in local NED frame of the vehicle to track.
            vel_max (float, optional): Maximum magnitude of velocity along trajectory
            acc_max (float, optional): Maximum magnitude of acceleration along trajectory
            add_curr_odom_position_constraint (bool, optional): Add a start constraint of current linear velocity vector, so that the planned trajectory is smooth if the drone is already moving. 
            add_curr_odom_velocity_constraint (bool, optional): Should only be set to true if add_curr_odom_position_constraint is True. 
                If this is set to true, the trajectory planning backend will add a velocity vector constraint equivalent to the current odometry / linear velocity, so that the planned trajectory is smooth if the drone is already moving. 
            viz_traj (bool, optional): set this to True. visualize trajectory in unreal window. Note that visualization would appear in the FPV image, so this should only be used for debugging. 
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveOnSpline', waypoints, add_curr_odom_position_constraint, add_curr_odom_velocity_constraint, vel_max, acc_max, viz_traj, vehicle_name)

    def moveOnSplineVelConstraintsAsync(self, waypoints, velocity_constraints, vel_max=15.0, acc_max=7.5, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=True, vehicle_name = ''):
        """
        - Fits a minimum jerk trajectory to the list of given 3D waypoints (specified by the waypoints parameter).
        - Also adds corresponding 3D velocity vector constraints (specified by the velocity_constraints parameter).
        - Uses ETHZ-ASL's [mav_trajectory_generation](https://github.com/ethz-asl/mav_trajectory_generation) as the trajectory planning backend.
        - Tracks the references positions and velocities using a pure pursuit tracking controller. 
        The gains of the pure pursuit tracking controller are set by setTrajectoryTrackerGains.
        - Reference yaws are allocated along the tangent of the trajectory. Hence the drone will always look at the direction along which it is flying, behaving like a 3D car.
        - Reference yaws are allocated along the tangent of the trajectory. 
        - Note: setTrajectoryTrackerGains() must be called once before calling moveOnSpline()
        
        Args:
            waypoints (list[Vector3r]): List of 3D waypoints, defined in local NED frame of the vehicle to track.
            velocity_constraints (list[Vector3r]): List of 3D velocity vector constraints, defined in local NED frame of the vehicle to track.
            vel_max (float, optional): Maximum magnitude of velocity along trajectory
            acc_max (float, optional): Maximum magnitude of acceleration along trajectory
            add_curr_odom_position_constraint (bool, optional): Add a start constraint of current linear velocity vector, so that the planned trajectory is smooth if the drone is already moving. 
            add_curr_odom_velocity_constraint (bool, optional): Should only be set to true if add_curr_odom_position_constraint is True. 
                If this is set to true, the trajectory planning backend will add a velocity vector constraint equivalent to the current odometry / linear velocity, so that the planned trajectory is smooth if the drone is already moving. 
            viz_traj (bool, optional): set this to True. visualize trajectory in unreal window. Note that visualization would appear in the FPV image, so this should only be used for debugging. 
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveOnSplineVelConstraints', waypoints, velocity_constraints, add_curr_odom_position_constraint, add_curr_odom_velocity_constraint, vel_max, acc_max, viz_traj, vehicle_name)

    def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), 
        lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''):
        """
        
        Args:
            x (float): Description
            y (float): Description
            z (float): Description
            velocity (float): Description
            timeout_sec (float, optional): Description
            drivetrain (DrivetrainType, optional): Description
            yaw_mode (YawMode, optional): Description
            lookahead (float, optional): Description
            adaptive_lookahead (int, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name)

    def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''):
        """
        
        Args:
            z (float): Description
            velocity (float): Description
            timeout_sec (float, optional): Description
            yaw_mode (YawMode, optional): Description
            lookahead (float, optional): Description
            adaptive_lookahead (int, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name)

    def moveToYawAsync(self, yaw, timeout_sec = 3e+38, margin = 5, vehicle_name = ''):
        """
        
        Args:
            yaw_rate (float): Desired yaw angle, in **degrees per second**.
            timeout_sec (float, optional): Description
            margin (int, optional): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('rotateToYaw', yaw, timeout_sec, margin, vehicle_name)

    def moveByYawRateAsync(self, yaw_rate, duration, vehicle_name = ''):
        """
        
        Args:
            yaw_rate (float): Desired yaw rate, in **degrees per second**.
            duration (float): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('rotateByYawRate', yaw_rate, duration, vehicle_name)

    def hoverAsync(self, vehicle_name = ''):
        """
        
        Args:
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            bool: Success
        """
        return self.client.call_async('hover', vehicle_name)

    def plot_transform(self, pose_list, vehicle_name = ''):
        """
        Plots a transform for a requested list of poses

        Args:
            pose_list (list[Pose]): Description
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        """
        self.client.call('plot_tf', pose_list, 10.0, vehicle_name) 
      
    # query vehicle state
    def getMultirotorState(self, vehicle_name = ''):
        """
        
        Args:
            vehicle_name (str, optional): Name of the multirotor to send this command to 
        
        Returns:
            MultirotorState: Description
        """
        return MultirotorState.from_msgpack(self.client.call('getMultirotorState', vehicle_name))
    getMultirotorState.__annotations__ = {'return': MultirotorState}
