from pybricks.pupdevices import DCMotor, Motor, Remote, Light
from pybricks.parameters import Port, Stop, Button, Color
from pybricks.hubs import CityHub
from pybricks.tools import wait, StopWatch
from pybricks.iodevices import PUPDevice
from pybricks.pupdevices import ColorDistanceSensor 
from uerrno import ENODEV
from pybricks.parameters import *  # Color

# --- Configuration ---
MOTOR_PORT = Port.A
SENSOR_PORT = Port.B 
MOTOR_POWER = -60

last_command_processed = None

# --- Initialization ---
hub = CityHub(broadcast_channel=2, observe_channels=[1])
motor = DCMotor(MOTOR_PORT)
sensor = ColorDistanceSensor(SENSOR_PORT)



# HSV to RGB conversion function
def hsv_to_rgb(hsv):
    h, s, v = hsv[0], hsv[1] / 100, hsv[2] / 100
    c = v * s
    h1 = h / 60
    x = c * (1 - abs(h1 % 2 - 1))
    if h1 < 1:
        r1, g1, b1 = c, x, 0
    elif h1 < 2:
        r1, g1, b1 = x, c, 0
    elif h1 < 3:
        r1, g1, b1 = 0, c, x
    elif h1 < 4:
        r1, g1, b1 = 0, x, c
    elif h1 < 5:
        r1, g1, b1 = x, 0, c
    else:
        r1, g1, b1 = c, 0, x

    m = v - c
    return (int((r1 + m) * 255), int((g1 + m) * 255), int((b1 + m) * 255))



print("✅ Slave train with Handshake Protocol ready.")
print("Waiting for 'REQUEST_RUN' on channel 1...")

# --- Main Loop ---
while True:
    data = hub.ble.observe(1)

    if data and data != last_command_processed:
        last_command_processed = data

        if data == b"REQUEST_RUN":
            # (ACK and motor run logic is the same)
            # ...
            hub.ble.broadcast(b"ACK_RUN")
            motor.dc(MOTOR_POWER)
            while True:
                hsv = sensor.hsv()
                rgb = hsv_to_rgb(hsv)
                r, g, b = hsv_to_rgb(hsv)
                if r > 170 and g < 25 and b < 25:
                    break
                wait(5)
            motor.stop()

            # Tell the master the run is complete (on channel 2)
            for i in range(5):
                hub.ble.broadcast(b"RUN_COMPLETE")
                wait(100) # Wait for 100 milliseconds

            
            print("\nSequence complete. Waiting for next request...")
        
        # FIX: The logic for STANDBY is no longer needed.
        # elif data == b"STANDBY":
        #    print("Received STANDBY from master. Ready for new cycle.")

    wait(10)