#!/usr/bin/env python3

from threading import Timer, Thread
from gpiozero import Servo, Device
from concurrent import futures
import time

import grpc

import control.motorService_pb2 as motorService_pb2
import control.motorService_pb2_grpc as motorService_pb2_grpc
from control.gpiozero.vehicle import Vehicle
from control.gpiozero.mockvehicle import MockVehicle
from slam.slam_servicer import SlamServicer
import slam.SlamController_pb2_grpc as SlamController_pb2_grpc


class MotorServicer(motorService_pb2_grpc.CarControlServicer):
    def __init__(self, vehicle):
        self.vehicle = vehicle
        self._timer = None

    def SetThrottle(self, request, context):
        # gRPC streams currently don't work between python and android.
        # If we don't get a response every 3 seconds, stop the car.
        throttleFailed = False
        print('Setting throttle to: ' + str(request.throttle))
        self.set_timeout(3)
        throttleFailed = self.vehicle.set_throttle(request.throttle)
        return motorService_pb2.ThrottleResponse(throttleSet=throttleFailed)

    def SetSteering(self, request, context):
        print('Setting steering to: ' + str(request.steering))
        self.vehicle.steering = request.steering
        return motorService_pb2.SteeringResponse(steeringSet=True)

    def set_timeout(self, min_timeout):
        """Stops the old timer and restarts it to the specified time.

        min_timeout -- The minimum time that can be used for the timer.
        """
        if self._timer is not None:
            self._timer.cancel()
        self._timer = Timer(min_timeout, self.timeout_elapsed)
        self._timer.start()

    def timeout_elapsed(self):
        """Election or heartbeat timeout has elapsed."""
        print("Node timeout elapsed")
        self.vehicle.stop()

    def start_server(self):
        server = grpc.server(futures.ThreadPoolExecutor(max_workers=8))
        motorService_pb2_grpc.add_CarControlServicer_to_server(self, server)
        SlamController_pb2_grpc.add_SlamControlServicer_to_server(
            self.create_slam_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_slam_servicer(self):
        return SlamServicer('/dev/ttyUSB0')

    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()
    servicer = MotorServicer(vehicle)

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