# **************************************************************************************

# @package        satelles
# @license        MIT License Copyright (c) 2025 Michael J. Roberts

# **************************************************************************************

from datetime import datetime
from math import asin, atan2, cos, degrees, pi, pow, radians, sin, sqrt

from celerity.coordinates import EquatorialCoordinate, GeographicCoordinate
from celerity.temporal import get_greenwich_sidereal_time

from .common import CartesianCoordinate
from .earth import EARTH_EQUATORIAL_RADIUS, EARTH_FLATTENING_FACTOR
from .orbit import get_orbital_radius
from .vector import rotate

# **************************************************************************************


def get_perifocal_coordinate(
    semi_major_axis: float,
    mean_anomaly: float,
    true_anomaly: float,
    eccentricity: float,
) -> CartesianCoordinate:
    """
    Calculate the position in the perifocal coordinate system for a satellite.

    The perifocal coordinate system is a coordinate system that is centered on the
    focal point of the orbit, with the x-axis aligned with the periapsis direction.
    The y-axis is perpendicular to the x-axis in the orbital plane, and the z-axis
    is perpendicular to the orbital plane.

    Args:
        semi_major_axis: The semi-major axis (a) (in meters).
        mean_anomaly: The mean anomaly (M) (in degrees).
        true_anomaly: The true anomaly (ν) (in degrees).
        eccentricity: The orbital eccentricity (e), (unitless).

    Returns:
        CartesianCoordinate: The position in the perifocal coordinate system (x, y, z).
    """
    # Calculate the orbital radius (r) for the body:
    r = get_orbital_radius(
        semi_major_axis=semi_major_axis,
        mean_anomaly=mean_anomaly,
        eccentricity=eccentricity,
    )

    x_perifocal = r * cos(radians(true_anomaly))
    y_perifocal = r * sin(radians(true_anomaly))

    # The z-coordinate is always zero in the perifocal frame:
    return CartesianCoordinate(x=x_perifocal, y=y_perifocal, z=0.0)


# **************************************************************************************


def convert_perifocal_to_eci(
    perifocal: CartesianCoordinate,
    argument_of_perigee: float,
    inclination: float,
    raan: float,
) -> CartesianCoordinate:
    """
    Convert perifocal coordinates to Earth-Centered Inertial (ECI) coordinates.

    Args:
        perifocal (CartesianCoordinate): The perifocal coordinates (x, y, z).
        argument_of_perigee (float): The argument of perigee (ω) (in degrees).
        inclination (float): The inclination (i) (in degrees).
        raan (float): The right ascension of ascending node (Ω) (in degrees).

    Returns:
        CartesianCoordinate: The ECI coordinates (x, y, z).
    """
    # Rotate by argument of perigee around the z-axis:
    rotated_z = rotate(perifocal, argument_of_perigee, "z")

    # Rotate by inclination around the x-axis:
    rotated_x = rotate(rotated_z, inclination, "x")

    # Rotate by Right Ascension of Ascending Node (RAAN) around the z-axis:
    eci = rotate(rotated_x, raan, "z")

    # The ECI coordinates are now in the rotated frame:
    return eci


# **************************************************************************************


def convert_eci_to_ecef(
    eci: CartesianCoordinate,
    when: datetime,
) -> CartesianCoordinate:
    """
    Convert Earth-Centered Inertial (ECI) coordinates to Earth-Centered Earth Fixed (ECEF)
    coordinates.

    Args:
        eci (CartesianCoordinate): The ECI coordinates (x, y, z).
        when (datetime): The date and time for the conversion.

    Returns:
        CartesianCoordinate: The ECEF coordinates (x, y, z).
    """
    # Get the Greenwich Mean Sidereal Time (GMST) for the given date:
    GMST = get_greenwich_sidereal_time(date=when)

    # Rotate around Z-axis (from ECI to ECEF) using the GMST:
    return CartesianCoordinate(
        x=(eci["x"] * cos(radians(GMST))) + (eci["y"] * sin(radians(GMST))),
        y=(eci["x"] * -sin(radians(GMST))) + (eci["y"] * cos(radians(GMST))),
        z=eci["z"],
    )


# **************************************************************************************


def convert_eci_to_equatorial(
    eci: CartesianCoordinate,
) -> EquatorialCoordinate:
    """
    Convert ECI coordinates to equatorial coordinates.

    Args:
        eci (CartesianCoordinate): The ECI coordinates (x, y, z).

    Raises:
        ValueError: If the ECI coordinates are a zero vector.

    Returns:
        EquatorialCoordinate: The equatorial coordinates (RA, Dec).
    """
    x, y, z = eci["x"], eci["y"], eci["z"]

    r = sqrt(x**2 + y**2 + z**2)

    if r == 0:
        raise ValueError("Cannot convert zero vector to equatorial coordinates.")

    ra = degrees(atan2(y, x))

    dec = degrees(asin(z / r))

    if ra < 0:
        ra += 360

    return EquatorialCoordinate(ra=ra % 360, dec=dec)


# **************************************************************************************


def convert_lla_to_ecef(
    lla: GeographicCoordinate,
) -> CartesianCoordinate:
    """
    Convert geographic coordinates (latitude, longitude, height) to ECEF coordinates.

    Args:
        lla (GeographicCoordinate): The geographic coordinates (lat, lon, el).

    Returns:
        CartesianCoordinate: The ECEF coordinates (x, y, z).

    Raises:
        ValueError: If the flattening factor is not between 0 and 1.
        ValueError: If the latitude is not between -90 and 90 degrees.
        ValueError: If the longitude is not between -180 and 180 degrees.
    """
    earth_radius = EARTH_EQUATORIAL_RADIUS

    # Convert the latitude to radians:
    φ = radians(lla["lat"])

    # Convert the longitude to radians:
    θ = radians(lla["lon"])

    # The height (el) is the height above the ellipsoid (e.g., the elevation):
    h = lla["el"]

    # The flattening factor (f) is the ratio of the difference between the equatorial
    # and polar radii to the equatorial radius, in the WGS-84 ellipsoid model:
    f = EARTH_FLATTENING_FACTOR

    # Ensure that the latitude (φ) is between -π/2 and π/2 radians:
    if abs(φ) > (pi / 2):
        raise ValueError("Latitude must be between -90 and 90 degrees.")

    # Ensure that the longitude (θ) is between -π and π radians:
    if abs(θ) > pi:
        raise ValueError("Longitude must be between -180 and 180 degrees.")

    # Ensure that f is between 0 and 1:
    if f < 0 or f > 1:
        raise ValueError("Earth flattening factor must be between 0 and 1.")

    # The square of the flattening factor (FF) is used to calculate the radius of
    # curvature in the prime vertical (N):
    FF = f * (2 - f)

    # The radius of curvature in the prime vertical (N) is the Earth’s east–west
    # curvature radius at the given latitude, accounting for ellipsoidal flattening:
    C = 1 / sqrt(1 - (FF * pow(sin(φ), 2)))

    # The radius of curvature in the meridian (S) is the Earth’s north–south curvature
    # radius at the given latitude, scaled by the polar flattening factor:
    S = C * FF

    # The radius of curvature in the prime vertical (N):
    N = earth_radius * C

    # Compute the x coordinate in the ECEF frame:
    x = (N + h) * cos(φ) * cos(θ)

    # Compute the y coordinate in the ECEF frame:
    y = (N + h) * cos(φ) * sin(θ)

    # Compute the z coordinate in the ECEF frame:
    z = (earth_radius * S + h) * sin(φ)

    return CartesianCoordinate(
        x=x,
        y=y,
        z=z,
    )


# **************************************************************************************
