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

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)

    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.cache.start_cache(mf.getZmqPubSubStreamer(request.value))