"""Sequential "pin-and-push" shepherd-dog controller.

Single-target alternative to Strömbom: each step, target the sheep
closest to the pen, park behind it, drive it through; once it latches
penned the next-closest sheep becomes the target. Naturally queues
the flock through a narrow gate.
"""

import math

from herding.world.geometry import GATE_Y, PEN_ENTRY, in_pen


DELTA_DRIVE = 1.5     # standoff behind the target sheep
APPROACH_GAIN = 1.0   # action magnitude scale (1 = full speed)


def _unit(x, y):
    d = math.hypot(x, y)
    if d < 1e-6:
        return 0.0, 0.0
    return x / d, y / d


def _is_active(x, y) -> bool:
    return (not in_pen(x, y)) and y > GATE_Y


def compute_action(dog_xy, sheep_positions, pen_target=PEN_ENTRY):
    """Return ``(vx, vy, mode)`` — same call signature as Strömbom."""
    active = [(name, x, y) for name, (x, y) in sheep_positions.items()
              if _is_active(x, y)]
    if not active:
        return 0.0, 0.0, "idle"

    name, sx, sy = min(
        active,
        key=lambda s: math.hypot(s[1] - pen_target[0], s[2] - pen_target[1]),
    )

    ux, uy = _unit(sx - pen_target[0], sy - pen_target[1])
    tx = sx + DELTA_DRIVE * ux
    ty = sy + DELTA_DRIVE * uy

    ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1])
    return APPROACH_GAIN * ax, APPROACH_GAIN * ay, f"drive:{name}"


def compute_action_debug(dog_xy, sheep_positions, pen_target=PEN_ENTRY):
    """``compute_action`` plus a debug dict (target, drive point)."""
    active = [(name, x, y) for name, (x, y) in sheep_positions.items()
              if _is_active(x, y)]
    if not active:
        return 0.0, 0.0, "idle", {
            "n_active": 0, "target_name": "",
            "target_x": 0.0, "target_y": 0.0,
            "drive_x": dog_xy[0], "drive_y": dog_xy[1],
        }

    name, sx, sy = min(
        active,
        key=lambda s: math.hypot(s[1] - pen_target[0], s[2] - pen_target[1]),
    )

    ux, uy = _unit(sx - pen_target[0], sy - pen_target[1])
    tx = sx + DELTA_DRIVE * ux
    ty = sy + DELTA_DRIVE * uy
    ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1])

    return APPROACH_GAIN * ax, APPROACH_GAIN * ay, f"drive:{name}", {
        "n_active": len(active), "target_name": name,
        "target_x": sx, "target_y": sy,
        "drive_x": tx, "drive_y": ty,
    }