from pynput import keyboard
import time

from vision import Tracker
import cv2
from enum import Enum



TICKS_PER_SEC = 30


class Status(Enum):
    INIT = 0
    DETECT = 1
    TRACK = 2
    LANDING = 3

class PIDController:
    def __init__(self, kp, ki, kd, current_time):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.previous_error = 0
        self.integral = 0
        self.last_time = current_time

    def control(self, error, current_time):
        current_time = current_time
        delta_time = current_time - self.last_time
        if delta_time == 0:
            delta_time = 1 / TICKS_PER_SEC
        derivative = (error - self.previous_error) / delta_time
        self.integral += error * delta_time

        output = self.kp * error + self.ki * self.integral + self.kd * derivative

        self.previous_error = error
        self.last_time = current_time

        return output


class ManualControl:
    def __init__(self, uav):
        self.uav = uav
        self.listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release)
        
    def start(self):
        self.listener.start()

    def on_press(self, key):
        try:
            if key == keyboard.Key.up:
                self.uav.for_back_velocity = 50
            elif key == keyboard.Key.down:
                self.uav.for_back_velocity = -50
            elif key == keyboard.Key.left:
                self.uav.left_right_velocity = -50
            elif key == keyboard.Key.right:
                self.uav.left_right_velocity = 50
            elif key.char == 'w':
                self.uav.up_down_velocity = 50
            elif key.char == 's':
                self.uav.up_down_velocity = -50
            elif key.char == 'q':
                self.uav.yaw_velocity = -50
            elif key.char == 'e':
                self.uav.yaw_velocity = 50
            # print(f'left_right_velocity: {self.uav.left_right_velocity}, for_back_velocity: {self.uav.for_back_velocity}, up_down_velocity: {self.uav.up_down_velocity}, yaw_velocity: {self.uav.yaw_velocity}')
        except AttributeError:
            pass

    def on_release(self, key):
        try:
            if key == keyboard.Key.up or key == keyboard.Key.down:
                self.uav.for_back_velocity = 0
            elif key == keyboard.Key.left or key == keyboard.Key.right:
                self.uav.left_right_velocity = 0
            elif key.char == 'w' or key.char == 's':
                self.uav.up_down_velocity = 0
            elif key.char == 'q' or key.char == 'e':
                self.uav.yaw_velocity = 0
        except AttributeError:
            pass


class AutoControl:
    def __init__(self, uav):
        self.uav = uav
        self.target = [0, 0, 0, 0]
        self.pid = PIDController(1, 0, 0, time.time())

    def detect(self):
        front_img = self.uav.frame_queue.get()
        cv2.imshow("ROI select", front_img[:, :, 0:3])
        self.gROI = cv2.selectROI("ROI select", front_img[:, :, 0:3], False)
        if (not self.gROI):
            print("空框选，退出")
            quit()
        self.gTracker = Tracker(tracker_type="KCF")
        self.gTracker.initWorking(front_img[:, :, 0:3], self.gROI)
        print("start tracking")
        self.uav.status = Status.TRACKING
        cv2.destroyWindow("ROI select")

    def track(self):
        front_img = self.uav.frame_queue.get()


class FakeUAV:
    def __init__(self) -> None:
        self.left_right_velocity = 0
        self.for_back_velocity = 0
        self.up_down_velocity = 0
        self.yaw_velocity = 0

    def send_rc_control(self, left_right_velocity, for_back_velocity, up_down_velocity, yaw_velocity):
        print(f'left_right_velocity: {left_right_velocity}, for_back_velocity: {for_back_velocity}, up_down_velocity: {up_down_velocity}, yaw_velocity: {yaw_velocity}')

if __name__ == '__main__':
    # uav = FakeUAV()
    # manual_control=ManualControl(uav)
    # manual_control.start()
    # while True:
    #     time.sleep(0.05)

    import logging
    logging.info('aaa')