"""World geometry and robot specs.

Coordinates are metres; (0, 0) is the field centre, +x east, +y north.
These constants mirror ``worlds/field.wbt`` and the proto files — if
the world changes, this file is the single point of update.

    field        +y north
    +-----------+
    |           |
    |           |
    |  ......   |
    +---||||----+   y = -15  (south wall, 3 m gate at x ∈ [10, 13])
        ||||
        |pen|       y ∈ [-22, -15]
        +---+
"""

import math

# Field (square, stone-walled)
FIELD_X = (-15.0, 15.0)
FIELD_Y = (-15.0, 15.0)
FIELD_INSIDE_MARGIN = 0.5

# Pen (external, south of the field)
PEN_X = (10.0, 13.0)
PEN_Y = (-22.0, -15.0)
PEN_CENTER = (0.5 * (PEN_X[0] + PEN_X[1]), 0.5 * (PEN_Y[0] + PEN_Y[1]))
PEN_ENTRY = (0.5 * (PEN_X[0] + PEN_X[1]), -15.0)

# Gate (hole in the south wall)
GATE_X = PEN_X
GATE_Y = -15.0

# Dog spec — protos/ShepherdDog.proto
DOG_WHEEL_RADIUS = 0.038         # m
DOG_WHEEL_BASE = 0.28            # m, axle-to-axle
DOG_MAX_WHEEL_OMEGA = 70.0       # rad/s
DOG_MAX_LINEAR = DOG_WHEEL_RADIUS * DOG_MAX_WHEEL_OMEGA  # ≈ 2.66 m/s

# Sheep spec — protos/Sheep.proto
SHEEP_WHEEL_RADIUS = 0.031       # m
SHEEP_WHEEL_BASE = 0.20          # m
SHEEP_MAX_WHEEL_OMEGA = 25.0     # rad/s
SHEEP_MAX_LINEAR = SHEEP_WHEEL_RADIUS * SHEEP_MAX_WHEEL_OMEGA  # ≈ 0.78 m/s

WEBOTS_DT = 0.016                # seconds (matches WorldInfo.basicTimeStep)

# Virtual south wall — env and controller both keep the dog north of this.
DOG_SOUTH_LIMIT = -14.5

MAX_SHEEP = 10


def in_pen(x: float, y: float) -> bool:
    """True if (x, y) lies inside the external pen rectangle."""
    return PEN_X[0] < x < PEN_X[1] and PEN_Y[0] < y < PEN_Y[1]


def in_field(x: float, y: float, margin: float = 0.0) -> bool:
    return (FIELD_X[0] + margin <= x <= FIELD_X[1] - margin
            and FIELD_Y[0] + margin <= y <= FIELD_Y[1] - margin)


def in_gate_corridor(x: float, y: float, margin: float = 0.0) -> bool:
    """True if (x, y) lies in the column of the gate (between field and pen)."""
    return (PEN_X[0] - margin <= x <= PEN_X[1] + margin
            and PEN_Y[0] - margin <= y <= GATE_Y + margin)


def is_penned_position(x: float, y: float, latch_margin: float = 0.2) -> bool:
    """True iff (x, y) is in the gate column and south of the gate line."""
    return (PEN_X[0] - latch_margin <= x <= PEN_X[1] + latch_margin
            and y <= GATE_Y)


def distance_to_pen_entry(x: float, y: float) -> float:
    return math.hypot(x - PEN_ENTRY[0], y - PEN_ENTRY[1])