import atexit
from enum import Enum

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

MOTOR_COUNT = 4


encoder_start_values = [0] * (MOTOR_COUNT + 1)


class Encoder(object):
    """Class used to read the encoders
    """

    @staticmethod
    def handle_wrap(raw_value, port):
        """Handle overflow and underflow of int for encoders.

        :param raw_value: Raw value which was read on port
        :param port: Port, which the motor is connected to. Between 1 and 4
        :raises: IndexError
        :return: Current encoder position
        """
        if port <= 0 or port > MOTOR_COUNT:
            raise IndexError("Invalid encoder port specified!")

        diff = raw_value - encoder_start_values[port]
        if diff > 2 ** 31:
            diff -= 2 ** 32
        elif diff < -2 ** 31:
            diff += 2 ** 32

        return diff

    @staticmethod
    def read_raw(port: int) -> int:
        """Read raw encoder from a specified port. Will not be reset to 0 at start.

        :param port: Port, which the motor is connected to. Between 1 and 4
        :raises: IndexError
        :return: Current encoder position
        """
        if port <= 0 or port > MOTOR_COUNT:
            raise IndexError("Invalid encoder port specified!")

        raw_value = 0

        if port == 1:
            raw_value = Spi.read(Register.MOTOR_1_POS_B3, 4)
        elif port == 2:
            raw_value = Spi.read(Register.MOTOR_2_POS_B3, 4)
        elif port == 3:
            raw_value = Spi.read(Register.MOTOR_3_POS_B3, 4)
        elif port == 4:
            raw_value = Spi.read(Register.MOTOR_4_POS_B3, 4)

        return raw_value

    @staticmethod
    def read_all_raw():
        """Read all encoders at once.
        This is faster than read_raw as it only uses one SPI call for all encoders!

        :return: Tuple of all current raw encoder positions
        """

        encoders = Spi.read_array(Register.MOTOR_1_POS_B3, 4 * 4)

        encoder_1 = int.from_bytes(
            encoders[0:4], byteorder='big', signed=False)
        encoder_2 = int.from_bytes(
            encoders[4:8], byteorder='big', signed=False)
        encoder_3 = int.from_bytes(
            encoders[8:12], byteorder='big', signed=False)
        encoder_4 = int.from_bytes(
            encoders[12:16], byteorder='big', signed=False)

        return (encoder_1, encoder_2, encoder_3, encoder_4)

    @staticmethod
    def read_all():
        """Read all encoders at once.
        This is faster than read as it only uses one SPI call for all encoders!

        :return: Tuple of all current encoder positions
        """
        encoders = Encoder.read_all_raw()

        return (Encoder.handle_wrap(encoders[0], 1), 
                Encoder.handle_wrap(encoders[1], 2),
                Encoder.handle_wrap(encoders[2], 3),
                Encoder.handle_wrap(encoders[3], 4))

    @staticmethod
    def read(port: int) -> int:
        """Read encoder from a specified port

        :param port: Port, which the motor is connected to. Between 1 and 4
        :raises: IndexError
        :return: Current encoder position
        """
        if port <= 0 or port > MOTOR_COUNT:
            raise IndexError("Invalid encoder port specified!")

        return Encoder.handle_wrap(Encoder.read_raw(port), port)

    @staticmethod
    def clear(port: int):
        """Reset encoder position to 0

        :param port: Port, which the motor is connected to. Between 1 and 4
        :raises: IndexError
        """
        if port <= 0 or port > MOTOR_COUNT:
            raise IndexError("Invalid encoder port specified!")

        encoder_start_values[port] = Encoder.read_raw(port)

    @staticmethod
    def clear_all():
        """Reset all encoder positions to 0
        """

        for i in range(1, MOTOR_COUNT + 1):
            encoder_start_values[i] = Encoder.read_raw(i)