import asyncio
import time
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

from robot_control.src.api.gpio import PiGPIO

"""
This is a test script for the robot movement system.
It initializes the GRBL handler and the robot movement system,
and performs a series of simple A->B movements to test the system.
Can also use GPIO pins to control pump and valve.
"""

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

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

pump_pin = 17
valve_pin = 27

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

y_offset = 130

# Real positions
# 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
# ]

# Test positions
positions = [
    (100, 100, 0),
    (100, 100, 107),
    (100, 100, 0),
    (150, 150, 0),
    (150, 150, 40),
    (150, 150, 0),
    (100, 150, 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 idx, pos in enumerate(positions):
        print(f"Moving to position {pos}...")
        if idx == 1:
            gpio.set_pin(pump_pin, 1)
            await asyncio.sleep(4)
            gpio.set_pin(pump_pin, 0)
        if idx == 2:
            gpio.set_pin(valve_pin, 1)
            await asyncio.sleep(0.5)
        if idx == 5:
            gpio.set_pin(valve_pin, 0)
            await asyncio.sleep(0.5)
        await movement.move_to_position(*pos)

async def test_movement():
    await grbl_handler.connect()
    
    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...")