import atexit
from enum import IntEnum

from compLib.LogstashLogging import Logging
from compLib.Spi import Spi, Register

MOTOR_COUNT = 4
MAX_MOTOR_SPEED = 65535
MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0


class MotorMode(IntEnum):
    COAST = 0,
    FORWARD = 1,
    BACKWARD = 2,
    BREAK = 3


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

    @staticmethod
    def pwm(port: int, pwm: int, mode: MotorMode):
        """Set specified motor to a specific pwm value and motor mode

        :param port: Port, which the motor is connected to. 1-4 allowed
        :param pwm: Raw PWM value which the motor should be set to. From 0 to 2^16 - 1
        :param mode: Motor mode. See enum MotorMode for more info
        :raises: IndexError
        """
        if port <= 0 or port > MOTOR_COUNT:
            raise IndexError("Invalid Motor port specified!")

        if port == 1:
            Spi.write(Register.MOTOR_1_PWM_H, 2, pwm)
            Spi.write(Register.MOTOR_1_CTRL, 1, int(mode))
        elif port == 2:
            Spi.write(Register.MOTOR_2_PWM_H, 2, pwm)
            Spi.write(Register.MOTOR_2_CTRL, 1, int(mode))
        elif port == 3:
            Spi.write(Register.MOTOR_3_PWM_H, 2, pwm)
            Spi.write(Register.MOTOR_3_CTRL, 1, int(mode))
        elif port == 4:
            Spi.write(Register.MOTOR_4_PWM_H, 2, pwm)
            Spi.write(Register.MOTOR_4_CTRL, 1, int(mode))

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

        :param port: Port, which the motor is connected to. 1-4
        :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!")

        mode = MotorMode.COAST
        if percent < 0:
            percent = abs(percent)
            mode = MotorMode.BACKWARD
        elif percent > 0:
            mode = MotorMode.FORWARD

        pwm = percent * MOTOR_PERCENTAGE_MULT

        Motor.pwm(port, int(pwm), mode)

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

        for i in range(1, MOTOR_COUNT + 1):
            Motor.active_break(i)

    @staticmethod
    def active_break(port: int):
        """
        Actively break with a specific motor

        :param port: Port, which the motor is connected to. 1-4
        """
        Motor.pwm(port, 0, MotorMode.BREAK)


atexit.register(Motor.all_off)