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
import tracking.devices.factory as lidar_factory

from messaging import messages
import tracking.algorithms as alg

class LidarServicer(PersonTrackingServicer):

    def __init__(self, vehicle=None):
        # TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer.
        # Also, it would allow creating the service without the lidar being connected. 
        self.cache = LidarCache(lidar_factory.get_lidar(), measurements=100)
        self.cache.add_groups_changed_listener(self)
        self._mFactory = None
        self._port = None
        self._vehicle = vehicle
        self._tracked_group = None

    def set_tracking_group(self, request, context):
        self._tracked_group = request.value

    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()

    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()))

        if self._tracked_group is not None and self._vehicle is not None:
            # Update vehicle to correctly follow the tracked group.
            # Leave for now, need to work out exactly how this will change.
            # alg.dualServoChange(alg.find_centre())
            pass