"""waypoint_sequencer.py — Pure-Python waypoint sequencing logic (Issue #599).

No ROS2 dependencies — importable and fully unit-testable without a live
ROS2 install.

Design
──────
WaypointSequencer tracks a list of (x, y, yaw_deg) waypoints and the index of
the currently active goal.  The ROS2 node (waypoint_follower_node.py) drives
this state machine by calling:

    sequencer.start(waypoints)          — load a new mission
    sequencer.advance()                 — mark current waypoint reached
    sequencer.abort(reason)             — mark mission failed
    sequencer.cancel()                  — mark mission cancelled

State transitions:
    IDLE → RUNNING  (on start())
    RUNNING → SUCCEEDED  (on advance() when last waypoint reached)
    RUNNING → ABORTED    (on abort())
    RUNNING → CANCELED   (on cancel())
    ABORTED / CANCELED / SUCCEEDED → any state via start()
"""

from __future__ import annotations

from dataclasses import dataclass
from typing import List, Optional, Tuple


# ── Waypoint data type ────────────────────────────────────────────────────────

@dataclass
class Waypoint:
    """A single navigation goal in the map frame."""
    x: float
    y: float
    yaw_deg: float = 0.0

    def __post_init__(self):
        if not (-360.0 <= self.yaw_deg <= 360.0):
            raise ValueError(
                f"yaw_deg must be in [-360, 360], got {self.yaw_deg}"
            )

    @classmethod
    def from_dict(cls, d: dict) -> "Waypoint":
        """Construct from a plain dict (e.g. parsed from JSON)."""
        return cls(
            x=float(d["x"]),
            y=float(d["y"]),
            yaw_deg=float(d.get("yaw_deg", 0.0)),
        )

    def to_dict(self) -> dict:
        return {"x": self.x, "y": self.y, "yaw_deg": self.yaw_deg}


# ── State machine ─────────────────────────────────────────────────────────────

class WaypointSequencer:
    """Tracks state for sequential waypoint execution.

    Thread-safety: external callers (ROS2 callbacks, action server) should
    hold their own lock when calling multiple methods atomically.
    """

    IDLE      = "idle"
    RUNNING   = "running"
    SUCCEEDED = "succeeded"
    ABORTED   = "aborted"
    CANCELED  = "canceled"

    def __init__(self) -> None:
        self._waypoints:  List[Waypoint] = []
        self._current:    int = 0
        self._state:      str = self.IDLE
        self._abort_reason: Optional[str] = None

    # ── Control methods ────────────────────────────────────────────────────

    def start(self, waypoints: List[Waypoint]) -> None:
        """Load a new mission and set state to RUNNING.

        Raises:
            ValueError: if waypoints is empty.
        """
        if not waypoints:
            raise ValueError("Waypoint list must not be empty")
        self._waypoints   = list(waypoints)
        self._current     = 0
        self._state       = self.RUNNING
        self._abort_reason = None

    def advance(self) -> None:
        """Mark the current waypoint as reached and move to the next.

        If the last waypoint is reached the state transitions to SUCCEEDED.

        Raises:
            RuntimeError: if not RUNNING.
        """
        if self._state != self.RUNNING:
            raise RuntimeError(
                f"advance() called in state {self._state!r} (must be RUNNING)"
            )
        self._current += 1
        if self._current >= len(self._waypoints):
            self._state = self.SUCCEEDED

    def abort(self, reason: str = "") -> None:
        """Mark the mission as aborted (e.g. Nav2 returned FAILED)."""
        self._state = self.ABORTED
        self._abort_reason = reason

    def cancel(self) -> None:
        """Mark the mission as cancelled (e.g. operator preempt)."""
        self._state = self.CANCELED

    # ── Query methods ──────────────────────────────────────────────────────

    @property
    def state(self) -> str:
        return self._state

    @property
    def is_running(self) -> bool:
        return self._state == self.RUNNING

    @property
    def is_done(self) -> bool:
        return self._state in (self.SUCCEEDED, self.ABORTED, self.CANCELED)

    @property
    def current_waypoint(self) -> Optional[Waypoint]:
        """The active goal, or None if not RUNNING or already done."""
        if self._state != self.RUNNING:
            return None
        if self._current >= len(self._waypoints):
            return None
        return self._waypoints[self._current]

    @property
    def current_index(self) -> int:
        """0-based index of the active waypoint (0 if not started)."""
        return self._current

    @property
    def total(self) -> int:
        """Total number of waypoints in the current mission."""
        return len(self._waypoints)

    @property
    def progress(self) -> Tuple[int, int]:
        """Return (completed_count, total_count)."""
        return self._current, len(self._waypoints)

    @property
    def remaining(self) -> int:
        return max(0, len(self._waypoints) - self._current)

    @property
    def abort_reason(self) -> Optional[str]:
        return self._abort_reason

    def status_dict(self) -> dict:
        """Serialisable status snapshot for MQTT / diagnostics."""
        wp = self.current_waypoint
        return {
            "state":         self._state,
            "current_index": self._current,
            "total":         len(self._waypoints),
            "remaining":     self.remaining,
            "current_wp":    wp.to_dict() if wp else None,
            "abort_reason":  self._abort_reason,
        }
