#!/usr/bin/env python3

import rospy
from tauv_msgs.msg import BucketList, BucketDetection, Pose, RegisterObjectDetections
from visualization_msgs.msg import Marker, MarkerArray
import typing
from geometry_msgs.msg import Vector3
from std_msgs.msg import ColorRGBA

rospy.init_node('bucket_to_tf')
pub = rospy.Publisher("/markers", MarkerArray, queue_size=10)

# def hsv_to_rgb(h, s, v):
#     if s == 0.0: return (v, v, v)
#     i = int(h*6.) # XXX assume int() truncates!
#     f = (h*6.)-i; p,q,t = v*(1.-s), v*(1.-s*f), v*(1.-s*(1.-f)); i%=6
#     if i == 0: return (v, t, p)
#     if i == 1: return (q, v, p)
#     if i == 2: return (p, v, t)
#     if i == 3: return (p, q, v)
#     if i == 4: return (t, p, v)
#     if i == 5: return (v, p, q)

# def string_to_color(s: str, alpha):
#     hue = (s.__hash__() % 1000000) / 1000000
#     rgb = hsv_to_rgb(hue, 1, 1)
#     return ColorRGBA(rgb[0], rgb[1], rgb[2], alpha) 

colormap = {
    "badge": ColorRGBA(1,1,0,1),
    "phone": ColorRGBA(1,0,0,1),
    "notebook": ColorRGBA(0,0,1,1)
}

det_id_counter = 0
def update_dets(msg: RegisterObjectDetections):
    global det_id_counter
    #print("bucket to tf running")
    dets: typing.List[BucketDetection] = msg.objdets
    ma = MarkerArray()
    for d in dets:
        m = Marker()
        m.type = Marker.CUBE
        m.ns = "bucket_detection"
        m.pose.position = d.position
        m.scale = Vector3(.1, .1, .1)
        m.header.stamp = rospy.Time.now()
        m.header.frame_id = "odom_ned"
        m.action = 0
        m.id = det_id_counter
        m.lifetime = rospy.Duration(2)
        det_id_counter += 1
        m.frame_locked = True
        m.color = colormap[d.tag]

        ma.markers.append(m)

    pub.publish(ma)

def update(msg: BucketList):
    #print("bucket to tf running")
    dets: typing.List[BucketDetection] = msg.bucket_list
    ma = MarkerArray()
    id = 0
    for d in dets:
        m = Marker()
        m.type = Marker.SPHERE
        m.ns = "bucket_item"
        m.pose.position = d.position
        m.scale = Vector3(.7, .7, .7)
        m.header.stamp = rospy.Time.now()
        m.header.frame_id = "odom_ned"
        m.action = 0
        m.id = id
        id += 1
        m.frame_locked = True
        c = colormap[d.tag]
        c.a = 0.3
        m.color = c

        ma.markers.append(m)

    pub.publish(ma)

sub = rospy.Subscriber("/register_object_detection", RegisterObjectDetections, update_dets)
sub = rospy.Subscriber("/bucket_list", BucketList, update)

rospy.spin()