"""
motor_controller.py

Controls one DC motor through a TB6612FNG motor driver.

Connections:
    PWMA -> GPIO12
    AIN1 -> GPIO23
    AIN2 -> GPIO25

The motor starts with a short high-power boost, then runs
continuously at a constant cruising speed.
"""

from time import sleep

from gpiozero import PWMOutputDevice, DigitalOutputDevice


# ============================================================
# GPIO PINS
# ============================================================

PWMA_PIN = 12
AIN1_PIN = 23
AIN2_PIN = 25


# ============================================================
# MOTOR SETTINGS
# ============================================================

PWM_FREQUENCY = 1000

# Brief startup boost helps the motor begin spinning.
START_SPEED = 0.60
START_TIME = 0.50

# Normal constant running speed.
DEFAULT_RUN_SPEED = 0.45

# Default motor direction:
#  1 = forward
# -1 = reverse
DEFAULT_DIRECTION = 1


# ============================================================
# HARDWARE SETUP
# ============================================================

PWMA = PWMOutputDevice(
    PWMA_PIN,
    frequency=PWM_FREQUENCY,
    initial_value=0
)

AIN1 = DigitalOutputDevice(
    AIN1_PIN,
    initial_value=False
)

AIN2 = DigitalOutputDevice(
    AIN2_PIN,
    initial_value=False
)


# ============================================================
# HELPER FUNCTIONS
# ============================================================

def clamp_speed(speed):
    """
    Limit a PWM speed value to the valid range of 0.0 to 1.0.
    """

    try:
        speed = float(speed)
    except (TypeError, ValueError):
        raise ValueError("Motor speed must be a number from 0.0 to 1.0")

    return max(0.0, min(1.0, speed))


def set_direction(direction):
    """
    Set the motor's direction.

    Args:
        direction:
             1 = forward
            -1 = reverse
             0 = stop
    """

    if direction == 1:
        AIN1.on()
        AIN2.off()

    elif direction == -1:
        AIN1.off()
        AIN2.on()

    elif direction == 0:
        stop()

    else:
        raise ValueError("Motor direction must be 1, -1, or 0")


# ============================================================
# MOTOR CONTROL
# ============================================================

def run_motor(
    direction=DEFAULT_DIRECTION,
    speed=DEFAULT_RUN_SPEED,
    startup_boost=True
):
    """
    Start the motor and leave it running continuously.

    Args:
        direction:
             1 = forward
            -1 = reverse

        speed:
            Constant running speed from 0.0 to 1.0.

        startup_boost:
            When True, briefly runs at START_SPEED before
            dropping to the requested constant speed.
    """

    speed = clamp_speed(speed)

    if direction not in (1, -1):
        raise ValueError("Motor direction must be 1 or -1")

    if speed == 0:
        stop()
        return

    set_direction(direction)

    if startup_boost:
        boost_speed = max(speed, START_SPEED)

        print(
            f"Motor startup boost: "
            f"direction={direction}, speed={boost_speed:.2f}"
        )

        PWMA.value = boost_speed
        sleep(START_TIME)

    PWMA.value = speed

    print(
        f"Motor running: "
        f"direction={direction}, speed={speed:.2f}"
    )


def set_speed(speed):
    """
    Change the motor speed without changing direction.

    Args:
        speed:
            PWM value from 0.0 to 1.0.
    """

    speed = clamp_speed(speed)
    PWMA.value = speed

    print(f"Motor speed changed to {speed:.2f}")


def reverse(speed=DEFAULT_RUN_SPEED):
    """
    Safely reverse the motor direction.
    """

    speed = clamp_speed(speed)

    stop()
    sleep(0.15)

    run_motor(
        direction=-1,
        speed=speed,
        startup_boost=True
    )


def forward(speed=DEFAULT_RUN_SPEED):
    """
    Run the motor forward.
    """

    run_motor(
        direction=1,
        speed=speed,
        startup_boost=True
    )


def stop():
    """
    Stop the motor completely.
    """

    PWMA.value = 0

    AIN1.off()
    AIN2.off()

    print("Motor stopped")


# ============================================================
# CLEANUP
# ============================================================

def cleanup():
    """
    Stop the motor and release all GPIO resources.
    """

    try:
        stop()
    except Exception:
        pass

    try:
        PWMA.close()
    except Exception:
        pass

    try:
        AIN1.close()
    except Exception:
        pass

    try:
        AIN2.close()
    except Exception:
        pass


# ============================================================
# STANDALONE TEST
# ============================================================

if __name__ == "__main__":

    print("Motor controller test starting...")

    try:
        run_motor(
            direction=DEFAULT_DIRECTION,
            speed=DEFAULT_RUN_SPEED
        )

        while True:
            sleep(1)

    except KeyboardInterrupt:
        print("\nMotor test stopped")

    finally:
        cleanup()