#!/usr/bin/env python3

import sys
import rospy
import threading

from sensor_msgs.msg import PointCloud2
from lasr_vision_msgs.srv import YoloDetection3D, YoloDetection3DRequest

# figure out what model we are using
if len(sys.argv) >= 2:
    model = sys.argv[1]
else:
    model = "yolov8n-seg.pt"

processing = False


def detect(pcl):
    global processing
    processing = True
    rospy.loginfo("Received pcl message")

    try:
        detect_service = rospy.ServiceProxy("/yolov8/detect3d", YoloDetection3D)
        req = YoloDetection3DRequest()
        req.pcl = pcl
        req.dataset = model
        req.confidence = 0.25
        req.nms = 0.4
        resp = detect_service(req)
        print(resp)
    except rospy.ServiceException as e:
        rospy.logerr("Service call failed: %s" % e)
    finally:
        processing = False


def pcl_callback(pcl):
    global processing
    if processing:
        return

    t = threading.Thread(target=detect, args=(pcl,))
    t.start()


def listener():
    rospy.init_node("pcl_listener", anonymous=True)
    rospy.wait_for_service("/yolov8/detect3d")
    rospy.Subscriber("/xtion/depth_registered/points", PointCloud2, pcl_callback)
    rospy.spin()


if __name__ == "__main__":
    listener()