import asyncio
from robot_control.src.robot.movement import RobotMovement
from robot_control.src.api.grbl_handler import GRBLHandler
from robot_control.src.utils.config import GRBLConfig, MovementConfig
from robot_control.src.api.gpio import PiGPIO

"""
Interactive movement script for the robot.
- Homes only once at startup.
- Lets you enter X, Y, Z positions in a loop.
- No restart or re-homing between moves.
- Type 'exit' to quit.
"""

# GPIO pin assignments
pump_pin = 17
valve_pin = 27
servo_pin = 13

grbl_config = GRBLConfig(port="/dev/ttyACM0", baudrate=115200)
test_config = MovementConfig(
    speed_mmmin=15000,
    safe_height=10
)

grbl_handler = GRBLHandler(
    grbl_config.port,
    grbl_config.baudrate
)
movement = RobotMovement(test_config, grbl_handler)

gpio = PiGPIO([pump_pin, valve_pin, servo_pin], [])

pump_state = 0
valve_state = 0

async def main():
    await grbl_handler.connect()
    print("Robot ready. Enter X Y Z [speed_mmmin], 'p' to toggle pump, 'v' to toggle valve, 's <angle> <speed>' for smooth servo (or 'exit' to quit):")
    global pump_state, valve_state
    while True:
        user_input = input("Target position (X Y Z [speed] | p | v | s <angle> <speed>): ").strip().lower()
        if user_input in ("exit", "quit"):
            print("Exiting...")
            break
        elif user_input == "p":
            pump_state ^= 1
            gpio.set_pin(pump_pin, pump_state)
            print(f"Pump {'ON' if pump_state else 'OFF'}")
            continue
        elif user_input == "v":
            valve_state ^= 1
            gpio.set_pin(valve_pin, valve_state)
            print(f"Valve {'ON' if valve_state else 'OFF'}")
            continue
        elif user_input.startswith("s "):
            try:
                _, angle_str, duration_str = user_input.split()
                angle = float(angle_str)
                duration = float(duration_str)
                print(f"Moving servo on pin {servo_pin} to {angle}° for {duration}ms...")
                gpio.set_servo_angle_smooth(servo_pin, angle, duration)
                print("Servo move complete.")
            except Exception:
                print("Invalid servo command. Use: s <angle> <time_in_ms>")
            continue
        try:
            parts = user_input.split()
            if len(parts) not in (3, 4):
                raise ValueError
            x, y, z = map(float, parts[:3])
            if len(parts) == 4:
                speed = int(parts[3])
            else:
                speed = test_config.speed_mmmin
        except Exception:
            print("Invalid input. Please enter three or four numbers separated by spaces, 'p', 'v', 's <angle> <speed>', or 'exit'.")
            continue
        print(f"Moving to ({x}, {y}, {z}) at {speed} mm/min...")
        await movement.move_to_position(x, y, z, speed_mmmin=speed)
        print("Move complete. Enter next position, 'p', 'v', 's <angle> <speed>', or 'exit'.")

if __name__ == "__main__":
    try:
        asyncio.run(main())
    except KeyboardInterrupt:
        print("\nExiting...")