#!/usr/bin/env python3
import rospy

from serial import Serial

from simple_example.msg import Neopixel, Neopixels
from simple_example.srv import FillNeopixels, FillNeopixelsResponse


class NeopixelDriverNode:
    """ROS Node for controllering NeoPixels."""

    def __init__(self):
        self._msg = Neopixels
        self.pixel_count = rospy.get_param('/simple_example/pixel_count', 1)
        self.port = rospy.get_param('/simple_example/port', "/dev/ttyUSB0")
        self.serial = Serial(self.port, 115200, timeout=1)

    def listen(self):
        rospy.init_node("light_ctrl", anonymous=True)
        rospy.Subscriber("/neopixel", self._msg, self.neopixel_ctrl_callback)
        rate = rospy.spin()

    def neopixel_ctrl_callback(self, data):
        rospy.loginfo(f"Recieved: {data}")
        #import pdb;pdb.set_trace()
        cmds = ""
        for i, pixel in enumerate(data.pixels):
            cmds += f"{i},{pixel.r},{pixel.g},{pixel.b};"
        cmds += "\n"
        rospy.loginfo(f"Sending to ESP32: {cmds}")
        self.serial.write(cmds.encode("ascii"))


if __name__ == "__main__":
    rospy.loginfo("Firing up neopixel driver.")
    node = NeopixelDriverNode()
    node.listen()
    rospy.loginfo("Neopixel driver is shutting down.")