"""
navlogger.py — Robot Tracker v009
Zeichnet alle Navigationsdaten auf.
Kompatibel mit speed_l/speed_r Befehlen (mm/s).
"""

import csv
import logging
import math
import time
from pathlib import Path

logger = logging.getLogger(__name__)


class NavLogger:
    def __init__(self, log_dir: str = "logs"):
        self.log_dir = Path(log_dir)
        self.log_dir.mkdir(exist_ok=True)
        self.file = None
        self.writer = None
        self.session_start = None
        self._open_new_file()

    def _open_new_file(self):
        ts = time.strftime("%Y%m%d_%H%M%S")
        path = self.log_dir / f"nav_{ts}.csv"
        self.file = open(path, "w", newline="")
        self.writer = csv.writer(self.file)
        self.writer.writerow([
            "timestamp",
            "elapsed_s",
            "robot_x_mm", "robot_y_mm", "robot_theta",
            "goal_x_mm", "goal_y_mm",
            "dist_to_goal_mm",
            "angle_error_deg",
            "cmd_speed_l", "cmd_speed_r", "cmd_status",
            "num_obstacles",
            "nearest_obstacle_id",
            "nearest_obstacle_dist_mm",
            "all_obstacle_dists",
        ])
        self.session_start = time.time()
        logger.info(f"NavLogger: Neue Log-Datei {path}")

    def log(
        self,
        robot_x: float, robot_y: float, robot_theta: float,
        goal_x: float, goal_y: float,
        cmd: dict,
        obstacles: list,
        angle_error: float,
    ):
        now = time.time()
        elapsed = round(now - self.session_start, 3)
        dist_goal = math.sqrt((goal_x - robot_x)**2 + (goal_y - robot_y)**2)

        nearest_id   = None
        nearest_dist = float("inf")
        obs_dists    = []

        for obs in obstacles:
            d = math.sqrt((robot_x - obs["x"])**2 + (robot_y - obs["y"])**2)
            obs_dists.append(f"{obs['id']}:{d:.0f}")
            if d < nearest_dist:
                nearest_dist = d
                nearest_id   = obs["id"]

        # Kompatibel mit speed_l/speed_r (v009) und left/right (v008)
        speed_l = cmd.get("speed_l", cmd.get("left", 0))
        speed_r = cmd.get("speed_r", cmd.get("right", 0))

        self.writer.writerow([
            round(now, 3),
            elapsed,
            round(robot_x, 1), round(robot_y, 1), round(robot_theta, 1),
            round(goal_x, 1), round(goal_y, 1),
            round(dist_goal, 1),
            round(angle_error, 1),
            round(speed_l, 1), round(speed_r, 1), cmd.get("status", ""),
            len(obstacles),
            nearest_id,
            round(nearest_dist, 1) if nearest_dist < float("inf") else "",
            " | ".join(obs_dists),
        ])
        self.file.flush()

    def close(self):
        if self.file:
            self.file.close()