import threading
import time
import sys
import select
import queue
from adafruit_servokit import ServoKit  # type: ignore

# Initialize ServoKit
kit = ServoKit(channels=16)

# Constants for delay
DELAY = 0.15

# Servo angle configurations
FORWARD = {
    0: (0, 110, 70),
    1: (0, 110, 70),
    2: (0, 110, 70),
    3: (0, 110, 70),
    4: (0, 110, 70),
    5: (0, 110, 70),
    6: (10, 110, 90),
    7: (10, 120, 65),
    8: (70, 170, 90),
    9: (170, 70, 90),
    10: (120, 10, 65),
    11: (110, 10, 90),
}

BACK = { 
    0: (0, 110, 70),
    1: (0, 110, 70),
    2: (0, 110, 70),
    3: (0, 110, 70),
    4: (0, 110, 70),
    5: (0, 110, 70),
    6: (110, 10, 90),
    7: (120, 10, 65),
    8: (170, 70, 90),
    9: (70, 170, 90),
    10: (10, 120, 65),
    11: (10, 110, 90),
}

ROTATE_RIGHT = { 
    0: (0, 110, 70),
    1: (0, 110, 70),
    2: (0, 110, 70),
    3: (0, 110, 70),
    4: (0, 110, 70),
    5: (0, 110, 70),
    6: (10, 170, 90),
    7: (10, 120, 65),
    8: (10, 170, 90),
    9: (10, 170, 90),
    10: (10, 120, 65),
    11: (10, 170, 90),
}

ROTATE_LEFT = {  
    0: (0, 110, 70),
    1: (0, 110, 70),
    2: (0, 110, 70),
    3: (0, 110, 70),
    4: (0, 110, 70),
    5: (0, 110, 70),
    6: (170, 10, 90),
    7: (120, 10, 65),
    8: (170, 10, 90),
    9: (170, 10, 90),
    10: (120, 10, 65),
    11: (170, 10, 90),
}

# Movement functions
def move_leg(servo1, servo2, servo_angles):
    up, down, neutral1 = servo_angles[servo1]
    front, back, neutral2 = servo_angles[servo2]

    kit.servo[servo1].angle = up
    time.sleep(DELAY)
    kit.servo[servo2].angle = front
    time.sleep(DELAY)

    kit.servo[servo1].angle = down
    time.sleep(DELAY)

    kit.servo[servo2].angle = back
    time.sleep(DELAY*2)

    kit.servo[servo1].angle = up
    time.sleep(DELAY)

    kit.servo[servo2].angle = neutral2
    time.sleep(DELAY)

    kit.servo[servo1].angle = neutral1

# Leg movement functions
def left_leg_0(servo_angles):
    move_leg(0, 6, servo_angles)

def left_leg_1(servo_angles):
    move_leg(1, 7, servo_angles)

def left_leg_2(servo_angles):
    move_leg(2, 8, servo_angles)

def right_leg_3(servo_angles):
    move_leg(3, 9, servo_angles)

def right_leg_4(servo_angles):
    move_leg(4, 10, servo_angles)

def right_leg_5(servo_angles):
    move_leg(5, 11, servo_angles)

# Group movement functions
def group_1(servo_angles):
    threads = [
        threading.Thread(target=left_leg_0, args=(servo_angles,)),
        threading.Thread(target=left_leg_2, args=(servo_angles,)),
        threading.Thread(target=right_leg_4, args=(servo_angles,)),
    ]
    for thread in threads:
        thread.start()
    for thread in threads:
        thread.join()

def group_2(servo_angles):
    threads = [
        threading.Thread(target=left_leg_1, args=(servo_angles,)),
        threading.Thread(target=right_leg_3, args=(servo_angles,)),
        threading.Thread(target=right_leg_5, args=(servo_angles,)),
    ]
    for thread in threads:
        thread.start()
    for thread in threads:
        thread.join()

# Main loop control
def is_input_available():
    return select.select([sys.stdin], [], [], 0)[0]

# Main execution
if __name__ == "__main__":
    print("Press 'w' for FORWARD, 'a' for LEFT, 'd' for RIGHT, 's' for BACK. Press 'q' to quit.")

    command_queue = queue.Queue()

    try:
        while True:
            if is_input_available():
                input_char = sys.stdin.read(1).strip()
                if input_char in ['w', 'a', 'd', 's']:
                    command_queue.put(input_char)
                elif input_char == 'q':
                    print("Exiting...")
                    break

            while not command_queue.empty():
                command = command_queue.get()
                if command == 'w':
                    print("Executing FORWARD movement...")
                    group_1(FORWARD)
                    group_2(FORWARD)
                elif command == 'a':
                    print("Executing LEFT movement...")
                    group_1(ROTATE_LEFT)
                    group_2(ROTATE_LEFT)
                elif command == 'd':
                    print("Executing RIGHT movement...")
                    group_1(ROTATE_RIGHT)
                    group_2(ROTATE_RIGHT)
                elif command == 's':
                    print("Executing BACK movement...")
                    group_1(BACK)
                    group_2(BACK)

            time.sleep(0.1)  # Idle loop
    except KeyboardInterrupt:
        print("\nProgram terminated.")