import atexit

from compLib.PCA9685 import PCA9685
from compLib.LogstashLogging import Logging

pwm = PCA9685(0x40, debug=True)
pwm.setPWMFreq(50)

MOTOR_COUNT = 4
MAX_MOTOR_SPEED = 4095.0
MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0


class Motor(object):
    """Class used to control the motors
    """

    @staticmethod
    def power(port: int, percent: float):
        """Set specified motor to percentage power

        :param port: Port, which the motor is connected to. 0-3, 0 -> top left, 3 -> top right
        :param percent: Percentage of max speed. between -100 and 100
        :raises: IndexError
        """
        Logging.get_logger().debug(f"Motor.power {port} {percent}")

        if port < 0 or port >= MOTOR_COUNT:
            raise IndexError("Invalid Motor port specified!")

        if percent < -100 or percent > 100:
            raise IndexError("Invalid Motor speed specified! Speed is between -100 and 100 percent!")

        forward = True
        if percent < 0:
            percent = abs(percent)
            forward = False

        # bottom left motor is inverted - REEEEEEEEEEEE
        if port == 1:
            forward = not forward

        adjusted_speed = int(min(max(0.0, percent), 100.0) * MOTOR_PERCENTAGE_MULT)

        if forward:
            pwm.setMotorPwm(port * 2, adjusted_speed)
            pwm.setMotorPwm(port * 2 + 1, 0)
        else:
            pwm.setMotorPwm(port * 2, 0)
            pwm.setMotorPwm(port * 2 + 1, adjusted_speed)

    @staticmethod
    def all_off():
        """
        Turns of all motors
        """
        Logging.get_logger().debug(f"Motor.all_off")

        for i in range(0, MOTOR_COUNT):
            Motor.power(i, 0)


atexit.register(Motor.all_off)