#!/usr/bin/env python3

from threading import Timer, Thread
from concurrent import futures
import time

import grpc

import control.motorService_pb2_grpc as motorService_pb2_grpc
from control.gpio.vehicle import Vehicle
from control.motor_servicer import MotorServicer
from slam.slam_servicer import SlamServicer
import slam.SlamController_pb2_grpc as SlamController_pb2_grpc
import tracking.lidar_tracker_pb2_grpc as lidar_tracker_pb2_grpc
from tracking.lidar_servicer import LidarServicer


class CarServer():

    def __init__(self, vehicle):
        self.vehicle = vehicle

    def start_server(self):
        server = grpc.server(futures.ThreadPoolExecutor(max_workers=8))
        motorService_pb2_grpc.add_CarControlServicer_to_server(self.create_motor_servicer(), server)
        SlamController_pb2_grpc.add_SlamControlServicer_to_server(
            self.create_slam_servicer(), server)
        lidar_tracker_pb2_grpc.add_PersonTrackingServicer_to_server(
            self.create_lidar_servicer(), server)
        # Disable tls for local testing.
        # server.add_secure_port('[::]:50051', self.create_credentials())
        server.add_insecure_port('[::]:50051')
        server.start()
        while True:
            time.sleep(60*60)

    def create_motor_servicer(self):
        return MotorServicer(self.vehicle)

    def create_slam_servicer(self):
        return SlamServicer()

    def create_lidar_servicer(self):
        return LidarServicer()

    def create_credentials(self):
        # Relativise this stuff.
        pvtKeyPath = '/home/pi/tls/device.key'
        pvtCertPath = '/home/pi/tls/device.crt'

        pvtKeyBytes = open(pvtKeyPath, 'rb').read()
        pvtCertBytes = open(pvtCertPath, 'rb').read()

        return grpc.ssl_server_credentials([[pvtKeyBytes, pvtCertBytes]])


if __name__ == '__main__':
    vehicle = Vehicle()
    server = CarServer(vehicle)

    # Can't remember why I do this, is it even needed?
    service_thread = Thread(target=server.start_server)
    service_thread.start()