"""Webots R2025a adapter for the platform-neutral OpenLab controller."""

from __future__ import annotations

from pathlib import Path
import sys

PROJECT_ROOT = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(PROJECT_ROOT))

from controller_core import ControllerConfig, ControllerState, Measurements, step_controller  # noqa: E402


def normalized_obstacle_level(values: list[float]) -> float:
    """Map e-puck raw proximity response to 0..1; this is not a metric distance."""

    return max(0.0, min(1.0, max(values, default=0.0) / 4096.0))


def main() -> None:
    from controller import Robot

    robot = Robot()
    time_step_ms = int(robot.getBasicTimeStep())
    dt_s = time_step_ms / 1000.0

    left_motor = robot.getDevice("left wheel motor")
    right_motor = robot.getDevice("right wheel motor")
    left_motor.setPosition(float("inf"))
    right_motor.setPosition(float("inf"))
    left_motor.setVelocity(0.0)
    right_motor.setVelocity(0.0)

    sensors = [robot.getDevice(name) for name in ("ps0", "ps1", "ps6", "ps7")]
    for sensor in sensors:
        sensor.enable(time_step_ms)

    config = ControllerConfig()
    state = ControllerState()

    while robot.step(time_step_ms) != -1:
        output = step_controller(
            dt_s,
            Measurements(
                timestamp_s=robot.getTime(),
                obstacle_level=normalized_obstacle_level([sensor.getValue() for sensor in sensors]),
                heading_error_rad=0.0,
            ),
            state,
            config,
        )
        state = output.state
        left_motor.setVelocity(output.left_rad_s)
        right_motor.setVelocity(output.right_rad_s)


if __name__ == "__main__":
    main()
