"""Synthetic dataset — straight-line eastward flight at constant altitude.

Used for harness self-tests. No image files; image_path is empty so the harness
can substitute a generated checkerboard so existing OpenCV readers can open them.
"""

from __future__ import annotations

from typing import Iterator

import numpy as np

from gps_denied.testing.datasets.base import (
    DatasetAdapter,
    DatasetCapabilities,
    DatasetFrame,
    DatasetIMU,
    DatasetPose,
    PlatformClass,
)

GRAVITY = 9.81
EARTH_R = 6_378_137.0  # WGS84 semi-major axis, metres


class SyntheticAdapter(DatasetAdapter):
    """Deterministic straight-line eastward flight for harness smoke-tests."""

    def __init__(
        self,
        num_frames: int = 100,
        fps: float = 5.0,
        imu_rate_hz: float = 100.0,
        speed_m_s: float = 10.0,
        altitude_m: float = 100.0,
        origin_lat: float = 49.0,
        origin_lon: float = 32.0,
    ) -> None:
        self._num_frames = num_frames
        self._fps = fps
        self._imu_rate_hz = imu_rate_hz
        self._speed = speed_m_s
        self._altitude = altitude_m
        self._origin_lat = origin_lat
        self._origin_lon = origin_lon

    @property
    def name(self) -> str:
        return "synthetic"

    @property
    def capabilities(self) -> DatasetCapabilities:
        return DatasetCapabilities(
            has_raw_imu=True,
            has_rtk_gt=True,
            has_loop_closures=False,
            platform_class=PlatformClass.SYNTHETIC,
        )

    def iter_frames(self) -> Iterator[DatasetFrame]:
        period_ns = int(1e9 / self._fps)
        for i in range(self._num_frames):
            yield DatasetFrame(
                frame_idx=i,
                timestamp_ns=i * period_ns,
                image_path="",  # harness fills from generator
            )

    def iter_imu(self) -> Iterator[DatasetIMU]:
        duration_s = self._num_frames / self._fps
        n_samples = int(duration_s * self._imu_rate_hz)
        period_ns = int(1e9 / self._imu_rate_hz)
        for i in range(n_samples):
            yield DatasetIMU(
                timestamp_ns=i * period_ns,
                accel=(0.0, 0.0, -GRAVITY),  # gravity only; constant velocity
                gyro=(0.0, 0.0, 0.0),
            )

    def iter_ground_truth(self) -> Iterator[DatasetPose]:
        period_ns = int(1e9 / self._fps)
        for i in range(self._num_frames):
            t_s = i / self._fps
            east_m = self._speed * t_s
            # Local tangent plane approximation (small distance)
            dlon = np.degrees(east_m / (EARTH_R * np.cos(np.radians(self._origin_lat))))
            yield DatasetPose(
                timestamp_ns=i * period_ns,
                lat=self._origin_lat,
                lon=self._origin_lon + dlon,
                alt=self._altitude,
                qx=0.0, qy=0.0, qz=0.0, qw=1.0,
            )
