import atexit
from enum import IntEnum

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

MOTOR_COUNT = 4
MAX_MOTOR_SPEED = 65535
MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
NEW_MOTOR_CURVE = [0.0, 0.0, 0.5, 60.0, 199.83333333333334, 377.66666666666663, 990.3333333333333, 1860.6666666666665, 2587.0, 3091.6666666666665, 3489.0, 3860.5, 4197.333333333333, 4432.166666666667, 4647.166666666666, 4873.166666666666, 5054.333333333334, 5208.666666666667, 5353.0, 5466.5, 5604.0]
MOTOR_CURVE = [0.0, 0.0, 426.5, 692.0, 842.5, 953.5, 1032.5, 1090.5, 1135.5, 1171.0, 1203.5, 1230.0, 1249.5, 1268.0, 1283.0, 1298.5, 1308.0, 1320.0, 1332.0, 1339.5, 1352.5]

SPEED_LOCK = False
SPEED_MULT = 1.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 SPEED_LOCK:
            return

        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.PWM_1_CTRL, 1, int(mode))
        elif port == 2:
            Spi.write(Register.MOTOR_2_PWM_H, 2, pwm)
            Spi.write(Register.PWM_2_CTRL, 1, int(mode))
        elif port == 3:
            Spi.write(Register.MOTOR_3_PWM_H, 2, pwm)
            Spi.write(Register.PWM_3_CTRL, 1, int(mode))
        elif port == 4:
            Spi.write(Register.MOTOR_4_PWM_H, 2, pwm)
            Spi.write(Register.PWM_4_CTRL, 1, int(mode))

    @staticmethod
    def power_raw(port: int, percent: float, log_metric = True):
        """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
        """

        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!")

        if log_metric:
            MetricsLogging.put("MotorRaw", float(percent), port)

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

        percent *= SPEED_MULT
        pwm = percent * MOTOR_PERCENTAGE_MULT

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

    @staticmethod
    def power(port: int, percent: float):
        """Set specified motor to percentage power, percentage is linearized
        so that it's roughly proportional to the speed

        :param port: Port, which the motor is connected to. 1-4
        :param percent: Percentage of max speed. between -100 and 100
        :raises: IndexError
        """
        raw_power = raw_power = Motor.__linearizePower(MOTOR_CURVE, abs(percent))
        if percent < 0:
            raw_power *= -1

        MetricsLogging.put("Motor", float(percent), port)
        Motor.power_raw(port, raw_power, False)

    @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)

    @staticmethod
    def set_motor_curve(curve):
        """
        Set the global motor curve, must be a float array with exactly 21 elements.
        [0]  = x ticks/s  (0% power)
        [1]  = x ticks/s  (5% power)
        [2]  = x ticks/s  (10% power)
        ...
        [20] = x ticks/s  (100% power)

        :param curve: float array with 21 elements
        :raises: ValueError
        """
        if (len(curve) != 21):
            raise ValueError('The motor curve is invalid, check documentation for set_motor_curve()!')
        
        global MOTOR_CURVE
        MOTOR_CURVE = curve

    @staticmethod
    def get_motor_curve():
        """
        Get the currently active motor curve. Check set_motor_curve() for more info.

        :return: current motor curve
        """
        return MOTOR_CURVE

    @staticmethod
    def __map(x, in_min, in_max, out_min, out_max):
        """
        Linear interpolation. Check https://www.arduino.cc/reference/en/language/functions/math/map/
        """
        x = float(x)
        in_min = float(in_min)
        in_max = float(in_max)
        out_min = float(out_min)
        out_max = float(out_max)

        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

    @staticmethod
    def __interpolateSpeed(curve, speed):
        """
        Interpolate a speed in the specified motor curve and return 
        the 'power percentage that is needed to reach that speed'
        """
        if speed < min(curve) or speed > max(curve):
            raise ValueError(f'Speed out of range: {str(speed)} ticks/s')

        for index in range(len(curve) - 1):
            if speed >= curve[index] and speed <= curve[index + 1] and curve[index] != curve[index + 1]:
                return Motor.__map(speed, curve[index], curve[index + 1], index * 5, index * 5 + 5)

        return 0

    @staticmethod
    def __linearizePower(curve, power):
        """
        Takes raw power and returns it corrected so that the motor speed would be roughly linear
        """
        if power < 0 or power > 100:
            raise ValueError(f'Power out of range: {str(power)}%')

        requiredSpeed = Motor.__map(power, 0, 100, min(curve), max(curve))

        return Motor.__interpolateSpeed(curve, requiredSpeed)


    @staticmethod
    def robot_state_loop():
        import time
        from compLib.Api import Seeding

        global SPEED_LOCK
        global SPEED_MULT

        while True:
            try:
                state, status = Seeding.get_robot_state()
                SPEED_MULT = state["speed"] / 100.0

                if state["left"] != -1 or state["right"] != -1:
                    Motor.power(1, state["right"])
                    Motor.power(4, state["left"])
                    SPEED_LOCK = True
                    time.sleep(state["time"])
                    SPEED_LOCK = False
                    Motor.power(1, 0)
                    Motor.power(4, 0)
                else:
                    time.sleep(0.25)
            except:
                time.sleep(0.25)
                pass

    @staticmethod
    def start_robot_state_loop():
        from threading import Thread
        robot_state_thread = Thread(target=Motor.robot_state_loop)
        robot_state_thread.setDaemon(True)
        robot_state_thread.start()

atexit.register(Motor.all_off)
