"""Schemas for Factor Graph Optimizer (F10)."""

from datetime import datetime
from typing import List, Optional

import numpy as np
from pydantic import BaseModel


class Pose(BaseModel):
    """Optimized pose representation."""
    model_config = {"arbitrary_types_allowed": True}

    frame_id: int
    position: np.ndarray  # (3,)
    orientation: np.ndarray  # (3, 3) matrix or (4,) quaternion
    timestamp: datetime
    covariance: Optional[np.ndarray] = None  # (6, 6)


class OptimizationResult(BaseModel):
    """Result of GTSAM optimization step."""
    converged: bool
    final_error: float
    iterations_used: int
    optimized_frames: List[int]
    mean_reprojection_error: float


class FactorGraphConfig(BaseModel):
    """Configuration for GTSAM operations."""
    robust_kernel_type: str = "Huber"
    huber_threshold: float = 1.0  # pixels
    altitude_prior_noise: float = 5.0  # meters
    user_anchor_noise: float = 2.0  # meters
    litesam_anchor_noise: float = 20.0  # meters
