import threading
import time
from adafruit_servokit import ServoKit # type: ignore

# Initialize ServoKit
kit = ServoKit(channels=16)

# Constants for delay
DELAY = 0.25

# front/up back/down neutral
SERVO_ANGLES = {
    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: (170, 10, 90),
    10: (120, 10, 65),
    11: (170, 10, 90)
}

# Movement functions
def move_leg(servo1, servo2):
    """Moves a pair of servos synchronously and prints their positions."""
    front1, back1, neutral1 = SERVO_ANGLES[servo1]
    front2, back2, neutral2 = SERVO_ANGLES[servo2]

    kit.servo[servo1].angle = front1
    time.sleep(DELAY)
    kit.servo[servo2].angle = front2
    print(f"Servos {servo1} and {servo2} set to FRONT angles: {front1}, {front2}")
    time.sleep(DELAY)

    kit.servo[servo1].angle = back1
    print(f"Servo {servo1} set to BACK angle: {back1}")
    time.sleep(DELAY)

    kit.servo[servo2].angle = back2
    print(f"Servo {servo2} set to BACK angle: {back2}")
    time.sleep(DELAY)

    kit.servo[servo1].angle = front1
    print(f"Servo {servo1} set to FRONT angle: {front1}")
  
    time.sleep(DELAY)

    kit.servo[servo2].angle = neutral2
    print(f"Servo {servo2} set to NEUTRAL angle: {neutral2}")

    time.sleep(DELAY)

    kit.servo[servo1].angle = neutral1
    print(f"Servo {servo1} set to NEUTRAL angle: {neutral1}")

    

def left_leg_0():
    move_leg(0, 6)

def left_leg_1():
    move_leg(1, 7)

def left_leg_2():
    move_leg(2, 8)

def right_leg_3():
    move_leg(3, 9)

def right_leg_4():
    move_leg(4, 10)

def right_leg_5():
    move_leg(5, 11)

# List of leg movements
def group_1():
    threads = [
        threading.Thread(target=left_leg_0),
        threading.Thread(target=left_leg_2),
        threading.Thread(target=right_leg_4),
    ]
    for thread in threads:
        thread.start()
    for thread in threads:
        thread.join()

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

# Main loop
if __name__ == "__main__":
    while True:
        print("Executing Group 1 movements...")
        group_1()
        time.sleep(DELAY)

        print("Executing Group 2 movements...")
        group_2()
        time.sleep(DELAY)