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("  s <servo_id> <angle> [speed_ms] - move servo (1: pin 6, 2: pin 13)")
    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")
    print("  s 1 90 1000     - move servo 1 to 90° in 1000ms")
    print("  s 2 45          - move servo 2 to 45° with default speed")

    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:
                    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]")
        elif cmd.startswith("s "):
            try:
                parts = cmd.split()
                if len(parts) < 3:
                    print("Usage: s <servo_id> <angle> [speed_ms]")
                    continue
                servo_id = int(parts[1])
                angle = float(parts[2])
                speed_ms = float(parts[3]) if len(parts) > 3 else 1000
                if servo_id == 1:
                    servo_pin = 6
                elif servo_id == 2:
                    servo_pin = 13
                else:
                    print(f"Unknown servo_id: {servo_id}. Use 1 or 2.")
                    continue
                gpio.set_servo_angle_smooth(servo_pin, angle, speed_ms)
                print(f"✓ Moved servo {servo_id} (pin {servo_pin}) to {angle}° in {speed_ms}ms")
            except Exception as e:
                print(f"Invalid servo command: {e}")
                print("Usage: s <servo_id> <angle> [speed_ms]")
        else:
            print("Unknown command.")

if __name__ == "__main__":
    main()