#!/usr/bin/python3
import rospy

from std_msgs.msg import UInt8MultiArray, UInt16MultiArray, Int16MultiArray, String,UInt32MultiArray
from geometry_msgs.msg import TwistStamped, Vector3
import time
import sys
import os
import numpy as np
import mml
import math
from pathlib import Path



################################################################

def error(msg):
	print(msg)
	sys.exit(0)

class client:
	def control_led(self, led_pos, colour_str, brightness):
		r = int(colour_str[1:3], 16)
		g = int(colour_str[3:5], 16)
		b = int(colour_str[5:7], 16)
		led_value = (int(brightness) << 24) | (r << 16) | (g << 8) | b
		self.illum.data[led_pos] = led_value
		self.pub_illum.publish(self.illum)
		
	# def callback_stream(self, msg):
		# self.buffer_space = msg.data[0]
		# self.buffer_total = msg.data[1]
		# self.buffer_stuff = self.buffer_total - self.buffer_space
		
	# # def callback_clap(self, msg):
		# # print(msg)
		
		
	def loop(self):

		# loop
		while not rospy.core.is_shutdown():
			color = sys.argv[1]
			PositionArray = sys.argv[2].replace(']','').replace('[','')
			position = PositionArray.replace('"','').split(",")

			for x in position:
				print(x)
				self.control_led(int(x),color,255) #LM 
			time.sleep(2)
			
			exit()
		

	def __init__(self):
		# Variables for Illumination
		self.illum = UInt32MultiArray()
		self.illum.data = [0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF]
		self.illumInt = 255
		self.illumState = 0
		
		# robot name
		topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME")

		#Publisher for Illum to control LED's while we are processing requests
		topic = topic_base_name + "/control/illum"
		self.pub_illum = rospy.Publisher(topic, UInt32MultiArray, queue_size=0)
		#self.velocity_pub = rospy.Publisher(topic_base_name + "/control/cmd_vel", TwistStamped, queue_size=0)
		

		

if __name__ == "__main__":

	rospy.init_node("Set_Color", anonymous=False)
	main = client()
	main.loop()




