import math
import collections
from .pca9685 import ServoController, map_range
from .dc_motor_controller import DcMotor, setup_gpio

ServoMinMax = collections.namedtuple('ServoMinMax', ['minval', 'maxval'])

try:
    from RPi import GPIO
except ImportError:
    from .mockGpio import GPIO

PIN_MOTOR_A_ENABLE = 6
PIN_MOTOR_A_REVERSE = 12

PIN_MOTOR_B_ENABLE = 0
PIN_MOTOR_B_REVERSE = 5

SERVO_ARM_L = 15
SERVO_ARM_R = 14
SERVO_EYE_L = 13
SERVO_EYE_R = 12
SERVO_NECK_ROTATE = 11
SERVO_NECK_TOP = 10
SERVO_NECK_BOTTOM = 9

SERVO_MIN_MAX = {
    SERVO_ARM_L: ServoMinMax(50, 130),
    SERVO_ARM_R: ServoMinMax(50, 130),
    SERVO_EYE_L: ServoMinMax(40, 100),
    SERVO_EYE_R: ServoMinMax(80, 120),
    SERVO_NECK_ROTATE: ServoMinMax(60, 120),  # 60 - 90 - 120
    SERVO_NECK_TOP: ServoMinMax(30, 180),
    SERVO_NECK_BOTTOM: ServoMinMax(10, 180),
}


class WallE:
    def __init__(self):
        setup_gpio()
        self.motor_a = DcMotor(PIN_MOTOR_A_ENABLE, PIN_MOTOR_A_REVERSE)
        self.motor_b = DcMotor(PIN_MOTOR_B_ENABLE, PIN_MOTOR_B_REVERSE)
        self.servo_controller = ServoController()
        self.servo_controller.setup()

    def set_servo(self, channel, value):
        try:
            min_val, max_val = SERVO_MIN_MAX[channel]
        except IndexError:
            return None
        value = min(max_val, max(min_val, value))
        self.servo_controller.write(channel, value)

    def set_arm_l(self, val):
        self.set_servo(SERVO_ARM_L, val)

    def set_arm_r(self, val):
        self.set_servo(SERVO_ARM_R, val)

    def set_eye_l(self, val):
        self.set_servo(SERVO_EYE_L, val)

    def set_eye_r(self, val):
        self.set_servo(SERVO_EYE_R, val)

    def set_neck_rotate(self, val):
        self.set_servo(SERVO_NECK_ROTATE, val)

    def set_neck_top(self, val):
        self.set_servo(SERVO_NECK_TOP, val)

    def set_neck_bottom(self, val):
        self.set_servo(SERVO_NECK_BOTTOM, val)

    def set_movement(self, angle: float, force: float):
        if force > 1:
            force = 1.0

        x = math.cos(angle) * force
        y = math.sin(angle) * force

        if y >= 0:
            n_mot_premix_l = 1.0 if x >= 0 else 1.0 + x
            n_mot_premix_r = 1.0 - x if x >= 0 else 1.0
        else:
            n_mot_premix_l = 1.0 - x if x >= 0 else 1.0
            n_mot_premix_r = 1.0 if x >= 0 else 1.0 + x

        n_mot_premix_l = n_mot_premix_l * y / 1.0
        n_mot_premix_r = n_mot_premix_r * y / 1.0
        n_piv_speed = x
        f_piv_scale = 0.0 if abs(y) > .4 else 1.0 - abs(y) / .4
        mot_mix_l = (1.0 - f_piv_scale) * n_mot_premix_l + f_piv_scale * n_piv_speed
        mot_mix_r = (1.0 - f_piv_scale) * n_mot_premix_r + f_piv_scale * (-n_piv_speed)
        self.motor_a.set(mot_mix_l)
        self.motor_b.set(mot_mix_r)

    def set_eye_velocity(self, angle: float, distance: float):
        if distance > 1:
            distance = 1
        up_down = distance * math.sin(angle)
        left_right = distance * math.cos(angle)
        self.set_servo(SERVO_ARM_L, map_range(up_down, -1.0, 1.0, 50, 130))
        self.set_servo(SERVO_ARM_R, map_range(left_right, -1.0, 1.0, 130, 50))