import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import serial
import time
import threading
class TM512AC_RS485_Driver:
    def __init__(self, port, baudrate=250000, num_ic=5):
        self.ser = serial.Serial(
            port=port,
            baudrate=baudrate,
            bytesize=serial.EIGHTBITS,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_TWO,
            timeout=0.1,
            rtscts=False,
            dsrdtr=False
        )
        self.num_ic = num_ic  # TM512芯片数量
        self.channels = 3 * num_ic  # 总通道数（RGB模式）

    def _send_break(self):
        self.ser.sendBreak(0.000088)  # 88μs Break
        time.sleep(0.000008)  # 8μs MAB

    def _send_dmx_byte(self, value):
        self.ser.write(bytes([value]))

    def send_frame(self, rgb_data):
        self._send_break()
        self._send_dmx_byte(0x00)
        for val in rgb_data[:self.channels]:
            self._send_dmx_byte(val)
        remaining = 512 - self.channels
        if remaining > 0:
            self.ser.write(bytes([0] * remaining))

    def close(self):
        self.ser.close()
class LedCommand(Node):
    def __init__(self):
        super().__init__('led_dev_node')
        self.create_subscription(String,'/led_cmd',self.led_cmd_cb,10)
        self.driver = TM512AC_RS485_Driver(port='/dev/ttyUSB0', num_ic=5)
        self.cur_color='green'
        self.cur_val=5
        self.update=0
        self.get_logger().info("led_dev_node will start....")
        self.th1=threading.Thread(target=self.com_task)
        self.th1.start()
        
    def com_task(self):
        idx=0
        while True:
            #self.led_blink(self.cur_color,self.cur_val)
            self.all_color(self.cur_color, -self.cur_val)
            self.all_color(self.cur_color, self.cur_val)
    def wait_task(self):
        self.th1.join()
    def all_color(self,rgb_data,xnum):
        num_ic=4
        rgb = [0, 0, 0,0] * num_ic
        start_id=0
        end_id=256
        if xnum<0:
            start_id=256
            end_id=0
        val=0
        if xnum==0:
            val=255
            if rgb_data=='red':
                rgb = [val, 0, 0,0] * num_ic
            elif rgb_data=='green':
                rgb = [0, val, 0,0] * num_ic
            elif rgb_data=='blue':
                rgb = [0, 0, val,0] * num_ic
            elif rgb_data=='cyan':#青色
                rgb = [0, val, val,0] * num_ic
            elif rgb_data=='yellow':#黄色
                rgb = [val, val, 0,0] * num_ic
            elif rgb_data=='purple':#紫色
                rgb = [val, 0, val,0] * num_ic
            elif rgb_data=='white':#紫色
                rgb = [val, val, val,0] * num_ic
            print(rgb)
            self.driver.send_frame(rgb)
            return
        for i in range(start_id,end_id,xnum):
            #offset=i*5
            val=i##offset
            if val > 255:
                val=255
            if rgb_data=='red':
                rgb = [val, 0, 0,0] * num_ic 
            elif rgb_data=='green':
                rgb = [0, val, 0,0] * num_ic
            elif rgb_data=='blue':
                rgb = [0, 0, val,0] * num_ic
            elif rgb_data=='cyan':#青色
                rgb = [0, val, val,0] * num_ic
            elif rgb_data=='yellow':#黄色
                rgb = [val, val, 0,0] * num_ic
            elif rgb_data=='purple':#紫色
                rgb = [val, 0, val,0] * num_ic
            elif rgb_data=='white':#紫色
                rgb = [val, val, val,0] * num_ic
            print(rgb)
            self.driver.send_frame(rgb)
            if self.update==1:
                self.update=0
                break
            time.sleep(0.01)
    def led_blink(self,color,val):
        self.all_color(color, -val)
        self.all_color(color, val)
    def led_cmd_cb(self,msg):
        xcmd=msg.data
        print('led cmd:',xcmd)
        cmd,val=xcmd.split(',')
        if cmd in ['red','green','blue','cyan','yellow','purple','white','off']:
            self.cur_color=cmd
            self.cur_val=int(val)
            self.update=1
        ###self.led_blink(cmd,int(val))
def main():
    rclpy.init()
    node=LedCommand()
    rclpy.spin(node)
    node.wait_task()
    node.destroy_node()
    rclpy.shutdown()

if __name__=='__main__':
    main()


