import pprint
import time

class DroneController:
    def __init__(self,client,name):
        self.client = client
        self.name = name
        self.state = self.client.getMultirotorState()
        print(self.state.kinematics_estimated.orientation.z_val)    #偏航角

    def get_position(self):
        state = self.client.getMultirotorState()
        x = state.kinematics_estimated.position.x_val
        y = state.kinematics_estimated.position.y_val
        z = state.kinematics_estimated.position.z_val
        s = pprint.pformat((x,y,z))
        print("state: %s" % s)
        return (x,y,z)

    def initFly(self):
        self.client.confirmConnection()
        self.client.enableApiControl(True)
        self.client.armDisarm(True)              # 解锁
        self.client.takeoffAsync().join()        # 起飞
        self.client.moveToZAsync(-1.5, 0.2).join()   # 上升到1.5米高度
        return

    def motion_task(self,args):
        if args['task'] == 'flyFowardWithYawRate':
            self.flyFowardWithYawRate(args)
            return
        elif args['task'] == 'flyFowardWithYawRate':
            self.flyFowardWithYawAngle(args)
            return
        elif args['task'] == 'trunUntilYawAngle':
            self.trunUntilYawAngle(args)
            return
        elif args['task'] == 'flyFowardbyVelocity':
            self.flyFowardbyVelocity(args)
            return
        elif args['task'] == 'flyFowardbyZVelocity':
            self.flyFowardbyZVelocity(args)
        else:
            pass

    # def flySquare(self):    #An example
    #     # 飞正方形
    #     self.client.moveByVelocityZAsync(1, 0, -2, 1).join()     # 以1m/s速度向前飞2秒钟
    #     self.client.moveByVelocityZAsync(0, 1, -2, 2).join()     # 以1m/s速度向右飞2秒钟
    #     self.client.moveByVelocityZAsync(-1, 0, -2, 2).join()    # 以1m/s速度向后飞2秒钟
    #     self.client.moveByVelocityZAsync(0, -1, -2, 2).join()    # 以1m/s速度向左飞2秒钟
    #     return

    def flyFowardWithYawAngle(self,args):
        self.client.moveByRollPitchYawZAsync(roll = args['roll'], pitch = args['pitch'], yaw = args['yaw'], z = args['z'], duration = args['duration']).join()
        return

    def flyFowardWithYawRate(self,args):
        self.client.moveByRollPitchYawrateZAsync(roll = args['roll'], pitch = args['pitch'], yaw_rate = args['yaw_rate'], z = args['z'], duration = args['duration']).join()
        return

    def flyFowardbyVelocity(self,args):
        self.client.moveByVelocityAsync(vx = args['vx'], vy = args['vy'], vz = args['vz'], duration = args['duration']).join()

    def flyFowardbyZVelocity(self,args):
        self.client.moveByVelocityZAsync(vx = args['vx'], vy = args['vy'], z = args['z'], duration = args['duration']).join()        

    def trunUntilYawAngle(self,args):
        while (abs (self.state.kinematics_estimated.orientation.z_val - args['yaw'])> 0.05):
            self.state = self.client.getMultirotorState()
            self.flyFowardWithYawRate(args)
            print(self.state.kinematics_estimated.orientation)


    def flyThroughRing_1(self):
        self.client.moveByRollPitchYawZAsync(roll = 0, pitch = 0.2, yaw = -1.57, z = -1.6, duration = 3.3).join()
        # self.client.moveByRollPitchYawrateZAsync(roll = 0, pitch = 0.2, yaw_rate = 0, z = -1.5, duration = 2).join()
    
    def flyThroughRing_2(self):
        self.client.moveByRollPitchYawrateZAsync(roll = -0.2, pitch = 0.02, yaw_rate = 0, z = -1, duration = 0.8).join()
        # self.client.moveByRollPitchYawrateZAsync(roll = 0, pitch = -0.1, yaw_rate = 0, z = -1, duration = 1).join()

    def flyThroughRing_3(self):
        self.client.moveByRollPitchYawrateZAsync(roll = 0, pitch = -0.4, yaw_rate = -0.8, z = -2.0, duration = 0.8).join()
        self.client.moveByRollPitchYawrateZAsync(roll = 0.7, pitch = -0.55, yaw_rate = -0.8, z = -2.0, duration = 0.5).join()
        self.client.moveByRollPitchYawrateZAsync(roll = 0, pitch = 0.4, yaw_rate = -0.3, z = -2.0, duration = 0.5).join()
        self.client.moveByRollPitchYawrateZAsync(roll = 0, pitch = 0.2, yaw_rate = 0, z = -2.0, duration = 3.2).join()