# import my_setup_path
import airsim
from scheduler import Scheduler
from drone_control import DroneController
import img_lib
import time

import DetectObstacle
import numpy as np
import cv2
import pprint

MyDrone = DroneController(airsim.MultirotorClient(),name='GUdrone')
MyDrone.initFly()
MyDrone.client.moveByVelocityZAsync(vx=0, vy=2, z=-2, duration=1.5).join()
MyDrone.get_position()


# MyDrone.client.moveByVelocityZAsync(-1.5,4.8,-2,1.2).join()
# MyDrone.client.moveByVelocityAsync(1.5,5,0.6,2.4).join()
# MyDrone.client.moveByVelocityZAsync(-2,3.2,-0.6,1.4).join()
# MyDrone.client.moveByVelocityAsync(-1.6,1.6,0,2).join()
# MyDrone.client.moveByRollPitchYawZAsync(roll = 0, pitch = 0, yaw = -3.25, z = -2.0, duration = 2).join()
# MyDrone.client.moveByVelocityZAsync(vx=-2, vy=-2, z=-1.7, duration=2.2).join()



ImgHandler = img_lib.ImgHandler(MyDrone)
DroneSchedule = Scheduler()
ObstacleDetector = DetectObstacle.Obstacle()



# time.sleep(3)
print('Gu Drone, lets Go!')

level_flag=0
while (DroneSchedule.loop_count<1000):
    DroneSchedule.Run()
    print('Current ring = %d'%(level_flag+1))

    state = MyDrone.client.getMultirotorState()
    (x,y,z) = MyDrone.get_position()

    if (level_flag ==0)& (y<8):
        MyDrone.client.simPause(True)
        Depth_Img = ImgHandler.get_Depthimg_task(camera=0)
        args = ImgHandler.processing_depthIMG_task_1(Depth_Img)
        MyDrone.client.simPause(False)
        MyDrone.motion_task(args)
        if y >7:
            level_flag = 1
            MyDrone.client.moveByVelocityAsync(vx=1, vy=2, vz=0.5, duration=1).join()
            continue
    
    elif (level_flag ==1) & (y<18):
        MyDrone.client.simPause(True)
        Depth_Img = ImgHandler.get_Depthimg_task(camera=0)
        args = ImgHandler.processing_depthIMG_task_1(Depth_Img)
        MyDrone.client.simPause(False)
        MyDrone.motion_task(args)
        if y >15:
            level_flag = 2
            MyDrone.client.moveByRollPitchYawZAsync(roll = 0, pitch = 0, yaw = -2.5, z = -2.0, duration = 2).join()

    
    elif (level_flag ==2) & (x>-15):
        MyDrone.client.simPause(True)
        Depth_Img = ImgHandler.get_Depthimg_task(camera=0)
        args = ImgHandler.processing_depthIMG_task_ring_3(Depth_Img)
        MyDrone.client.simPause(False)
        print('fly to the ring_3')
        MyDrone.motion_task(args)
        MyDrone.client.moveByVelocityAsync(vx = -1.5, vy = 1.2, vz = 0, duration = 0.1).join()
        if x <-5:
            level_flag = 3
            time.sleep(4)
            MyDrone.client.moveByRollPitchYawZAsync(roll = 0, pitch = 0, yaw = -3.2, z = -1.2, duration = 2).join()
            MyDrone.client.moveByVelocityAsync(vx = -1.5, vy = -0.3, vz = 0, duration = 4).join()
            print('Test: going to the 4th ring!')


    elif (level_flag >= 3) & (x>-35) &(level_flag <6):
        MyDrone.client.simPause(True)
        Depth_Img = ImgHandler.get_Depthimg_task(camera=0)
        args = ImgHandler.processing_depthIMG_ring_4(Depth_Img)
        MyDrone.client.simPause(False)
        MyDrone.motion_task(args)
        if x <-15:
            level_flag = 4
            '''过了第4个圈'''
        if x <-24:
            level_flag = 5
            '''过了第五个圈'''
        if x <-26:
            level_flag = 6
            MyDrone.client.moveByVelocityAsync(vx = 0, vy = -1.5, vz = -1.2, duration = 1).join()
            time.sleep(4)
        continue

    elif (level_flag ==6) & (x>-40):
        MyDrone.client.simPause(True)
        Depth_Img = ImgHandler.get_Depthimg_task(camera=0)
        args = ImgHandler.processing_depthIMG_ring_4(Depth_Img)
        MyDrone.client.simPause(False)
        MyDrone.motion_task(args)
        if x <-35:
            level_flag = 7
            time.sleep(5)
            MyDrone.client.moveByRollPitchYawZAsync(roll = 0, pitch = 0, yaw = -3.14, z = -1.2, duration = 2).join()
            print('in front the gate')
        continue
    elif level_flag == 7:   #障碍物
        RGB_Img = ImgHandler.get_img_task(hsv=False)
        Gray_Img = cv2.cvtColor(RGB_Img, cv2.COLOR_BGR2GRAY)
        ret,Binary = cv2.threshold(Gray_Img,80,255,cv2.THRESH_BINARY)
        ImgHandler.save_an_img_task(Gray_Img)
        # Obstacle_Height = ObstacleDetector.Diff_callback(Gray_Img)
        # print('the bar is at:%f'%Obstacle_Height)
        if ImgHandler.save_count>20:         #x < -45:
            MyDrone.client.moveByVelocityZAsync(vx=-2, vy=-0.5, z=-0.9, duration=0.5).join()
            if x < -45:
                level_flag = 8
        continue
    elif level_flag == 8:
        MyDrone.client.simPause(True)
        Down_img = ImgHandler.get_Down_img_task(hsv=True)
        args = ImgHandler.processing_landing(Down_img)
        MyDrone.client.simPause(False)
        MyDrone.motion_task(args)
            



    # if (x<-27.0) & (level_flag==5):
    #     print('fly up to ring_6')
    #     MyDrone.client.moveByVelocityAsync(vx=0.3, vy=-1.2, vz=-1.0, duration=1).join()
    #     level_flag=1
    #     continue

    
    # MyDrone.client.simPause(True)
    # HSV_Img = ImgHandler.get_img_task(hsv=True)
    # RGB_Img = ImgHandler.get_img_task(hsv=False)
    # Depth_Img = ImgHandler.get_Depthimg_task(camera=0)
    # img_red_filtered = ImgHandler.select_red(HSV_Img)
    # Attitudes_Dict = ImgHandler.processing_img_task(RGB_Img,img_red_filtered,Depth_Img)
    # MyDrone.client.simPause(False)
    # MyDrone.motion_task(args = Attitudes_Dict)

    


print('Finished')
exit()