from . import SlamController_pb2_grpc
from . import SlamController_pb2
from . import slam_streamer as slam
from multiprocessing import Process


class SlamServicer(SlamController_pb2_grpc.SlamControlServicer):
    slam_thread = None

    def __init__(self, lidar_connection):
        print('Servicer initialised')
        self.slam = slam.SlamStreamer(lidar_connection=lidar_connection)

    def start_map_streaming(self, request, context):
        print('Received Map Start Streaming Request')
        if self.slam_thread is None:
            print('initialising slam_thread')
            # Don't bother creating and starting slam more than once.
            self.slam.port = request.port
            self.slam.map_pixels = request.map_size_pixels
            self.slam.map_meters = request.map_size_meters
            self.slam_thread = Process(target=self.slam.start)
            self.slam_thread.start()
        return SlamController_pb2.Empty()

    def stop_streaming(self, request, context):
        if self.slam_thread is not None:
            self.slam.stop_scanning()
            self.slam_thread.join()
            self.slam = None
        return SlamController_pb2.Empty()
