import atexit
import logging

try:
    from RPi import GPIO
except ImportError:
    from .mockGpio import GPIO

LOG = logging.getLogger(__name__)

PWM_FREQUENCY = 10_000  # 10 KHz
MOTORS = []


def setup_gpio():
    GPIO.setmode(GPIO.BCM)

    atexit.register(cleanup_gpio)


def cleanup_gpio():
    LOG.info("cleaning up")
    for motor in MOTORS:
        # noinspection PyProtectedMember
        motor._stop()
    GPIO.cleanup()


class DcMotor:
    def __init__(self, pin_enable, pin_reverse):
        self.pin_enable = pin_enable
        self.pin_reverse = pin_reverse

        GPIO.setup(pin_enable, GPIO.OUT)
        GPIO.setup(pin_reverse, GPIO.OUT)
        GPIO.output(pin_reverse, GPIO.LOW)

        self.pwm_enable = GPIO.PWM(pin_enable, PWM_FREQUENCY)
        self.pwm_enable.start(0.0)
        self.is_reverse = False

        MOTORS.append(self)

    def set(self, val):
        """
        Set the speed of the motor

        Args:
            val: Value between 1.0 and -1.0
        """
        self._set_reverse(val)
        absval = abs(val)
        if absval > 1:
            logging.warning(f"clipping {val} to 1")
            absval = 1

        self.pwm_enable.ChangeDutyCycle(absval * 100)

    def _set_reverse(self, val):
        if val >= 0 and self.is_reverse:
            GPIO.output(self.pin_reverse, GPIO.LOW)
            self.is_reverse = False
        elif val < 0 and not self.is_reverse:
            GPIO.output(self.pin_reverse, GPIO.HIGH)
            self.is_reverse = True

    def _stop(self):
        self.pwm_enable.stop()
        GPIO.output(self.pin_reverse, GPIO.LOW)

    def stop(self):
        self._stop()
        MOTORS.remove(self)