#!/usr/bin/env python3

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

import grpc

import MotorControl.motorService_pb2 as motorService_pb2
import MotorControl.motorService_pb2_grpc as motorService_pb2_grpc
from MotorControl.gpiozero.motor_session import Motor

servo_pin = 18

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

    def SetThrottle(self, request_iterator, context):
        # If we don't get a response every 3 seconds, stop the car.
        # This isn't a stream right now, however may change it to be so since we'll constantly
        # be sending values...
        throttleFailed = False
        for throttleRequest in request_iterator:
            print('Setting throttle to: ' + str(throttleRequest.throttle))
            self.set_timeout(3)
            throttleFailed = self.motor.set_throttle(throttleRequest.throttle)
            if not throttleFailed:
                break
        return motorService_pb2.ThrottleResponse(throttleSet = throttleFailed)

    def SetSteering(self, request, context):
        # TODO: Fix this to use the motor object as well to check for bounds.
        print('Setting steering to: ' + str(request.steering))
        self.servo.value = 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.
        vary_timout -- Default 200, the additional random varying time (0 - vary_timeout) to add to 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.motor.stop()

    def start_server(self):
        server = grpc.server(futures.ThreadPoolExecutor(max_workers=8))
        motorService_pb2_grpc.add_CarControlServicer_to_server(self, server)
        server.add_secure_port('[::]:50051', self.create_credentials())
        server.start()
        while True:
            time.sleep(60*60)

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

        pvtKeyBytes = bytes(open(pvtKeyPath).read())
        pvtCertBytes = bytes(open(pvtCertPath).read())

        return grpc.ssl_channel_credentials(pvtCertBytes, pvtKeyBytes)

motor = Motor()
servo = Servo(servo_pin)
servicer = MotorServicer(motor, servo)

service_thread = Thread(target=servicer.start_server)
service_thread.start()