import tracking.lidar_tracker_pb2 as lidar_tracker_pb2
from tracking.lidar_tracker_pb2_grpc import PersonTrackingServicer
from tracking.lidar_cache import LidarCache
from multiprocessing import Process
import messaging.message_factory as mf
from rplidar import RPLidar
from Messaging import messages


class LidarServicer(PersonTrackingServicer):

    def __init__(self):
        # TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer.
        self.cache = LidarCache(RPLidar('/dev/ttyUSB0'), measurements=100)
        self._mFactory = None
        self._port = None

    def set_tracking_group(self, request, context):
        pass

    def stop_tracking(self, request, context):
        self.cache.stop_scanning()

    def start_tracking(self, request, context):
        """Starts the lidar cache, streaming on the provided port."""
        self._port = request.value
        self.cache.start_cache(self)

    def onGroupsChanged(self, message):
        if self._mFactory is None:
            # Create the zmq socket in the thread that it will be used, just to be safe.
            self._mFactory = mf.getZmqPubSubStreamer(self._port)
        self._mFactory.send_message_topic("lidar_map", messages.ProtoMessage(message=message.SerializeToString()))