from machine import Pin, ADC, PWM
from servo import Servo
import os
servo_top = Servo(22)
servo_middle = Servo(23)
servo_bottom = Servo(21)

def convert(val, in_max, out_max) -> int:
    return val * out_max // in_max
def parseCommand(command):
    cmd_prts = command.split(" ")
    if cmd_prts[0] == "move":
        try:
            if 0 <= int(cmd_prts[1]) <= 180:
                if cmd_prts[2] == "top":
                    print("Moving top servo to angle", cmd_prts[1] + "...")
                    servo_top.move(int(cmd_prts[1]))
                elif cmd_prts[2] == "middle":
                    print("Moving middle servo to angle", cmd_prts[1] + "...")
                    servo_middle.move(int(cmd_prts[1]))
                elif cmd_prts[2] == "bottom":
                    print("Moving bottom servo to angle", cmd_prts[1] + "...")
                    servo_bottom.move(int(cmd_prts[1]))  
                else:
                    print("Invalid Input at arguement 2, Please enter (top), (middle) or (bottom).")  
            else:
                print ("Invalid Input at arguement 1, Please enter a value from 0 to 180.")
        except ValueError as E:
            print("Invalid Input at arguement 1, Please enter a value from 0 to 180.")
    elif cmd_prts[0] == "help":
        print(" ")
        print("Commands:")
        print(" ")
        print("move {angle} {servo} ===== Moves one of the servos to a certain angle")
        print("{angle} takes a number between 0 and 180.")
        print("{servo} takes (top), (middle) or (bottom).")
        print(" ")
        print("help ===== Displays the list of commands.")
    else:
        Print("Invalid Command, Type (help) to know what commands you can use.")

    
if __name__ == "__main__":
    print("")
    print("Welcome to the servo controller Serial console.")
    print("Use the (help) command to know what to do")
    print(" ")
    while True:
        command = input("> ")
        parseCommand(command)