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

# Initialize ServoKit
kit = ServoKit(channels=16)

# Constants for angles
ANGLE_START = 0
ANGLE_MIDDLE = 110
ANGLE_NEUTRAL = 55

# Constants for delay
DELAY = 1

# Movement functions
def move_leg(servo1, servo2):
    """Moves a pair of servos synchronously and prints their positions."""
    kit.servo[servo1].angle = ANGLE_START
    kit.servo[servo2].angle = ANGLE_START
    print(f"Servos {servo1} and {servo2} set to ANGLE_START: {ANGLE_START}")
    time.sleep(DELAY)

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

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

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

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

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)