"""Error-State Kalman Filter schemas."""

from enum import Enum
from typing import Optional

import numpy as np
from pydantic import BaseModel


class ConfidenceTier(str, Enum):
    """ESKF confidence tier for GPS_INPUT fix_type mapping."""
    HIGH = "HIGH"       # Satellite match <30s ago, covariance < 400 m^2
    MEDIUM = "MEDIUM"   # VO tracking OK, no recent satellite match
    LOW = "LOW"         # IMU-only, cuVSLAM lost
    FAILED = "FAILED"   # 3+ consecutive total failures


class IMUMeasurement(BaseModel):
    """Single IMU reading from flight controller."""
    model_config = {"arbitrary_types_allowed": True}

    accel: np.ndarray  # (3,) m/s^2 in body frame
    gyro: np.ndarray   # (3,) rad/s in body frame
    timestamp: float   # seconds since epoch


class ESKFConfig(BaseModel):
    """ESKF tuning parameters."""

    # Process noise (from IMU datasheet — ICM-42688-P)
    accel_noise_density: float = 6.86e-4   # 70 ug/sqrt(Hz) -> m/s^2/sqrt(Hz)
    gyro_noise_density: float = 5.24e-5    # 3.0e-3 deg/s/sqrt(Hz) -> rad/s/sqrt(Hz)
    accel_random_walk: float = 2.0e-3      # m/s^3/sqrt(Hz)
    gyro_random_walk: float = 8.73e-7      # 5.0e-5 deg/s^2/sqrt(Hz) -> rad/s^2/sqrt(Hz)

    # VO measurement noise
    vo_position_noise: float = 0.3         # meters (cuVSLAM at 600m altitude)

    # Satellite measurement noise range
    sat_noise_min: float = 5.0             # meters (high-confidence RANSAC)
    sat_noise_max: float = 20.0            # meters (low-confidence RANSAC)

    # Confidence tier thresholds
    satellite_max_age: float = 30.0        # seconds
    covariance_high_threshold: float = 400.0  # m^2 (trace of position covariance)

    # Initial covariance diagonals
    init_pos_var: float = 100.0            # m^2
    init_vel_var: float = 1.0              # (m/s)^2
    init_att_var: float = 0.01             # rad^2
    init_accel_bias_var: float = 0.01      # (m/s^2)^2
    init_gyro_bias_var: float = 1e-6       # (rad/s)^2

    # Mahalanobis outlier rejection (chi-squared threshold for 3-DOF at 5-sigma)
    mahalanobis_threshold: float = 16.27   # chi2(3, 0.99999) ≈ 5-sigma gate


class ESKFState(BaseModel):
    """Full ESKF nominal state snapshot."""
    model_config = {"arbitrary_types_allowed": True}

    position: np.ndarray       # (3,) ENU meters from origin (East, North, Up)
    velocity: np.ndarray       # (3,) ENU m/s
    quaternion: np.ndarray     # (4,) [w, x, y, z] body-to-ENU
    accel_bias: np.ndarray     # (3,) m/s^2
    gyro_bias: np.ndarray      # (3,) rad/s
    covariance: np.ndarray     # (15, 15)
    timestamp: float           # seconds since epoch
    confidence: ConfidenceTier
    last_satellite_time: Optional[float] = None
    last_vo_time: Optional[float] = None