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

"""
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.
"""

grbl_config = GRBLConfig(port="/dev/ttyUSB0", baudrate=115200)
test_config = MovementConfig(
    speed=1000,
    safe_height=10
)

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

async def main():
    await grbl_handler.connect()
    print("Robot ready. Enter X Y Z coordinates (or 'exit' to quit):")
    while True:
        user_input = input("Target position (X Y Z): ").strip()
        if user_input.lower() in ("exit", "quit"):  # Allow exit
            print("Exiting...")
            break
        try:
            x, y, z = map(float, user_input.split())
        except Exception:
            print("Invalid input. Please enter three numbers separated by spaces, or 'exit'.")
            continue
        print(f"Moving to ({x}, {y}, {z})...")
        await movement.move_to_position(x, y, z)
        print("Move complete. Enter next position or 'exit'.")

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