# @Time : 2022/4/20 12:27 
# @Author : 2890199310@qq.com
# @File : KeyboardControl.py.py 
# @Software: PyCharm
# @Function:
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../PaddleClas-release-2.3")
import logging
import time
import cv2
from djitellopy import tello
# import Tello.KeyPressModule as kp  # 用于获取键盘按键
from time import sleep

# Tello初始化设置
Drone = tello.Tello()  # 创建飞行器对象
Drone.connect()  # 连接到飞行器
Drone.streamon()  # 开启视频传输
Drone.LOGGER.setLevel(logging.ERROR)  # 只显示错误信息
sleep(5)  #  等待视频初始化
# kp.init()  # 初始化按键处理模块

def getKeyboardInput(key):
    image = cv2.resize(OriginalImage, (Camera_Width, Camera_Height))
    speed = 70
    drone = Drone
    lr, fb, ud, yv = 0, 0, 0, 0
    key_pressed = 0
    # if kp.getKey("e"): #
    if key == "e":
        cv2.imwrite('D:/snap-{}.jpg'.format(time.strftime("%H%M%S", time.localtime())), image)
    # if kp.getKey("UP"):# 上升
    if key == "UP":
        Drone.takeoff()
    # elif kp.getKey("DOWN"):#下降
    if key == "DOWN":
        Drone.land()

    # if kp.getKey("j"):# 向左飞行
    if key == "j":
        key_pressed = 1
        lr = -speed
    # elif kp.getKey("l"): #向右飞行
    if key == "l":
        key_pressed = 1
        lr = speed

    # if kp.getKey("i"): #向前飞行
    if key == "i":
        key_pressed = 1
        fb = speed
    # elif kp.getKey("k"):# 向后飞行
    if key == "k":
        key_pressed = 1
        fb = -speed

    # if kp.getKey("w"):# 向上飞行
    if key == "w":
        key_pressed = 1
        ud = speed
    # elif kp.getKey("s"): #向下飞行
    if key == "s":
        key_pressed = 1
        ud = -speed

    # if kp.getKey("a"): # 向左旋转
    if key == "a":
        key_pressed = 1
        yv = -speed
    # elif kp.getKey("d"): #向右旋转
    if key == "d":
        key_pressed = 1
        yv = speed
    InfoText = "battery : {0}% height: {1}cm   time: {2}".format(drone.get_battery(), drone.get_height(), time.strftime("%H:%M:%S",time.localtime()))
    cv2.putText(image, InfoText, (10, 20), font, fontScale, (0, 0, 255), lineThickness)
    if key_pressed == 1:
        InfoText = "Command : lr:{0}% fb:{1} ud:{2} yv:{3}".format(lr, fb, ud, yv)
        cv2.putText(image, InfoText, (10, 40), font, fontScale, (0, 0, 255), lineThickness)

    drone.send_rc_control(lr, fb, ud, yv)

# 主程序
# 摄像头设置
Camera_Width = 720
Camera_Height = 480
DetectRange = [6000, 11000]  # DetectRange[0] 是保持静止的检测人脸面积阈值下限，DetectRange[0] 是保持静止的检测人脸面积阈值上限
PID_Parameter = [0.5, 0.0004, 0.4]
pErrorRotate, pErrorUp = 0, 0

# 字体设置
font = cv2.FONT_HERSHEY_SIMPLEX
fontScale = 0.5
fontColor = (255, 0, 0)
lineThickness = 1




while True:
    OriginalImage = Drone.get_frame_read().frame
    Image = cv2.resize(OriginalImage, (Camera_Width, Camera_Height))
    # getKeyboardInput(drone=Drone, speed=70, image=Image)  # 按键控制
    cv2.imshow("Drone Control Centre", Image)
    cv2.waitKey(1)