"""
视觉跟随圆移动控制飞机飞行。
圆形案板移动有前后，左右，上下移动

"""
import cv2
import threading
import time
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import sys
import numpy as np
import keyboard
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()


"""
初始化飞机，起飞,创建圆形目标
"""
vis = VisionCaptureApi.VisionCaptureApi()
# ue.sendUE4Cmd(b"r.setres 720x405w", 0)
ue.sendUE4Cmd(b"t.MaxFPS 30", 0)
ue.sendUE4PosNew(3, vehicleType=809, PosE=[2, 0, -2])
vis.jsonLoad()
time.sleep(1)
ue.sendUE4ExtAct(3, ActExt=[1, 0.8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0])
isSuss = vis.sendReqToUE4()  # 向RflySim3D发送取图请求，并验证
if not isSuss:  # 如果请求取图失败，则退出
    sys.exit(0)
vis.startImgCap()  # 开启取图，并启用共享内存图像转发，转发到填写的目录
time.sleep(2)

mav = PX4MavCtrl.PX4MavCtrler()
mav.InitMavLoop()
time.sleep(2)
mav.initOffboard()
time.sleep(2)
mav.SendMavArm(True)
mav.SendPosNED(0, 0, -2, 0)
time.sleep(10)
detect_flag = False

"""
开辟线线程,控制圆形目标移动
"""


Circle_pose = [2, 0, -2, 0]


def MoveCircle(val):
    """
    圆板目标移动没有旋转，只有平移，且始终朝向飞机
    """
    global vis
    global detect_flag
    global CalCirle_pose
    offset = 0.01  # 每次移动的距离
    yaw_offset = np.deg2rad(1)
    drone_pose = ue.getUE4Pos()
    # if not detect_flag:
    #     time.sleep(0.001)
    #     continue
    if val == "right":  # 如果按键按下，这个值为1
        # 向右移动
        Circle_pose[1] += offset
    if val == "left":
        # 向左移动
        Circle_pose[1] -= offset
    if val == "up":
        # 向上移动
        Circle_pose[2] -= offset
    if val == "down":
        # 向下移动
        Circle_pose[2] += offset
    if val == "front":
        # 向前移动
        Circle_pose[0] += offset
    if val == "back":
        # 向后移动
        Circle_pose[0] -= offset
    ue.sendUE4PosNew(
        3,
        vehicleType=809,
        PosE=[Circle_pose[0], Circle_pose[1], Circle_pose[2]],
        AngEuler=[0, 0, Circle_pose[3]],
    )


# move_circle_th = threading.Thread(target=MoveCircle)
# move_circle_th.start()

"""
开辟线程,获取图像,并控制飞机移动
"""


def CalCirle(img: np.array) -> tuple():
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    low_hsv = np.array([0, 43, 46])
    high_hsv = np.array([10, 255, 255])
    mask1 = cv2.inRange(hsv, lowerb=low_hsv, upperb=high_hsv)

    lower2 = np.array([156, 43, 46])
    upper2 = np.array([180, 255, 255])
    mask2 = cv2.inRange(hsv, lower2, upper2)
    mask = mask1 + mask2
    nonzero = np.nonzero(mask)
    if np.size(nonzero) == 0:
        return [-1, -1], [-1, -1], [-1, -1]
    row_min = np.min(nonzero[0])
    row_max = np.max(nonzero[0])
    col_min = np.min(nonzero[1])
    col_max = np.max(nonzero[1])
    mid_row = int((row_max + row_min) / 2)
    mid_col = int((col_max + col_min) / 2)

    cv2.line(
        img,
        (col_min, mid_row),
        (col_max, mid_row),
        (255, 255, 255),
        1,
    )
    cv2.line(
        img,
        (mid_col, row_min),
        (mid_col, row_max),
        (255, 255, 255),
        1,
    )
    cv2.imshow("img ", img)
    cv2.waitKey(1)
    # cv2.imshow("test", mask1 + mask2)
    # cv2.waitKey(1)

    return [col_min, col_max], [row_min, row_max], [mid_col, mid_row]


def CtrlDrone():
    global vis
    global mav
    global detect_flag
    img = None
    init_dist_x = 0
    init_dist_y = 0
    img_cnt_x = 320
    img_cnt_y = 240
    tolerance_cnt = 15
    tolerance_y = 15
    tolerance_x = 20
    kx = 0.015
    ky = 0.009
    kz = 0.005
    while True:
        if vis.hasData[0]:
            img = vis.Img[0]
            vis.hasData[0] = False
            # cv2.imshow("img", vis.Img[0])
            cv2.waitKey(1)
            [x_min, x_max], [y_min, y_max], [cnt_x, cnt_y] = CalCirle(img)
            if x_min == -1:
                time.sleep(0.001)
                continue
            if not detect_flag:
                detect_flag = True
            dist_x = x_max - x_min
            dist_y = y_max - y_min
            if init_dist_x == 0:
                init_dist_x = dist_x
            if init_dist_y == 0:
                init_dist_y = dist_y
            vx = 0
            vy = 0
            vz = 0
            yaw_rate = 0
            if abs(cnt_x - img_cnt_x) > tolerance_cnt:
                vy = ky * (cnt_x - img_cnt_x)
            if abs(cnt_y - img_cnt_y) > tolerance_cnt:
                vz = kz * (cnt_y - img_cnt_y)
            if abs(dist_y - init_dist_y) > tolerance_y:
                vx = kx * (init_dist_y - dist_y)

            mav.SendVelFRD(vx, vy, vz, 0)
            time.sleep(0.001)


ctrl_drone_th = threading.Thread(target=CtrlDrone)
ctrl_drone_th.start()

keyboard.add_hotkey("ctrl+up", MoveCircle, args=("front",))
keyboard.add_hotkey("ctrl+down", MoveCircle, args=("back",))
keyboard.add_hotkey("up", MoveCircle, args=("up",))
keyboard.add_hotkey("down", MoveCircle, args=("down",))
keyboard.add_hotkey("right", MoveCircle, args=("right",))
keyboard.add_hotkey("left", MoveCircle, args=("left",))

keyboard.wait()