import time
from adafruit_servokit import ServoKit

# Initialize ServoKit
kit = ServoKit(channels=16)

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

# Constants for delay
DELAY = 1

# Function to move a leg with user input
def move_leg_with_prompt(leg, angle):
    print(f"Leg {leg}: Currently at {kit.servo[leg].angle if kit.servo[leg].angle is not None else 'unknown'}. Moving to {angle}.")
    input("Press Enter to continue...")
    kit.servo[leg].angle = angle
    time.sleep(DELAY)

# Main function
def main():
    legs = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
    print("Moving all legs to the initial position (55 degrees).")
    for leg in legs:
        move_leg_with_prompt(leg, ANGLE_INITIAL)

    while True:
        for leg in legs:
            move_leg_with_prompt(leg, ANGLE_START)
            move_leg_with_prompt(leg, ANGLE_MIDDLE)

if __name__ == "__main__":
    main()