"""Platform-neutral control and kinematics for the DIS OpenLab starter rover.

This module deliberately imports no Webots, MicroPython, GPIO, or board APIs.
Adapters must translate their platform's measurements into the normalized
Measurements contract and safely apply the returned wheel targets.
"""

from __future__ import annotations

from dataclasses import dataclass, replace
from enum import Enum
import math


class Mode(str, Enum):
    STOPPED = "STOPPED"
    FORWARD = "FORWARD"
    TURNING = "TURNING"


@dataclass(frozen=True)
class ControllerConfig:
    max_wheel_speed_rad_s: float = 6.28
    cruise_speed_rad_s: float = 3.0
    turn_speed_rad_s: float = 2.2
    obstacle_enter: float = 0.35
    obstacle_exit: float = 0.20
    turn_hold_s: float = 0.55
    heading_kp: float = 1.1
    heading_ki: float = 0.15
    integral_limit: float = 1.0
    watchdog_s: float = 0.25


@dataclass(frozen=True)
class Measurements:
    timestamp_s: float
    obstacle_level: float
    heading_error_rad: float = 0.0
    enabled: bool = True


@dataclass(frozen=True)
class ControllerState:
    mode: Mode = Mode.STOPPED
    turn_until_s: float = 0.0
    heading_integral: float = 0.0


@dataclass(frozen=True)
class WheelTargets:
    left_rad_s: float
    right_rad_s: float
    state: ControllerState
    reason: str


@dataclass(frozen=True)
class Pose2D:
    x_m: float = 0.0
    y_m: float = 0.0
    heading_rad: float = 0.0


def _finite(*values: float) -> bool:
    return all(math.isfinite(value) for value in values)


def clamp(value: float, lower: float, upper: float) -> float:
    if lower > upper:
        raise ValueError("lower bound must not exceed upper bound")
    return max(lower, min(upper, value))


def stopped(state: ControllerState, reason: str) -> WheelTargets:
    return WheelTargets(0.0, 0.0, replace(state, mode=Mode.STOPPED), reason)


def validate_config(config: ControllerConfig) -> None:
    if not _finite(
        config.max_wheel_speed_rad_s,
        config.cruise_speed_rad_s,
        config.turn_speed_rad_s,
        config.obstacle_enter,
        config.obstacle_exit,
        config.turn_hold_s,
        config.heading_kp,
        config.heading_ki,
        config.integral_limit,
        config.watchdog_s,
    ):
        raise ValueError("controller configuration must be finite")
    if config.max_wheel_speed_rad_s <= 0 or config.watchdog_s <= 0:
        raise ValueError("speed limit and watchdog must be positive")
    if not 0 <= config.obstacle_exit < config.obstacle_enter <= 1:
        raise ValueError("obstacle thresholds must satisfy 0 <= exit < enter <= 1")
    if config.cruise_speed_rad_s < 0 or config.turn_speed_rad_s < 0:
        raise ValueError("commanded speeds must be non-negative")
    if max(config.cruise_speed_rad_s, config.turn_speed_rad_s) > config.max_wheel_speed_rad_s:
        raise ValueError("cruise and turn speeds must not exceed the wheel limit")


def step_controller(
    dt_s: float,
    measurements: Measurements,
    state: ControllerState,
    config: ControllerConfig = ControllerConfig(),
) -> WheelTargets:
    """Return bounded wheel targets and next immutable state.

    ``obstacle_level`` is a normalized adapter signal: 0 means clear and 1
    means the strongest/nearest detected obstacle. It is not a metric range.
    Invalid, stale, disabled, or non-finite inputs produce a zero command.
    """

    validate_config(config)
    if not _finite(dt_s, measurements.timestamp_s, measurements.obstacle_level, measurements.heading_error_rad):
        return stopped(state, "INVALID_MEASUREMENT")
    if not measurements.enabled:
        return stopped(state, "DISABLED")
    if dt_s <= 0 or dt_s > config.watchdog_s:
        return stopped(state, "WATCHDOG")

    obstacle = clamp(measurements.obstacle_level, 0.0, 1.0)
    should_enter_turn = obstacle >= config.obstacle_enter
    should_hold_turn = state.mode == Mode.TURNING and (
        measurements.timestamp_s < state.turn_until_s or obstacle > config.obstacle_exit
    )

    if should_enter_turn or should_hold_turn:
        turn_until = max(state.turn_until_s, measurements.timestamp_s + config.turn_hold_s) if should_enter_turn else state.turn_until_s
        next_state = ControllerState(
            mode=Mode.TURNING,
            turn_until_s=turn_until,
            heading_integral=0.0,
        )
        return WheelTargets(
            left_rad_s=-config.turn_speed_rad_s,
            right_rad_s=config.turn_speed_rad_s,
            state=next_state,
            reason="OBSTACLE_AVOIDANCE",
        )

    integral = clamp(
        state.heading_integral + measurements.heading_error_rad * dt_s,
        -config.integral_limit,
        config.integral_limit,
    )
    correction = config.heading_kp * measurements.heading_error_rad + config.heading_ki * integral
    left = clamp(config.cruise_speed_rad_s - correction, -config.max_wheel_speed_rad_s, config.max_wheel_speed_rad_s)
    right = clamp(config.cruise_speed_rad_s + correction, -config.max_wheel_speed_rad_s, config.max_wheel_speed_rad_s)
    next_state = ControllerState(mode=Mode.FORWARD, heading_integral=integral)
    return WheelTargets(left, right, next_state, "TRACK_HEADING")


def wheel_targets_to_twist(
    left_rad_s: float,
    right_rad_s: float,
    wheel_radius_m: float,
    wheelbase_m: float,
) -> tuple[float, float]:
    """Convert wheel angular rates to body linear and angular velocity."""

    if not _finite(left_rad_s, right_rad_s, wheel_radius_m, wheelbase_m):
        raise ValueError("kinematic inputs must be finite")
    if wheel_radius_m <= 0 or wheelbase_m <= 0:
        raise ValueError("wheel radius and wheelbase must be positive")
    linear_m_s = wheel_radius_m * (left_rad_s + right_rad_s) / 2.0
    angular_rad_s = wheel_radius_m * (right_rad_s - left_rad_s) / wheelbase_m
    return linear_m_s, angular_rad_s


def integrate_pose(pose: Pose2D, linear_m_s: float, angular_rad_s: float, dt_s: float) -> Pose2D:
    """Deterministic midpoint integration for a planar rover."""

    if not _finite(pose.x_m, pose.y_m, pose.heading_rad, linear_m_s, angular_rad_s, dt_s):
        raise ValueError("pose integration inputs must be finite")
    if dt_s <= 0:
        raise ValueError("dt_s must be positive")
    midpoint_heading = pose.heading_rad + angular_rad_s * dt_s / 2.0
    next_heading = math.atan2(
        math.sin(pose.heading_rad + angular_rad_s * dt_s),
        math.cos(pose.heading_rad + angular_rad_s * dt_s),
    )
    return Pose2D(
        x_m=pose.x_m + linear_m_s * math.cos(midpoint_heading) * dt_s,
        y_m=pose.y_m + linear_m_s * math.sin(midpoint_heading) * dt_s,
        heading_rad=next_heading,
    )
