from compLib.Spi import Spi, Register


SERVO_COUNT = 8

MIN_ANGLE = -90.0
MAX_ANGLE = 90.0


def map(x, in_min, in_max, out_min, out_max):
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


class Servo:
    """Control the servo ports on the robot
    """

    @staticmethod
    def set_position(port: int, angle: float, offset: float = 0.0):
        """Set position of servo connected to port

        :param port: port between 1 and 8
        :param angle: Angle of servo
        """
        if port <= 0 or port > SERVO_COUNT:
            raise IndexError("Invalid Servo port specified!")
        
        angle = max(min(angle, MAX_ANGLE), MIN_ANGLE)

        mapped_angle = int(map(angle, MIN_ANGLE, MAX_ANGLE, 0, 65535))
        print(mapped_angle)

        if port == 1:
            Spi.write(Register.SERVO_1_PWM_H, 2, mapped_angle)
            Spi.write(Register.PWM_1_CTRL, 1, 4)
        elif port == 2:
            Spi.write(Register.SERVO_2_PWM_H, 2, mapped_angle)
            Spi.write(Register.PWM_1_CTRL, 1, 4)
        elif port == 3:
            Spi.write(Register.SERVO_3_PWM_H, 2, mapped_angle)
            Spi.write(Register.PWM_2_CTRL, 1, 4)
        elif port == 4:
            Spi.write(Register.SERVO_4_PWM_H, 2, mapped_angle)
            Spi.write(Register.PWM_2_CTRL, 1, 4)
        elif port == 5:
            Spi.write(Register.SERVO_5_PWM_H, 2, mapped_angle)
            Spi.write(Register.PWM_3_CTRL, 1, 4)
        elif port == 6:
            Spi.write(Register.SERVO_6_PWM_H, 2, mapped_angle)
            Spi.write(Register.PWM_3_CTRL, 1, 4)
        elif port == 7:
            Spi.write(Register.SERVO_7_PWM_H, 2, mapped_angle)
            Spi.write(Register.PWM_4_CTRL, 1, 4)
        elif port == 8:
            Spi.write(Register.SERVO_8_PWM_H, 2, mapped_angle)
            Spi.write(Register.PWM_4_CTRL, 1, 4)

    @staticmethod
    def setup_position():
        """Set position of servos to the position used during the setup process
        """
        
        for i in range(1, SERVO_COUNT + 1):
            Servo.set_position(i, 0)