from machine import Pin
from time import sleep
from servo import Servo

servo_1 = Servo(17)
servo_2 = Servo(18)
servo_3 = Servo(19)

current_angle_1 = 0
current_angle_2 = 0
current_angle_3 = 0

def gradual_move(servo, target_angle, current_angle, step=5, delay=0.1):
    if current_angle < target_angle:
        for angle in range(current_angle, target_angle + 1, step):
            servo.move(angle)
            sleep(delay)
        current_angle = target_angle 
    elif current_angle > target_angle:
        for angle in range(current_angle, target_angle - 1, -step):
            servo.move(angle)
            sleep(delay)
        current_angle = target_angle  
    
    return current_angle

while True:
    control_servo = int(input("Enter (1 for Picking Item / 2 for Placing Items / 3 for Resetting Servos / 4 for Exiting): "))
    print(control_servo)
    
    if control_servo == 1:
        print("Picking Item...")
        current_angle_3 = gradual_move(servo_3, 90, current_angle_3)
        current_angle_2 = gradual_move(servo_2, 90, current_angle_2)
        current_angle_1 = gradual_move(servo_1, 90, current_angle_1)
    elif control_servo == 2:
        print("Placing Item...")
        current_angle_1 = gradual_move(servo_1, 45, current_angle_1)
        current_angle_2 = gradual_move(servo_2, 45, current_angle_2)
        current_angle_3 = gradual_move(servo_3, 45, current_angle_3)
    elif control_servo == 3:
        print("Resetting Servos...")
        current_angle_1 = gradual_move(servo_1, 0, current_angle_1)
        current_angle_2 = gradual_move(servo_2, 0, current_angle_2)
        current_angle_3 = gradual_move(servo_3, 0, current_angle_3)
    elif control_servo == 4:
        print("Exiting...")
        break
    else:
        print("Invalid input.")