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

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


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_movement(self, angle: float, distance: float):
        if distance > 0:
            distance = distance/50.0  # map 0 - 50 to 0 - 1

        angle_rad = math.radians(angle)
        cos_angle = math.cos(angle_rad)
        sin_angle = math.sin(angle_rad)

        velocity_l = 0
        velocity_r = 0

        if cos_angle > 0 and sin_angle > 0:
            if cos_angle > sin_angle:
                velocity_r = cos_angle * distance
                velocity_l = sin_angle * distance
            elif cos_angle < sin_angle:
                velocity_r = sin_angle * distance
                velocity_l = cos_angle * distance
        elif cos_angle < 0 and sin_angle > 0:
            if cos_angle < sin_angle:
                velocity_r = 1 - sin_angle * distance
                velocity_l = sin_angle * distance
            elif cos_angle > abs(sin_angle):
                velocity_r = sin_angle * distance
                velocity_l = cos_angle * distance
        else:
            pass
        self.motor_a.set(velocity_l)
        self.motor_b.set(velocity_r)

    def set_eye_velocity(self, up_down: float, left_right: float):
        minval = 50
        maxval = 130
        self.servo_controller.write(SERVO_ARM_L, map_range(up_down, 0.0, 1.0, minval, maxval))
        self.servo_controller.write(SERVO_ARM_R, map_range(left_right, 0.0, 1.0, minval, maxval))