"""Generate deterministic, non-Webots controller telemetry as CSV."""

from __future__ import annotations

import argparse
import csv
from pathlib import Path

from controller_core import (
    ControllerConfig,
    ControllerState,
    Measurements,
    Pose2D,
    integrate_pose,
    step_controller,
    wheel_targets_to_twist,
)


def obstacle_signal(step: int) -> float:
    if 20 <= step < 26:
        return 0.72
    if 26 <= step < 32:
        return 0.12
    return 0.05


def generate_rows(steps: int = 60, dt_s: float = 0.1) -> list[dict[str, str]]:
    config = ControllerConfig()
    state = ControllerState()
    pose = Pose2D()
    rows: list[dict[str, str]] = []

    for step in range(steps):
        timestamp_s = step * dt_s
        measurements = Measurements(
            timestamp_s=timestamp_s,
            obstacle_level=obstacle_signal(step),
            heading_error_rad=0.08 if step < 18 else 0.0,
        )
        output = step_controller(dt_s, measurements, state, config)
        linear, angular = wheel_targets_to_twist(
            output.left_rad_s,
            output.right_rad_s,
            wheel_radius_m=0.0205,
            wheelbase_m=0.052,
        )
        pose = integrate_pose(pose, linear, angular, dt_s)
        state = output.state
        rows.append(
            {
                "step": str(step),
                "time_s": f"{timestamp_s:.3f}",
                "obstacle_level": f"{measurements.obstacle_level:.3f}",
                "mode": state.mode.value,
                "reason": output.reason,
                "left_rad_s": f"{output.left_rad_s:.6f}",
                "right_rad_s": f"{output.right_rad_s:.6f}",
                "x_m": f"{pose.x_m:.6f}",
                "y_m": f"{pose.y_m:.6f}",
                "heading_rad": f"{pose.heading_rad:.6f}",
            }
        )
    return rows


def main() -> None:
    parser = argparse.ArgumentParser()
    parser.add_argument("--output", type=Path, default=Path("controller-scenarios.csv"))
    parser.add_argument("--steps", type=int, default=60)
    args = parser.parse_args()
    if args.steps <= 0:
        raise SystemExit("--steps must be positive")

    rows = generate_rows(args.steps)
    args.output.parent.mkdir(parents=True, exist_ok=True)
    with args.output.open("w", newline="", encoding="utf-8") as stream:
        writer = csv.DictWriter(stream, fieldnames=list(rows[0]))
        writer.writeheader()
        writer.writerows(rows)
    print(f"wrote {len(rows)} deterministic rows to {args.output}")


if __name__ == "__main__":
    main()
