import sys
sys.path.append('/home/pi/TurboPi/')
import signal
import curses
import cv2
import time
import threading
import Camera
import HiwonderSDK.PID as PID
import HiwonderSDK.mecanum as mecanum

sys.path.append('/home/pi/TurboPi/')

servo1 = 1760
servo2 = 1450
img_centerx = 320
line_centerx = -1
size = (640, 480)
__isRunning = False
swerve_pid = PID.PID(P=0.001, I=0.00001, D=0.000001)
servo_data = None
car = mecanum.MecanumChassis()
car_en = False

def init():
    print("Remote Control Init")

def start():
    global __isRunning
    __isRunning = True
    print("Remote Control Start")

def manual_stop(signum, frame):
    global __isRunning
    __isRunning = False
    car_stop()

def stop():
    global __isRunning
    global _stop
    __isRunning = False
    _stop = True
    car_stop()

def car_stop():
    car.set_velocity(0, 90, 0)

def control_robot(stdscr):
    stdscr.clear()
    stdscr.addstr(0, 0, "Use WASD to control the movement of the robot.")
    stdscr.addstr(1, 0, "Press 'q' to quit.")
    stdscr.refresh()

    key_pressed = {'w': False, 'a': False, 's': False, 'd': False}

    stdscr.nodelay(True)
    while __isRunning:
        try:
            key = stdscr.getch()

            if key == ord('w'):
                key_pressed['w'] = True
            elif key == ord('a'):
                key_pressed['a'] = True
            elif key == ord('s'):
                key_pressed['s'] = True
            elif key == ord('d'):
                key_pressed['d'] = True
            elif key == ord('q'):
                stop()
                break
            elif key == -1:  # No key is pressed
                key_pressed = {k: False for k in key_pressed}

            # Determine direction based on key states
            if key_pressed['w']:
                car.set_velocity(30, 90, 0)
            elif key_pressed['a']:
                car.set_velocity(0, 180, -0.3)
            elif key_pressed['s']:
                car.set_velocity(30, 270, 0)
            elif key_pressed['d']:
                car.set_velocity(0, 0, 0.3)
            else:
                car_stop()

            stdscr.refresh()
            time.sleep(0.035) #short delay to make the loop more efficient

        except Exception as e:
            car_stop()
            camera.camera_close()
            cv2.destroyAllWindows()
            break

def camera_stream():
    global __isRunning
    while __isRunning:
        img = camera.frame
        if img is not None:
            frame = img.copy()
            frame_resize = cv2.resize(frame, (320, 240))
            cv2.imshow('frame', frame_resize)
            key = cv2.waitKey(1)
            if key == 27:
                stop()
                break
            else:
                time.sleep(0.01)
    camera.camera_close()
    cv2.destroyAllWindows()

if __name__ == '__main__':
    init()
    start()
    camera = Camera.Camera()
    camera.camera_open(correction=True)

    signal.signal(signal.SIGINT, manual_stop)

    # Start the camera stream in a separate thread
    camera_thread = threading.Thread(target=camera_stream)
    camera_thread.start()

    # Start the robot control interface
    curses.wrapper(control_robot)

    # Ensure everything is stopped when done
    camera_thread.join()
    stop()
    
    # press q to quit program