#!/usr/bin/env python3
import random
import rospy
from simple_example.msg import Neopixel, Neopixels

COLORS = {
    "red": Neopixel(r=255, b=0, g=0, w=0),
    "blue": Neopixel(r=125, b=204, g=223, w=0),
    "yellow": Neopixel(r=120, b=153, g=23, w=0),
    "purple": Neopixel(r=255, b=0, g=153, w=0),
}

last_color = Neopixel()

msg = """
Starting Light Control.

This example is intended to work with a esp32 connected 
with via usb uart. It is listening for stdin lines of 
this format:

 "<pixel number: int>,<red: int>,<blue int>,<green: int>;\n"
------------------------------------------------------------
"""


def broadcaster():

    pub = rospy.Publisher("/neopixel", Neopixels, queue_size=1)
    rospy.init_node("light_control", anonymous=True, log_level=rospy.INFO)

    print(msg)

    while not rospy.is_shutdown():
        neo_pixel = pick_color()
        pub.publish(Neopixels(pixels=[neo_pixel]))
        rospy.sleep(2)


def pick_color():
    """Picks a random color from the color map"""
    global last_color

    color = random.choice(list(COLORS.keys()))
    while last_color == color:
        color = random.choice(list(COLORS.keys()))
    rospy.loginfo(f"Selected: {color}")
    return COLORS[color]


if __name__ == "__main__":
    try:
        broadcaster()
    except rospy.ROSInterruptException:
        pass