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_servo_pin = 6
    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> [speed_mmmin] [servo_speed_ms] - move to position")
    print("  q - quit")
    print("Examples:")
    print("  m -50 60        - move to -50mm, 60° with default speeds")
    print("  m -50 60 1000   - move to -50mm, 60° at 1000 mm/min")
    print("  m -50 60 1000 500 - move to -50mm, 60° at 1000 mm/min, servo 500ms")

    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:
                parts = cmd.split()
                if len(parts) < 3:
                    print("Usage: m <pos_mm> <rot_deg> [speed_mmmin] [servo_speed_ms]")
                    continue
                    
                pos_mm = float(parts[1])
                rot_deg = float(parts[2])
                speed_mmmin = float(parts[3]) if len(parts) > 3 else None
                servo_speed_ms = float(parts[4]) if len(parts) > 4 else 1000
                
                print(f"Moving to {pos_mm} mm, {rot_deg}°" + 
                      (f" at {speed_mmmin} mm/min" if speed_mmmin else " (default speed)") +
                      f" (servo: {servo_speed_ms}ms)")
                
                if speed_mmmin:
                    # Convert speed to step delay
                    step_delay = mag_dist._speed_to_step_delay(speed_mmmin)
                    mag_dist.move_mag_distributor_at_pos(pos_mm, rot_deg, servo_speed_ms, step_delay)
                else:
                    mag_dist.move_mag_distributor_at_pos(pos_mm, rot_deg, servo_speed_ms)
                    
                print(f"✓ Moved to {pos_mm} mm, {rot_deg}°")
            except Exception as e:
                print(f"Invalid move command: {e}")
                print("Usage: m <pos_mm> <rot_deg> [speed_mmmin] [servo_speed_ms]")
        else:
            print("Unknown command.")

if __name__ == "__main__":
    main()