"""Shared utility functions for geolocation solvers."""

from __future__ import annotations

from typing import TYPE_CHECKING

import numpy as np

from gri_utils.constants import ER_M, PR_M

if TYPE_CHECKING:
    from numpy.typing import NDArray


def default_initial_guess(
    collector_xyz: NDArray[np.floating],
) -> NDArray[np.floating]:
    """Compute default initial guess from collector positions.

    Computes the centroid of all collector positions and projects it to
    the mean Earth radius along the radial direction from Earth center.

    This provides a reasonable starting point for iterative geolocation
    solvers when no prior target position estimate is available.

    Args:
        collector_xyz: Collector positions in ECEF meters. Shape (N, 3)
            for N collectors, or (3,) for a single collector.

    Returns:
        Initial guess position in ECEF meters. Shape (3,).
    """
    collector_xyz = np.atleast_2d(collector_xyz)
    centroid = np.mean(collector_xyz, axis=0)
    direction = centroid / np.linalg.norm(centroid)
    mean_earth_radius = (2 * ER_M + PR_M) / 3
    return direction * mean_earth_radius
