from __future__ import annotations

from dataclasses import dataclass
from pathlib import Path
from typing import Dict, List, Optional

import yaml

from .messages import Waypoint


@dataclass(frozen=True)
class WaypointRegistry:
    waypoints: Dict[str, Waypoint]

    def list(self) -> List[Waypoint]:
        return list(self.waypoints.values())

    def get(self, name: str) -> Optional[Waypoint]:
        return self.waypoints.get(name)


def load_waypoints(path: str) -> WaypointRegistry:
    p = Path(path)
    if not p.exists():
        return WaypointRegistry(waypoints={})

    payload = yaml.safe_load(p.read_text()) or {}
    items = payload.get("waypoints") or []

    wps: Dict[str, Waypoint] = {}
    for item in items:
        if not isinstance(item, dict):
            continue
        name = str(item.get("name", "")).strip()
        if not name:
            continue
        wps[name] = Waypoint(
            name=name,
            x=float(item.get("x", 0.0)),
            y=float(item.get("y", 0.0)),
            theta=float(item.get("theta", 0.0)),
        )

    return WaypointRegistry(waypoints=wps)