import asyncio
from dataclasses import dataclass
from robot_control.src.robot.movement import RobotMovement
from robot_control.src.api.grbl_handler import GRBLHandler
from robot_control.src.utils.config import RobotConfig, GRBLConfig, MovementConfig, GPIOConfig, MQTTConfig


# grbl_config = GRBLConfig(port="debug", baudrate=115200)
grbl_config = GRBLConfig(port="/dev/ttyACM1", baudrate=115200)
test_config = MovementConfig(
        speed=1000,
        safe_height=10,
        work_height=0
    )

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

y_offset = 130

positions = [
    (453, 1028 - y_offset, 0),
    (453, 1028 - y_offset, 107),
    (453, 1028 - y_offset, 0),
    (170, 760 - y_offset, 0),
    (170, 760 - y_offset, 40),
    (170, 760 - y_offset, 0),
    (453, 600 - y_offset, 0), # neutral pos
]

async def wait_for_enter():
    # Use asyncio.Event to coordinate between input and async code
    event = asyncio.Event()
    
    def input_callback():
        input()  # Wait for Enter
        event.set()
    
    # Run input in a separate thread to not block the event loop
    loop = asyncio.get_running_loop()
    await loop.run_in_executor(None, input_callback)
    await event.wait()

async def perform_movement_sequence():
    for pos in positions:
        print(f"Moving to position {pos}...")
        await movement.move_to_position(*pos)

async def test_movement():
    await grbl_handler.connect()
    await movement.perform_homing()
    
    print("Press Enter to start movement sequence (or Ctrl+C to exit)...")
    while True:
        await wait_for_enter()
        print("Starting movement sequence...")
        await perform_movement_sequence()
        print("\nPress Enter to repeat sequence (or Ctrl+C to exit)...")

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