from gpiozero import Servo, Device
from gpiozero.pins.pigpio import PiGPIOFactory
import subprocess


def _safely_set_servo_value(servo, value):
    try:
        if value < -1 or value > 1:
            print("Not setting throttle, invalid value set.")
            return False
        servo.value = value
    except TypeError:
        print("throttle should be a number, preferably a float.")
        return False
    return True

def _is_pin_valid(pin):
    if isinstance(pin, int):
        if pin < 2 or pin > 21:
            print("Invalid GPIO pin")
            return False
        return True
    else:
        print("Value must be an int.")
        return False

# TODO: Allow a vector to be set to change the throttle/steering, for vehicles that don't use
# two servos for controls (e.g. drone, dog)
class Vehicle:
    def __init__(self, motor_pin=19, servo_pin=18):
        subprocess.call(['sudo', 'pigpiod'])
        Device.pin_factory = PiGPIOFactory()
        print('Using pin factory:')
        print(Device.pin_factory)
        self.motor_pin = motor_pin
        self.steering_pin = servo_pin
        self.initialise_motor()

    def initialise_motor(self):
        self._motor_servo = Servo(
            self._motor_pin, pin_factory=Device.pin_factory)
        self._steering_servo = Servo(self._steering_pin, pin_factory=Device.pin_factory)

    @property
    def throttle(self):
        return self._motor_servo.value

    @throttle.setter
    def throttle(self, value):
        _safely_set_servo_value(self._motor_servo, value)

    @property
    def steering(self):
        return self._motor_servo.value

    @steering.setter
    def steering(self, value):
        _safely_set_servo_value(self._motor_servo, value)

    @property
    def motor_pin(self):
        return self._motor_pin

    @motor_pin.setter
    def motor_pin(self, value):
        # TODO: Reinitialise the servo when the pin changes, or discard this method
        # (probably don't want to allow pin changes whilst the device is in use anyway)
        self._motor_pin = value if _is_pin_valid(value) else self._motor_pin

    @property
    def steering_pin(self):
        return self._steering_pin

    @steering_pin.setter
    def steering_pin(self, value):
        self._steering_pin = value if _is_pin_valid(value) else self._steering_pin

    def stop(self):
        self.throttle = 0
        self.steering = 0

    def change_with_vector(self, vector):
        pass