"""Metre-square swept-body check on CMU's observed terrain and chosen path.

CMU's circular path table proposes paths. This final adapter check preserves
the actual square chassis, including the initial turn, without widening it to
its circumscribed circle for straight travel. No scene geometry enters here.
"""

import math

import numpy as np

MAX_STEP_M = 0.10
FRAME_DEADLINE_SECONDS = 0.8


def _obstacles(terrain, pose):
    terrain = np.asarray(terrain, dtype=float)
    obstacles = terrain[terrain[:, 3] > MAX_STEP_M + 1e-4, :2] - np.asarray(pose[:2])
    x, y, z, w = pose[3:]
    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
    return obstacles @ np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]])


def _clearances(obstacles):
    return np.maximum(np.abs(obstacles[:, 0]) - 0.5, np.abs(obstacles[:, 1]) - 0.5)


def _clear(obstacles, position, angle):
    delta = obstacles - position
    c, s = math.cos(angle), math.sin(angle)
    along = delta[:, 0] * c + delta[:, 1] * s
    across = -delta[:, 0] * s + delta[:, 1] * c
    initial = _clearances(obstacles)
    if np.any(initial <= 0):
        return False  # Never excuse an overlap of the actual metre-square body.
    clearance = np.maximum(np.abs(along) - 0.5, np.abs(across) - 0.5)
    # An observed point may already be inside the 5 cm reserve behind the body.
    # Permit only motion that never decreases that initial clearance. This
    # cannot authorize moving toward it, reversing into it or corner penetration.
    return bool(np.all(clearance + 1e-6 >= np.minimum(initial, 0.05)))


def command_footprint_clear(speed, yaw_rate, terrain, pose):
    """Collision monitor over deadman latency plus a conservative braking arc.

    CMU replans the route continuously. A later blocked corner must not prevent
    safe progress on its prefix; this checks the command that can actually be
    applied before the source-frame deadline, plus braking and 0.5 s reserve.
    """
    obstacles = _obstacles(terrain, pose)
    horizon = FRAME_DEADLINE_SECONDS + 0.5 + abs(speed) / 0.4 + abs(yaw_rate) / 1.6
    for t in np.arange(0, horizon + 0.025, 0.025):
        angle = yaw_rate * t
        position = (
            np.array([speed * math.sin(angle) / yaw_rate, speed * (1 - math.cos(angle)) / yaw_rate])
            if abs(yaw_rate) > 1e-6
            else np.array([speed * t, 0])
        )
        if not _clear(obstacles, position, angle):
            return False
    return True


def regulate_command(speed, yaw_rate, terrain, pose):
    """Reduce speed along CMU's same arc when its full-speed stop is unsafe.

    Scaling both components preserves curvature. The shortened stopping envelope
    is a prefix of the original arc, so search for its largest admitted scale.
    Never choose another turn/direction, ignore a hazard or creep arbitrarily.
    """
    if command_footprint_clear(speed, yaw_rate, terrain, pose):
        return speed, yaw_rate, 1.0
    low, high = 0.2, 1.0
    if not command_footprint_clear(speed * low, yaw_rate * low, terrain, pose):
        return 0.0, 0.0, 0.0
    for _ in range(7):
        middle = (low + high) / 2
        if command_footprint_clear(speed * middle, yaw_rate * middle, terrain, pose):
            low = middle
        else:
            high = middle
    return speed * low, yaw_rate * low, low


def swept_footprint_clear(path, terrain, pose):
    path = np.asarray(path, dtype=float)[:, :2]
    terrain = np.asarray(terrain, dtype=float)
    if len(path) < 2 or terrain.ndim != 2 or terrain.shape[1] != 4:
        return False
    obstacles = _obstacles(terrain, pose)

    previous_angle = 0.0
    for start, end in zip(path[:-1], path[1:], strict=True):
        delta = end - start
        length = np.linalg.norm(delta)
        if length < 1e-6:
            continue
        angle = math.atan2(delta[1], delta[0])
        turn = math.atan2(math.sin(angle - previous_angle), math.cos(angle - previous_angle))
        for fraction in np.linspace(0, 1, max(2, math.ceil(abs(turn) / 0.035) + 1)):
            if not _clear(obstacles, start, previous_angle + fraction * turn):
                return False
        for fraction in np.linspace(0, 1, max(2, math.ceil(length / 0.025) + 1)):
            if not _clear(obstacles, start + fraction * delta, angle):
                return False
        previous_angle = angle
    return True