import sys
import time

from robot_control.src.robot.mag_distributor import MagDistributor
from robot_control.src.utils.config import ConfigParser
from robot_control.src.api.gpio import GPIOInterface, MockGPIO, PiGPIO

def main():
    # You may need to adjust these to your environment
    config = ConfigParser().config
    gpio_config = config.gpio
    gpio_config.mag_dist_rot_pin = 14
    if gpio_config.debug:
        gpio = MockGPIO()
    else:
        gpio = PiGPIO()

    mag_dist = MagDistributor(config, gpio)

    print("MagDistributor Playground")
    print("Commands:")
    print("  h - home")
    print("  m <pos_mm> <rot_deg> - move to position (mm, deg)")
    print("  q - quit")

    while True:
        cmd = input("Enter command: ").strip()
        if cmd == "q":
            print("Exiting.")
            break
        elif cmd == "h":
            print("Homing...")
            import asyncio
            asyncio.run(mag_dist.home())
            print("Homed.")
        elif cmd.startswith("m "):
            try:
                _, pos_mm, rot_deg = cmd.split()
                pos_mm = float(pos_mm)
                rot_deg = float(rot_deg)
                mag_dist.move_mag_distributor_at_pos(pos_mm, rot_deg)
                print(f"Moved to {pos_mm} mm, {rot_deg} deg.")
            except Exception as e:
                print(f"Invalid move command: {e}")
        else:
            print("Unknown command.")

if __name__ == "__main__":
    main()