#!/usr/bin/env python3

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

import grpc

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

servo_pin = 18
timeout_length = 3

class MotorServicer(motorService_pb2_grpc.CarControlServicer):
    def __init__(self):
        self.motor = Motor()
        self.servo = Servo(servo_pin)
        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: ' + throttleRequest.throttle)
            self.set_timeout(timeout_length)
            throttleFailed = self.motor.set_throttle(throttleRequest.throttle)
            if 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.
        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=10))
        motorService_pb2_grpc.add_CarControlServicer_to_server(MotorServicer(), server)
        server.add_insecure_port('[::]:50051')
        server.start()

servicer = MotorServicer()
servicer.start_server()

run = True
while run:
    inp = input('Please press q to stop the server')
    if inp == 'q' or inp == 'Q':
        run = False