"""waypoint_teacher.py -- Record and replay waypoint routes."""

import json
import time
import math
from pathlib import Path
from dataclasses import dataclass, asdict


@dataclass
class Waypoint:
    x: float
    y: float
    z: float
    yaw_deg: float
    timestamp: float
    label: str = ''


class WaypointRoute:
    """A named sequence of waypoints."""

    def __init__(self, name: str):
        self.name = name
        self.waypoints: list[Waypoint] = []
        self.created_at = time.time()

    def add(self, x, y, z, yaw_deg, label=''):
        self.waypoints.append(Waypoint(x, y, z, yaw_deg, time.time(), label))

    def to_dict(self):
        return {
            'name': self.name,
            'created_at': self.created_at,
            'waypoints': [asdict(w) for w in self.waypoints],
        }

    @classmethod
    def from_dict(cls, d):
        route = cls(d['name'])
        route.created_at = d.get('created_at', 0)
        route.waypoints = [Waypoint(**w) for w in d['waypoints']]
        return route

    def save(self, routes_dir: str):
        Path(routes_dir).mkdir(parents=True, exist_ok=True)
        path = Path(routes_dir) / f'{self.name}.json'
        path.write_text(json.dumps(self.to_dict(), indent=2))

    @classmethod
    def load(cls, routes_dir: str, name: str):
        path = Path(routes_dir) / f'{name}.json'
        return cls.from_dict(json.loads(path.read_text()))

    @staticmethod
    def list_routes(routes_dir: str) -> list[str]:
        d = Path(routes_dir)
        if not d.exists():
            return []
        return [p.stem for p in d.glob('*.json')]


class WaypointReplayer:
    """Iterates through waypoints, returning next target."""

    def __init__(self, route: WaypointRoute, arrival_radius: float = 0.3):
        self._route = route
        self._idx = 0
        self._arrival_radius = arrival_radius

    def current_waypoint(self) -> Waypoint | None:
        if self._idx < len(self._route.waypoints):
            return self._route.waypoints[self._idx]
        return None

    def check_arrived(self, robot_x, robot_y) -> bool:
        wp = self.current_waypoint()
        if wp is None:
            return False
        dist = math.hypot(robot_x - wp.x, robot_y - wp.y)
        if dist < self._arrival_radius:
            self._idx += 1
            return True
        return False

    @property
    def is_done(self) -> bool:
        return self._idx >= len(self._route.waypoints)

    def reset(self):
        self._idx = 0
