sguaba 0.10.3

use crate::float_math::FloatMath;
use crate::{Coordinate, Point3, systems::Ecef, util::BoundedAngle};
use core::fmt;
use core::fmt::Display;
use core::marker::PhantomData;
use uom::si::f64::{Angle, Length};
use uom::si::{
    angle::{degree, radian},
    length::meter,
};

#[cfg(any(test, feature = "approx"))]
use approx::{AbsDiffEq, RelativeEq};
use core::f64::consts::FRAC_PI_2;
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};
use uom::ConstZero;

// Parameters required for WGS84 ellipsoid
// https://nsgreg.nga.mil/doc/view?i=4085 table 3.1
#[doc(alias = "equatorial radius")]
#[doc(alias = "a")]
pub(crate) const SEMI_MAJOR_AXIS: f64 = 6_378_137.0;
#[doc(alias = "1/f")]
const FLATTENING_FACTOR: f64 = 298.257_223_563;
#[doc(alias = "f")]
const FLATTENING: f64 = 1.0 / FLATTENING_FACTOR;
#[doc(alias = "polar radius")]
#[doc(alias = "b")]
// b/a = 1 - f
// b = a * (1 - f)
//   = a - af
const SEMI_MINOR_AXIS: f64 = SEMI_MAJOR_AXIS * (1.0 - FLATTENING);
#[doc(alias = "e^2")]
// e^2 = 1 - b^2/a^2
//     = 1 - (a - af)^2 / a^2
//     = 1 - (a^2 - 2 * a * af + a^2 f^2) / a^2
//     = 1 - (1 - 2 * f + f^2)
//     = 1 - 1 + 2 * f - f^2
//     = 2 * f - f^2
const ECCENTRICITY_SQ: f64 = 2.0 * FLATTENING - FLATTENING * FLATTENING;
// l = a^2 * e^4
#[doc(alias = "l")]
const L: f64 = (SEMI_MAJOR_AXIS * SEMI_MAJOR_AXIS) * (ECCENTRICITY_SQ * ECCENTRICITY_SQ);
#[doc(alias = "a^2")]
const SEMI_MAJOR_AXIS_SQ: f64 = SEMI_MAJOR_AXIS * SEMI_MAJOR_AXIS;
#[doc(alias = "1 - e^2")]
const SQUARED_AXIS_RATIO: f64 = 1. - ECCENTRICITY_SQ;

// Altitude range for which we guarantee Coordinate<Ecef>::to_wgs84_fast will work.
const ECEF_TO_WGS84_FAST_MIN_ALTITUDE_M: f64 = -10_000.0;
const ECEF_TO_WGS84_FAST_MAX_ALTITUDE_M: f64 = 50_000.0;

const ECEF_TO_WGS84_FAST_MIN_GEO_CENTER_DISTANCE_M_SQ: f64 = (SEMI_MINOR_AXIS
    + ECEF_TO_WGS84_FAST_MIN_ALTITUDE_M)
    * (SEMI_MINOR_AXIS + ECEF_TO_WGS84_FAST_MIN_ALTITUDE_M);
const ECEF_TO_WGS84_FAST_MAX_GEO_CENTER_DISTANCE_M_SQ: f64 = (SEMI_MAJOR_AXIS
    + ECEF_TO_WGS84_FAST_MAX_ALTITUDE_M)
    * (SEMI_MAJOR_AXIS + ECEF_TO_WGS84_FAST_MAX_ALTITUDE_M);

// Altitude range for which we guarantee From<Ecef> for Wgs84.
const ECEF_TO_WGS84_MIN_ALTITUDE_M: f64 = -6.3e6;
const ECEF_TO_WGS84_MAX_ALTITUDE_M: f64 = 10e10;

const ECEF_TO_WGS84_MIN_GEO_CENTER_DISTANCE_M_SQ: f64 = (SEMI_MINOR_AXIS
    + ECEF_TO_WGS84_MIN_ALTITUDE_M)
    * (SEMI_MINOR_AXIS + ECEF_TO_WGS84_MIN_ALTITUDE_M);
const ECEF_TO_WGS84_MAX_GEO_CENTER_DISTANCE_M_SQ: f64 = (SEMI_MAJOR_AXIS
    + ECEF_TO_WGS84_MAX_ALTITUDE_M)
    * (SEMI_MAJOR_AXIS + ECEF_TO_WGS84_MAX_ALTITUDE_M);

// Iteration limit used in `to_wgs84()` as recommended in section 5.2 of
// the paper:
//   An iterative algorithm to compute geodetic coordinates
//   Chanfang Shu a,b,n, Fei Li
//   https://www.sciencedirect.com/science/article/pii/S0098300410001238
const GEODETIC_ITER_LIMIT: usize = 20;

/// Representing an Earth-bound location using the [World Geodetic System
/// '84](https://en.wikipedia.org/wiki/World_Geodetic_System#WGS_84).
///
/// Note that across longer time scales, WGS84 drifts, which you can read more about in the
/// crate-level docs on temporal drift.
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct Wgs84 {
    // NOTE(jon): note that uom does not guarantee how these angles are normalized -- they might
    // be [-180,180) or [0,360), or something else altogether. we do not normalize them ourselves
    // because callers will generally not care (they're more likely to feed the value into some
    // other formula that also doesn't care.
    pub(crate) latitude: Angle,
    pub(crate) longitude: Angle,
    altitude: Length,
}

impl Wgs84 {
    /// Constructs a world location from latitude, longitude, and altitude.
    ///
    /// The latitude must be in [-90°,90°] % 360°. If it is not, this function returns `None`.
    ///
    /// The altitude is measured as distance above the WGS84 datum reference ellipsoid.
    #[must_use]
    pub fn build(
        Components {
            latitude,
            longitude,
            altitude,
        }: Components,
    ) -> Option<Self> {
        Some(
            Self::builder()
                .latitude(latitude)?
                .longitude(longitude)
                .altitude(altitude)
                .build(),
        )
    }

    /// Provides a constructor for a [`Wgs84`] coordinate.
    pub fn builder() -> Builder<MissingLatitude, MissingLongitude, MissingAltitude> {
        Builder {
            under_construction: Self {
                latitude: Angle::ZERO,
                longitude: Angle::ZERO,
                altitude: Length::ZERO,
            },
            has: (PhantomData, PhantomData, PhantomData),
        }
    }

    /// Turns a [`Wgs84`] coordinate back into a builder so that its components can be modified.
    pub fn to_builder(self) -> Builder<HasLatitude, HasLongitude, HasAltitude> {
        Builder {
            under_construction: Self {
                latitude: self.latitude,
                longitude: self.longitude,
                altitude: self.altitude,
            },
            has: (PhantomData, PhantomData, PhantomData),
        }
    }

    /// Constructs a world location from latitude, longitude, and altitude.
    ///
    /// Prefer [`Wgs84::build`] or [`Wgs84::builder`] to avoid risk of argument order confusion.
    /// This function will be removed in a future version of Sguaba in favor of those.
    ///
    /// The latitude must be in [-90°,90°] % 360°. If it is not, this function returns `None`.
    ///
    /// The altitude is measured as distance above the WGS84 datum reference ellipsoid.
    #[must_use]
    #[deprecated = "prefer `Wgs84::build` or `Wgs84::builder` to avoid risk of argument order confusion"]
    pub fn new(
        latitude: impl Into<Angle>,
        longitude: impl Into<Angle>,
        altitude: impl Into<Length>,
    ) -> Option<Self> {
        Self::build(Components {
            latitude: latitude.into(),
            longitude: longitude.into(),
            altitude: altitude.into(),
        })
    }

    /// Computes the [great-circle distance] between the two locations on the surface of
    /// the earth.
    ///
    /// Note that this is an approximation as the earth is not a perfect sphere.
    ///
    /// The current implementation computes this [using the archaversine] (inverse haversine).
    ///
    /// [great-circle distance]: https://en.wikipedia.org/wiki/Great-circle_distance
    /// [using the archaversine]: https://en.wikipedia.org/wiki/Haversine_formula#Formulation
    #[doc(alias = "great_circle_distance")]
    #[must_use]
    pub fn haversine_distance_on_surface(&self, other: &Self) -> Length {
        let haversine = central_angle_by_inverse_haversine(
            self.latitude,
            other.latitude,
            self.longitude,
            other.longitude,
        );

        haversine * Length::new::<meter>(SEMI_MAJOR_AXIS)
    }

    /// Returns the number of degrees longitude north of the equator ("northing").
    ///
    /// The returned value is always in [-90, 90) ± N × 360°.
    #[must_use]
    pub fn latitude(&self) -> Angle {
        Angle::new::<radian>(BoundedAngle::new(self.latitude).to_signed_range())
    }

    /// Returns the number of degrees longitude east of the [IERS Reference Meridian] near
    /// Greenwich ("easting").
    ///
    /// There is no guarantee on the numeric range of the longitudinal angle.
    ///
    /// [IERS Reference Meridian]: https://en.wikipedia.org/wiki/IERS_Reference_Meridian
    #[must_use]
    pub fn longitude(&self) -> Angle {
        Angle::new::<radian>(BoundedAngle::new(self.longitude).to_signed_range())
    }

    /// Returns the distance in meters beyond the WGS84 vertical datum, ie the WGS84 ellipsoid.
    ///
    /// Note that the WGS84 ellipsoid is an approximation and does not perfectly align with ground
    /// level. Thus, while this is similar to altitude above sea/ground level, it is not equal to
    /// either of those measures.
    #[must_use]
    pub fn altitude(&self) -> Length {
        self.altitude
    }
}

impl Display for Wgs84 {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let lat = self.latitude();
        let lat_is_positive = lat.is_sign_positive();
        let lat = lat.abs().get::<degree>();
        let lon = self.longitude();
        let lon_is_positive = lon.is_sign_positive();
        let lon = lon.abs().get::<degree>();
        let alt = self.altitude.get::<meter>();
        match (lat_is_positive, lon_is_positive) {
            (true, true) => write!(f, "{lat}°N, {lon}°E, {alt}m"),
            (true, false) => write!(f, "{lat}°N, {lon}°W, {alt}m"),
            (false, true) => write!(f, "{lat}°S, {lon}°E, {alt}m"),
            (false, false) => write!(f, "{lat}°S, {lon}°W, {alt}m"),
        }
    }
}

impl Coordinate<Ecef> {
    /// Converts latitude, longitude, and altitude to the Earth-Centered, Earth-Fixed coordinate
    /// system.
    ///
    /// See:
    /// <https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates>
    #[must_use]
    pub fn from_wgs84(wgs84: &Wgs84) -> Self {
        // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
        let height_h = wgs84.altitude.get::<meter>();
        let lon_lambda = wgs84.longitude.get::<radian>();
        let lat_phi = wgs84.latitude.get::<radian>();

        // https://en.wikipedia.org/wiki/Earth_radius#Prime_vertical
        let cot_2_phi = 1. / lat_phi.tan().powi(2);
        let n_phi = SEMI_MAJOR_AXIS / (1. - ECCENTRICITY_SQ / (1. + cot_2_phi)).sqrt();

        let x = (n_phi + height_h) * lat_phi.cos() * lon_lambda.cos();
        let y = (n_phi + height_h) * lat_phi.cos() * lon_lambda.sin();
        let z = ((1. - ECCENTRICITY_SQ) * n_phi + height_h) * lat_phi.sin();

        Self::from_nalgebra_point(Point3::new(x, y, z))
    }

    /// Converts an Earth-Centered, Earth-Fixed coordinate into latitude, longitude, and altitude.
    ///
    /// Note that this conversion is not trivial and needs to be approximated.
    ///
    /// The method is usable over geodetic heights from -6.33×10⁶m to 10¹⁰m.
    ///
    /// This implementation currently uses different solutions depending on the geodetic height of
    /// the chosen point. It uses [`to_wgs84_fast`][Self::to_wgs84_fast] for points near the
    /// surface of the Earth and the slower [`to_wgs84_extended`][Self::to_wgs84_extended] for all
    /// other points, but this may change in the future.
    ///
    /// Since this implementation branches depending on the geodetic height of `self`, you may want
    /// to call [`to_wgs84_fast`][Self::to_wgs84_fast] directly for maximal performance if you know
    /// you will never exceed its supported range.
    #[must_use]
    pub fn to_wgs84(&self) -> Wgs84 {
        if self.is_in_fast_wgs84_range() {
            self.to_wgs84_fast_inner()
        } else {
            self.to_wgs84_extended()
        }
    }

    /// Converts an Earth-Centered, Earth-Fixed coordinate into latitude, longitude, and altitude.
    ///
    /// Note that this conversion is not trivial and needs to be approximated.
    ///
    /// The implementation currently only guarantees conversion to WGS84 datums with altitude
    /// between -10km and 50km from the surface of the WGS84 ellipsoid, roughly corresponding
    /// to the bottom of the Mariana Trench to the top of the stratosphere. However, it is also a
    /// decent amount faster than [`to_wgs84`][Self::to_wgs84]. Outside this range, the
    /// implementation may panic. If a wider altitude range is required, prefer
    /// [`to_wgs84`][Self::to_wgs84].
    ///
    /// This implementation currently uses [Ferrari's solution][ferrari], but this may change
    /// in the future.
    ///
    /// [ferrari]: https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution
    #[must_use]
    pub fn to_wgs84_fast(&self) -> Wgs84 {
        #[cfg(any(debug_assertions, test))]
        {
            if !self.is_in_fast_wgs84_range() {
                panic!(
                    "fast conversion from ECEF to WGS84 outside altitude range \
            {ECEF_TO_WGS84_FAST_MIN_ALTITUDE_M}..{ECEF_TO_WGS84_FAST_MAX_ALTITUDE_M} \
            is not supported: {self}"
                )
            }
        }
        self.to_wgs84_fast_inner()
    }

    fn to_wgs84_fast_inner(self) -> Wgs84 {
        let lon = FloatMath::atan2(self.point.y, self.point.x);

        // interestingly, there is no single way to convert from ECEF to WGS84.
        // however, there are a number of algorithms, some closed-form (non-iterative) and some
        // iterative/approximating. while it's tempting to use a closed-form version that doesn't
        // need any looping, like the original one in
        // <https://link.springer.com/article/10.1007/BF02520228>, in practice those algorithms
        // appear to have worse boundary conditions _and_ be slower, as shown in
        // <https://link.springer.com/article/10.1007/s001900050271>.
        //
        // there _is_ an iterative algorithm outlined on Wikipedia -- the Newton-Raphson method --
        // but interestingly after implementing it as presented I observed that it produces
        // slightly-wrong latitudes and very-wrong altitudes for a number of inputs. I then
        // replicating the Fukushima implementation (second paper above), but it produces
        // similarly-wrong results for points with a negative latitude. it also requires some
        // boundary post-processing to ensure it produces latitude in [-90,90] and altitudes near
        // the correct "side" of the earth (it will sometimes produces altitudes that are
        // `-ellipsoid_diameter-h`).
        //
        // I eventually found the more recent paper
        //
        //   An iterative algorithm to compute geodetic coordinates
        //   Chanfang Shu a,b,n, Fei Li
        //   https://www.sciencedirect.com/science/article/pii/S0098300410001238
        //
        // which seemed promising both computationally and accuracy-wise. and indeed, implementing
        // it gives us very fast convergence to correct values.
        //
        // TODO: better solution for altitudes close to the center of the geosphere. The above
        // paper specifically calls out convergence problems within 50km of earth center.
        let a = SEMI_MAJOR_AXIS;
        let b = SEMI_MINOR_AXIS;
        let a2 = FloatMath::powi(a, 2);
        let b2 = FloatMath::powi(b, 2);
        let ab = a * b;
        let z2 = FloatMath::powi(self.point.z, 2);
        let x2y2 = FloatMath::powi(self.point.x, 2) + FloatMath::powi(self.point.y, 2);
        let r2 = x2y2;
        let r = FloatMath::sqrt(x2y2);
        let bigr2 = x2y2 + z2;

        let k0 = ((FloatMath::sqrt(a2 * z2 + b2 * r2) - ab) * bigr2) / (a2 * z2 + b2 * r2);
        let mut k = k0;

        for _ in 0..GEODETIC_ITER_LIMIT {
            let p = a + b * k;
            let q = b + a * k;

            let p2 = FloatMath::powi(p, 2);
            let q2 = FloatMath::powi(q, 2);

            let f_k_value = p2 * q2 - r2 * q2 - z2 * p2;
            let f_k_derivative = 2. * (b * p * q2 + a * p2 * q - a * r2 * q - b * z2 * p);

            // NOTE(jon): dk here is the delta to the angle of the tangent _of the earth's
            // surface_, so it will get very small very quickly.
            let dk = -f_k_value / f_k_derivative;

            if !dk.is_normal() || FloatMath::abs(dk) < f64::EPSILON {
                // don't propagate NaNs and stop if there's no further refinement
                break;
            }

            k += dk;
        }

        let p = a + b * k;
        let q = b + a * k;
        let lat = FloatMath::atan((a * p * self.point.z) / (b * q * r));
        let altitude = k * FloatMath::sqrt(
            (b2 * r2 / FloatMath::powi(p, 2)) + (a2 * z2 / FloatMath::powi(q, 2)),
        );

        let wgs84 = Wgs84::builder()
            .latitude(Angle::new::<radian>(lat))
            .expect("produces lat in [-pi/2,pi/2]")
            .longitude(Angle::new::<radian>(lon))
            .altitude(Length::new::<meter>(altitude))
            .build();

        #[cfg(all(debug_assertions, any(test, feature = "approx")))]
        {
            // double check our math in tests
            let back_to_ecef = Self::from_wgs84(&wgs84);
            approx::assert_relative_eq!(self, &back_to_ecef, epsilon = Wgs84::default_epsilon());
        }

        wgs84
    }

    /// Converts an Earth-Centered, Earth-Fixed coordinate into latitude, longitude, and altitude.
    ///
    /// Note that this conversion is not trivial and needs to be approximated.
    ///
    /// This is a complete closed-form implementation based on the following paper:
    /// [A complete closed-form method for transformation from Cartesian to geodetic
    /// coordinates][quan_zhang_2024].
    ///
    /// The method is usable over geodetic heights from -6.33×10⁶m to 10¹⁰m. It achieves high
    /// precision at almost any point including the region near the pole, the equator and the
    /// center of the reference ellipsoid. This comes at the cost of a roughly 1.5 runtime increase
    /// over [`to_wgs84`][Self::to_wgs84]. Outside this range, the implementation may panic.
    ///
    /// [quan_zhang_2024]: https://link.springer.com/article/10.1007/s00190-024-01821-w
    #[must_use]
    pub fn to_wgs84_extended(&self) -> Wgs84 {
        #[cfg(any(debug_assertions, test))]
        {
            if !self.can_convert_to_wgs84() {
                panic!(
                    "conversion from ECEF to WGS84 outside altitude range \
            {ECEF_TO_WGS84_MIN_ALTITUDE_M}..{ECEF_TO_WGS84_MAX_ALTITUDE_M} \
            is not supported: {self}"
                )
            }
        }
        self.to_wgs84_extended_inner()
    }

    fn to_wgs84_extended_inner(self) -> Wgs84 {
        // Step 1
        let x = self.point.x;
        let y = self.point.y;
        let z = self.point.z;

        let m = FloatMath::powi(x, 2) + FloatMath::powi(y, 2);
        let n = FloatMath::powi(z, 2);
        let w = FloatMath::sqrt(m);

        let n_c = SQUARED_AXIS_RATIO * n;
        let p = m + n_c - L;
        let q = 27.0 * m * n_c * L;

        // Step 2
        let p3 = FloatMath::powi(p, 3);
        let p3q = p3 + q;

        let t = if p3q >= 0.0 {
            // TODO: Apply optimization if the point is not near the astroid
            // See: Appendix 1: Step 2
            //
            // The difficulty is that the paper doesn't give a threshold for
            // what is near the astroid.
            let p3q_root = FloatMath::sqrt(p3q);
            let q_root = FloatMath::sqrt(q);

            p + FloatMath::cbrt(FloatMath::powi(p3q_root + q_root, 2))
                + FloatMath::cbrt(FloatMath::powi(p3q_root - q_root, 2))
        } else {
            let qp3_root = FloatMath::sqrt(-q / p3);
            -p * (qp3_root / FloatMath::cos(FloatMath::acos(qp3_root) / 3.0))
        };

        // Step 3
        let u_m = FloatMath::sqrt(36.0 * m * L + FloatMath::powi(t, 2));
        let u_n_c = FloatMath::sqrt(36.0 * n_c * L + FloatMath::powi(t, 2));
        let v = u_m + u_n_c;
        let w_small = 2.0 * t + 6.0 * L + v;

        // There seems to be an error in the paper. Appendix 1 Step 3 describes:
        // I = k * W = (2.0 * (t + u_n_c)) / (w_small + FloatMath::sqrt(6.0 * L * (w_small + v + 6.0 * (m + n_c))))
        // But this formula results in incorrect results. Looking at Section 3.3 (45) and (46), it
        // seems that the given formula describes k only (46). Looking at (45) W still needs to be
        // multiplied to k to reach I.
        // I have emailed the author to hopefully receive confirmation of the above assumption.
        // Currently multiplying with W produces correct results in comparison to `to_wgs84`.
        let k = (2.0 * (t + u_n_c))
            / (w_small + FloatMath::sqrt(6.0 * L * (w_small + v + 6.0 * (m + n_c))));
        let i = k * w;
        let s = FloatMath::sqrt(FloatMath::powi(i, 2) + n);

        // Step 4 & 5
        let lambda =
            f64::signum(y) * (FRAC_PI_2 - 2.0 * FloatMath::atan(x / (w + FloatMath::abs(y))));
        let (phi, h) = if t == 0.0 && n == 0.0 {
            // Step 5
            // The paper states that φ = ±2 arctan(...), so there are two solutions for φ. Reading
            // the paper and doing some research, it looks like the solution is ambiguous from a
            // mathematical standpoint. Both solutions are correct, but in practice we need one. In
            // theory it would be nice to take the sign of the Z coordinate to select the correct
            // sign for φ, but this case only appears if n == 0 and n = Z², so we have no sign.
            // Looking at other libraries there is apparently no correct way to choose + or -.
            // GeographicLib also selects the positive result in the ambiguous case, so we do the
            // same.
            let phi = 2.0
                * FloatMath::atan(
                    FloatMath::sqrt(L - m)
                        / (FloatMath::sqrt(L - ECCENTRICITY_SQ * m)
                            + FloatMath::sqrt(SQUARED_AXIS_RATIO * m)),
                );
            let h = -FloatMath::sqrt(SQUARED_AXIS_RATIO)
                * FloatMath::sqrt(SEMI_MAJOR_AXIS_SQ - (m / ECCENTRICITY_SQ));

            (phi, h)
        } else if t > 0.0 || n > 0.0 {
            // Step 4
            let phi = 2.0 * FloatMath::atan(z / (i + s));
            // We use the optimized calculation for h, as we know that Earth is not a
            // perfect sphere.
            let h = ((k + ECCENTRICITY_SQ - 1.0) / (ECCENTRICITY_SQ * k)) * s;
            // let h =
            //     (w * i + n - SEMI_MAJOR_AXIS * FloatMath::sqrt(FloatMath::powi(i, 2) + n_c)) / s;

            (phi, h)
        } else {
            // Check Section 3.1 and 3.2
            // t is guaranteed to be >=0 and n is z^2 which can only be >=0 by definition
            unreachable!("Impossible state based on the algorithm in the paper");
        };

        let lon = lambda;
        let lat = phi;
        let altitude = h;

        let wgs84 = Wgs84::builder()
            .latitude(Angle::new::<radian>(lat))
            .expect("produces lat in [-pi/2,pi/2]")
            .longitude(Angle::new::<radian>(lon))
            .altitude(Length::new::<meter>(altitude))
            .build();

        #[cfg(all(debug_assertions, any(test, feature = "approx")))]
        {
            // double check our math in tests
            let back_to_ecef = Self::from_wgs84(&wgs84);
            approx::assert_relative_eq!(self, &back_to_ecef, epsilon = Wgs84::default_epsilon());
        }

        wgs84
    }

    /// Checks whether this ECEF coordinate is within the altitude range supported by
    /// [`to_wgs84_fast`][Self::to_wgs84_fast].
    ///
    /// The implementation of [`to_wgs84_fast`][Self::to_wgs84_fast] only guarantees correct
    /// conversion for geodetic heights between -10km and 50km from the surface of the WGS84
    /// ellipsoid. This method returns `true` if the coordinate falls within that range, and
    /// `false` otherwise.
    ///
    /// In debug builds, [`to_wgs84_fast`][Self::to_wgs84_fast] will panic if this method returns
    /// `false`. Use this method to check coordinates before conversion if you need to handle
    /// out-of-range coordinates gracefully.
    pub fn is_in_fast_wgs84_range(&self) -> bool {
        let geo_center_distance_sq =
            self.point.x * self.point.x + self.point.y * self.point.y + self.point.z * self.point.z;

        (ECEF_TO_WGS84_FAST_MIN_GEO_CENTER_DISTANCE_M_SQ
            ..=ECEF_TO_WGS84_FAST_MAX_GEO_CENTER_DISTANCE_M_SQ)
            .contains(&geo_center_distance_sq)
    }

    /// Checks whether this ECEF coordinate is within the altitude range supported by
    /// [`to_wgs84`][Self::to_wgs84].
    ///
    /// The implementation of [`to_wgs84`][Self::to_wgs84] only guarantees correct conversion for
    /// geodetic heights from -6.33×10⁶m to 10¹⁰m from the surface of the WGS84 ellipsoid. This
    /// method returns `true` if the coordinate falls within that range, and `false` otherwise.
    ///
    /// In debug builds, [`to_wgs84`][Self::to_wgs84] will panic if this method returns `false`.
    /// Use this method to check coordinates before conversion if you need to handle out-of-range
    /// coordinates gracefully.
    pub fn can_convert_to_wgs84(&self) -> bool {
        let geo_center_distance_sq =
            self.point.x * self.point.x + self.point.y * self.point.y + self.point.z * self.point.z;

        (ECEF_TO_WGS84_MIN_GEO_CENTER_DISTANCE_M_SQ..=ECEF_TO_WGS84_MAX_GEO_CENTER_DISTANCE_M_SQ)
            .contains(&geo_center_distance_sq)
    }
}

impl From<Coordinate<Ecef>> for Wgs84 {
    fn from(ecef: Coordinate<Ecef>) -> Self {
        ecef.to_wgs84()
    }
}

impl From<Wgs84> for Coordinate<Ecef> {
    fn from(wgs84: Wgs84) -> Self {
        Self::from_wgs84(&wgs84)
    }
}

/// Computes the central angle between the given lat/lon points.
///
/// To turn this angle into [great-circle distance], multiply this value by the radius of the
/// sphere (ie, of the earth).
///
/// The current implementation computes this [using the archaversine] (inverse haversine).
///
/// [great-circle distance]: https://en.wikipedia.org/wiki/Great-circle_distance
/// [using the archaversine]: https://en.wikipedia.org/wiki/Haversine_formula#Formulation
pub(crate) fn central_angle_by_inverse_haversine(
    lat_a: Angle,
    lat_b: Angle,
    lon_a: Angle,
    lon_b: Angle,
) -> Angle {
    let lat_a = lat_a.get::<radian>(); // φ1
    let lat_b = lat_b.get::<radian>(); // φ2
    let lon_a = lon_a.get::<radian>(); // λ1
    let lon_b = lon_b.get::<radian>(); // λ2
    let delta_lat = lat_b - lat_a;
    let delta_lon = lon_b - lon_a;

    let inner = 1. - FloatMath::cos(delta_lat)
        + FloatMath::cos(lat_a) * FloatMath::cos(lat_b) * (1. - FloatMath::cos(delta_lon));
    Angle::new::<radian>(2. * FloatMath::asin(FloatMath::sqrt(inner / 2.)))
}

#[cfg(any(test, feature = "approx"))]
impl AbsDiffEq<Self> for Wgs84 {
    type Epsilon = Length;

    fn default_epsilon() -> Self::Epsilon {
        // NOTE(jon): this value is in Meters, and realistically we're fine with just-sub-meter
        // precision. we kind of have to be since the conversion from ECEF to lat/lon is inherently
        // lossy (it needs the tangent to the curvature of the earth, which challenges f64's
        // epsilon).
        Length::new::<meter>(0.75)
    }

    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
        self.haversine_distance_on_surface(other) < epsilon
            && self
                .altitude
                .get::<meter>()
                .abs_diff_eq(&other.altitude.get::<meter>(), epsilon.get::<meter>())
    }
}

#[cfg(any(test, feature = "approx"))]
impl RelativeEq for Wgs84 {
    fn default_max_relative() -> Self::Epsilon {
        Length::new::<meter>(f64::default_max_relative())
    }

    fn relative_eq(
        &self,
        other: &Self,
        epsilon: Self::Epsilon,
        max_relative: Self::Epsilon,
    ) -> bool {
        self.haversine_distance_on_surface(other)
            .get::<meter>()
            .abs_diff_eq(&0., epsilon.get::<meter>())
            && self.altitude.get::<meter>().relative_eq(
                &other.altitude.get::<meter>(),
                epsilon.get::<meter>(),
                max_relative.get::<meter>(),
            )
    }
}

/// Argument type for [`Wgs84::build`].
#[derive(Debug, Default)]
#[must_use]
pub struct Components {
    /// The latitude angle of the proposed [`Wgs84`] coordinate.
    ///
    /// The latitude must be in [-90°,90°] % 360°. If it is not, this function returns `None`.
    pub latitude: Angle,

    /// The longitude angle of the proposed [`Wgs84`] coordinate.
    pub longitude: Angle,

    /// The altitude of the proposed [`Wgs84`] coordinate.
    ///
    /// The altitude is measured as distance above the WGS84 datum reference ellipsoid.
    pub altitude: Length,
}

/// Used to indicate that a partially-constructed [`Wgs84`] is missing the latitude component.
pub struct MissingLatitude;
/// Used to indicate that a partially-constructed [`Wgs84`] has the latitude component set.
pub struct HasLatitude;
/// Used to indicate that a partially-constructed [`Wgs84`] is missing the longitude component.
pub struct MissingLongitude;
/// Used to indicate that a partially-constructed [`Wgs84`] has the longitude component set.
pub struct HasLongitude;
/// Used to indicate that a partially-constructed [`Wgs84`] is missing the altitude component.
pub struct MissingAltitude;
/// Used to indicate that a partially-constructed [`Wgs84`] has the altitude component set.
pub struct HasAltitude;

/// [Builder] for a [`Wgs84`] coordinate.
///
/// Construct one through [`Wgs84::builder`] or [`Wgs84::to_builder`], and finalize with [`Builder::build`].
///
/// [Builder]: https://rust-unofficial.github.io/patterns/patterns/creational/builder.html
#[derive(Debug)]
#[must_use]
pub struct Builder<Latitude, Longitude, Altitude> {
    under_construction: Wgs84,
    has: (
        PhantomData<Latitude>,
        PhantomData<Longitude>,
        PhantomData<Altitude>,
    ),
}

// manual impls of Clone and Copy to avoid requiring In: Copy + Clone
impl<L1, L2, A> Clone for Builder<L1, L2, A> {
    fn clone(&self) -> Self {
        *self
    }
}
impl<L1, L2, A> Copy for Builder<L1, L2, A> {}

impl<L1, L2, A> Builder<L1, L2, A> {
    /// Sets the latitudal angle of the [`Wgs84`]-to-be.
    ///
    /// The latitude must be in [-90°,90°] % 360°. If it is not, this function returns `None`.
    pub fn latitude(mut self, latitude: impl Into<Angle>) -> Option<Builder<HasLatitude, L2, A>> {
        let latitude = latitude.into();
        let latitude_in_signed_radians = BoundedAngle::new(latitude).to_signed_range();
        if !(-FRAC_PI_2..=FRAC_PI_2).contains(&latitude_in_signed_radians) {
            None
        } else {
            self.under_construction.latitude = latitude;
            Some(Builder {
                under_construction: self.under_construction,
                has: (PhantomData::<HasLatitude>, self.has.1, self.has.2),
            })
        }
    }

    /// Sets the longitudal angle of the [`Wgs84`]-to-be.
    pub fn longitude(mut self, longitude: impl Into<Angle>) -> Builder<L1, HasLongitude, A> {
        self.under_construction.longitude = longitude.into();
        Builder {
            under_construction: self.under_construction,
            has: (self.has.0, PhantomData::<HasLongitude>, self.has.2),
        }
    }

    /// Sets the altitude of the [`Wgs84`]-to-be.
    ///
    /// The altitude is measured as distance above the WGS84 datum reference ellipsoid.
    pub fn altitude(mut self, altitude: impl Into<Length>) -> Builder<L1, L2, HasAltitude> {
        self.under_construction.altitude = altitude.into();
        Builder {
            under_construction: self.under_construction,
            has: (self.has.0, self.has.1, PhantomData::<HasAltitude>),
        }
    }
}

impl Builder<HasLatitude, HasLongitude, HasAltitude> {
    #[must_use]
    pub fn build(self) -> Wgs84 {
        self.under_construction
    }
}

/// Constructs a [`Wgs84`] coordinate with compile-time validated angles using unit suffixes.
///
/// This macro provides a safe way to construct WGS84 coordinates with compile-time known angles,
/// eliminating the need for `.expect()` calls on the latitude validation.
///
/// # Supported Units
/// - `deg` - degrees (for latitude and longitude)
/// - `rad` - radians (for latitude and longitude)
/// - `m` - meters (for altitude)
/// - `km` - kilometers (for altitude)
///
/// # Examples
/// ```rust
/// use sguaba::wgs84;
///
/// // Using degrees and meters
/// let location1 = wgs84!(latitude = deg(35.3619), longitude = deg(138.7280), altitude = m(2294.0));
///
/// // Using degrees and kilometers
/// let location2 = wgs84!(latitude = deg(35.3619), longitude = deg(138.7280), altitude = km(2.294));
///
/// // Using radians and meters
/// let location3 = wgs84!(latitude = rad(0.617), longitude = rad(2.413), altitude = m(2294.0));
/// ```
///
/// # Compile-time validation
///
/// The following examples should fail to compile because latitude is out of range:
///
/// ```compile_fail
/// # use sguaba::wgs84;
/// // Latitude > 90° should fail
/// let location = wgs84!(latitude = deg(91.0), longitude = deg(0.0), altitude = m(0.0));
/// ```
///
/// ```compile_fail
/// # use sguaba::wgs84;
/// // Latitude < -90° should fail
/// let location = wgs84!(latitude = deg(-91.0), longitude = deg(0.0), altitude = m(0.0));
/// ```
///
/// ```compile_fail
/// # use sguaba::wgs84;
/// // Latitude > π/2 radians should fail
/// let location = wgs84!(latitude = rad(1.58), longitude = rad(0.0), altitude = m(0.0));
/// ```
///
/// ```compile_fail
/// # use sguaba::wgs84;
/// // Latitude < -π/2 radians should fail
/// let location = wgs84!(latitude = rad(-1.58), longitude = rad(0.0), altitude = m(0.0));
/// ```
#[macro_export]
macro_rules! wgs84 {
    (latitude = deg($lat:expr), longitude = deg($lng:expr), altitude = m($alt:expr)) => {{
        const _: () = assert!(
            $lat >= -90.0 && $lat <= 90.0,
            "latitude must be in [-90°, 90°]"
        );
        $crate::systems::Wgs84::builder()
            .latitude(::uom::si::f64::Angle::new::<::uom::si::angle::degree>($lat))
            .expect("latitude is valid because it was checked at compile time")
            .longitude(::uom::si::f64::Angle::new::<::uom::si::angle::degree>($lng))
            .altitude(::uom::si::f64::Length::new::<::uom::si::length::meter>(
                $alt,
            ))
            .build()
    }};
    (latitude = deg($lat:expr), longitude = deg($lng:expr), altitude = km($alt:expr)) => {{
        const _: () = assert!(
            $lat >= -90.0 && $lat <= 90.0,
            "latitude must be in [-90°, 90°]"
        );
        $crate::systems::Wgs84::builder()
            .latitude(::uom::si::f64::Angle::new::<::uom::si::angle::degree>($lat))
            .expect("latitude is valid because it was checked at compile time")
            .longitude(::uom::si::f64::Angle::new::<::uom::si::angle::degree>($lng))
            .altitude(::uom::si::f64::Length::new::<::uom::si::length::kilometer>(
                $alt,
            ))
            .build()
    }};
    (latitude = rad($lat:expr), longitude = rad($lng:expr), altitude = m($alt:expr)) => {{
        const _: () = assert!(
            $lat >= -::core::f64::consts::FRAC_PI_2 && $lat <= ::core::f64::consts::FRAC_PI_2,
            "latitude must be in [-π/2, π/2] radians"
        );
        $crate::systems::Wgs84::builder()
            .latitude(::uom::si::f64::Angle::new::<::uom::si::angle::radian>($lat))
            .expect("latitude is valid because it was checked at compile time")
            .longitude(::uom::si::f64::Angle::new::<::uom::si::angle::radian>($lng))
            .altitude(::uom::si::f64::Length::new::<::uom::si::length::meter>(
                $alt,
            ))
            .build()
    }};
    (latitude = rad($lat:expr), longitude = rad($lng:expr), altitude = km($alt:expr)) => {{
        const _: () = assert!(
            $lat >= -::core::f64::consts::FRAC_PI_2 && $lat <= ::core::f64::consts::FRAC_PI_2,
            "latitude must be in [-π/2, π/2] radians"
        );
        $crate::systems::Wgs84::builder()
            .latitude(::uom::si::f64::Angle::new::<::uom::si::angle::radian>($lat))
            .expect("latitude is valid because it was checked at compile time")
            .longitude(::uom::si::f64::Angle::new::<::uom::si::angle::radian>($lng))
            .altitude(::uom::si::f64::Length::new::<::uom::si::length::kilometer>(
                $alt,
            ))
            .build()
    }};
}

#[cfg(test)]
mod tests {
    use std::panic;

    use super::Wgs84;
    use crate::coordinate;
    use crate::coordinate_systems::Ecef;
    use crate::coordinates::Coordinate;
    use crate::geodetic::{Components, ECEF_TO_WGS84_MAX_ALTITUDE_M, ECEF_TO_WGS84_MIN_ALTITUDE_M};
    use crate::util::BoundedAngle;
    use approx::{AbsDiffEq, assert_relative_eq};
    use quickcheck::quickcheck;
    use rstest::rstest;
    use std::boxed::Box;
    use std::f64::consts::{FRAC_PI_2, PI, TAU};
    use uom::si::f64::{Angle, Length};
    use uom::si::{
        angle::{degree, radian},
        length::{meter, micrometer},
    };

    fn m(meters: f64) -> Length {
        Length::new::<meter>(meters)
    }
    fn d(degrees: f64) -> Angle {
        Angle::new::<degree>(degrees)
    }

    impl quickcheck::Arbitrary for Wgs84 {
        fn arbitrary(g: &mut quickcheck::Gen) -> Self {
            // quickcheck will give us awkward f64 values -- we ignore those
            let latitude = loop {
                match f64::arbitrary(g) {
                    0. => break 0.,
                    f if f.is_normal() => break f,
                    _ => {}
                }
            };
            let longitude = loop {
                match f64::arbitrary(g) {
                    0. => break 0.,
                    f if f.is_normal() => break f,
                    _ => {}
                }
            };
            let altitude = loop {
                match f64::arbitrary(g) {
                    0. => break 0.,
                    f if f.is_normal() => break f,
                    _ => {}
                }
            };
            Self {
                latitude: Angle::new::<radian>(latitude.rem_euclid(PI) - FRAC_PI_2),
                longitude: Angle::new::<radian>(longitude.rem_euclid(TAU)),
                altitude: Length::new::<meter>(
                    // Generates values ranged ECEF_TO_WGS84_MIN_ALTITUDE_M..ECEF_TO_WGS84_MAX_ALTITUDE_M
                    altitude
                        .rem_euclid(ECEF_TO_WGS84_MAX_ALTITUDE_M - ECEF_TO_WGS84_MIN_ALTITUDE_M)
                        + ECEF_TO_WGS84_MIN_ALTITUDE_M,
                ),
            }
        }

        fn shrink(&self) -> Box<dyn Iterator<Item = Self>> {
            let Self {
                latitude,
                longitude,
                altitude,
            } = *self;
            if altitude.get::<meter>() == 0. {
                if longitude.get::<radian>() == 0. {
                    Box::new(latitude.get::<radian>().shrink().map(move |lat| Self {
                        latitude: Angle::new::<radian>(lat),
                        longitude,
                        altitude,
                    }))
                } else {
                    Box::new(longitude.get::<radian>().shrink().map(move |lon| Self {
                        latitude,
                        longitude: Angle::new::<radian>(lon),
                        altitude,
                    }))
                }
            } else {
                Box::new(altitude.get::<meter>().shrink().map(move |alt| Self {
                    latitude,
                    longitude,
                    altitude: Length::new::<meter>(alt),
                }))
            }
        }
    }

    #[test]
    fn wgs84_macro() {
        // Test degrees with meters
        let location1 = wgs84!(
            latitude = deg(35.3619),
            longitude = deg(138.7280),
            altitude = m(2294.0)
        );
        assert_eq!(location1.latitude(), d(35.3619));
        assert_eq!(location1.longitude(), d(138.7280));
        assert_eq!(location1.altitude(), m(2294.0));

        // Test degrees with kilometers
        let location2 = wgs84!(
            latitude = deg(35.3619),
            longitude = deg(138.7280),
            altitude = km(2.294)
        );
        assert_eq!(location2.latitude(), d(35.3619));
        assert_eq!(location2.longitude(), d(138.7280));
        assert_eq!(location2.altitude(), m(2294.0)); // Should be converted to meters

        // Test radians with meters
        let location3 = wgs84!(
            latitude = rad(0.617),
            longitude = rad(2.413),
            altitude = m(1000.0)
        );
        assert_relative_eq!(location3.latitude().get::<radian>(), 0.617);
        assert_relative_eq!(location3.longitude().get::<radian>(), 2.413);
        assert_eq!(location3.altitude(), m(1000.0));

        // Test radians with kilometers
        let location4 = wgs84!(
            latitude = rad(0.617),
            longitude = rad(2.413),
            altitude = km(1.0)
        );
        assert_relative_eq!(location4.latitude().get::<radian>(), 0.617);
        assert_relative_eq!(location4.longitude().get::<radian>(), 2.413);
        assert_eq!(location4.altitude(), m(1000.0)); // Should be converted to meters

        // Test boundary values for latitude
        let location5 = wgs84!(
            latitude = deg(90.0),
            longitude = deg(0.0),
            altitude = m(0.0)
        );
        assert_eq!(location5.latitude(), d(90.0));

        let location6 = wgs84!(
            latitude = deg(-90.0),
            longitude = deg(0.0),
            altitude = m(0.0)
        );
        assert_eq!(location6.latitude(), d(-90.0));

        // Test with radians at boundaries
        let location7 = wgs84!(
            latitude = rad(FRAC_PI_2),
            longitude = rad(0.0),
            altitude = m(0.0)
        );
        assert_relative_eq!(location7.latitude().get::<radian>(), FRAC_PI_2);

        let location8 = wgs84!(
            latitude = rad(-FRAC_PI_2),
            longitude = rad(0.0),
            altitude = m(0.0)
        );
        assert_relative_eq!(location8.latitude().get::<radian>(), -FRAC_PI_2);
    }

    #[test]
    fn wgs_builder() {
        let wgs = Wgs84::build(Components {
            latitude: d(1.0),
            longitude: d(2.0),
            altitude: m(3.0),
        })
        .unwrap();
        assert_eq!(wgs.latitude(), d(1.0));
        assert_eq!(wgs.longitude(), d(2.0));
        assert_eq!(wgs.altitude(), m(3.0));

        let wgs = wgs.to_builder().latitude(d(10.0)).unwrap().build();

        assert_eq!(wgs.latitude(), d(10.0));
        assert_eq!(wgs.longitude(), d(2.0));
        assert_eq!(wgs.altitude(), m(3.0));

        let wgs = wgs.to_builder().longitude(d(20.0)).build();

        assert_eq!(wgs.latitude(), d(10.0));
        assert_eq!(wgs.longitude(), d(20.0));
        assert_eq!(wgs.altitude(), m(3.0));

        let wgs = wgs.to_builder().altitude(m(30.0)).build();

        assert_eq!(wgs.latitude(), d(10.0));
        assert_eq!(wgs.longitude(), d(20.0));
        assert_eq!(wgs.altitude(), m(30.0));
    }

    #[rstest]
    #[case(d(90.9948211), d(7.8211606), m(1000.))]
    #[case(d(190.112282), d(19.880389), m(0.))]
    fn wgs_fails_with_bad_lat(
        #[case] latitude: Angle,
        #[case] longitude: Angle,
        #[case] altitude: Length,
    ) {
        assert_eq!(
            Wgs84::build(Components {
                latitude,
                longitude,
                altitude
            }),
            None,
            "WGS84 position with lat in (90,-90) should be bad"
        );
    }

    #[test]
    fn wgs_display() {
        for (lat, lon, alt) in [
            (0., 0., 0.),
            // Mt. Fuji
            (35.3619, 138.7280, 2294.0),
            (-35.3619, 138.7280, 2294.0),
            (35.3619, -138.7280, 2294.0),
            (-35.3619, -138.7280, 2294.0),
        ] {
            insta::assert_snapshot!(
                Wgs84::build(Components {
                    latitude: d(lat),
                    longitude: d(lon),
                    altitude: m(alt)
                })
                .unwrap()
            );
        }
    }

    fn try_wgs_ecef_roundtrip(wgs84: Wgs84, extended: bool) {
        let ecef = Coordinate::<Ecef>::from_wgs84(&wgs84);

        let lat = wgs84.latitude;
        let lon = wgs84.longitude;
        let alt = wgs84.altitude;

        // normalize lat/lon so nav_types doesn't get sad
        let lat = BoundedAngle::new(lat).to_signed_range().to_degrees();
        let lon = BoundedAngle::new(lon).to_signed_range().to_degrees();

        let location = nav_types::WGS84::from_degrees_and_meters(lat, lon, alt.get::<meter>());
        let location_ecef = nav_types::ECEF::from(location);

        let expected_ecef = Coordinate::<Ecef>::from(&location_ecef);

        // we use the WGS84 epsilon even for ECEF here since we have to allow for the loss of
        // precision when going from ECEF to lat/lon.
        assert_relative_eq!(ecef, expected_ecef, epsilon = Wgs84::default_epsilon());

        let wgs_84_result = if extended {
            ecef.to_wgs84_extended()
        } else {
            Wgs84::from(ecef)
        };
        assert_relative_eq!(wgs_84_result, wgs84);

        // also double-check that rotations of 360° are fine
        for rot in [-720., -360., 360., 720.] {
            let wgs84_rot = Wgs84::build(Components {
                latitude: d(lat + rot),
                longitude: d(lon),
                altitude: alt,
            })
            .unwrap();
            let ecef_rot = Coordinate::<Ecef>::from_wgs84(&wgs84_rot);
            assert_relative_eq!(ecef, ecef_rot, epsilon = Wgs84::default_epsilon());

            let wgs84_rot = Wgs84::build(Components {
                latitude: d(lat),
                longitude: d(lon + rot),
                altitude: alt,
            })
            .unwrap();
            let ecef_rot = Coordinate::<Ecef>::from_wgs84(&wgs84_rot);
            assert_relative_eq!(ecef, ecef_rot, epsilon = Wgs84::default_epsilon());
        }
    }

    quickcheck! {
        fn wgs_ecef_roundtrip(wgs84: Wgs84, extended: bool) -> () {
            try_wgs_ecef_roundtrip(wgs84, extended);
        }
    }

    // Check a few points that should definitely panic due to being outside the supported altitude.
    // wgs_ecef_roundtrip verifies that the conversion succeeds within the documented range.
    #[rstest]
    #[case(d(0.), d(0.), m(-50_000.))]
    #[case(d(90.), d(180.), m(-50_000.))]
    #[case(d(-90.), d(90.), m(-50_000.))]
    #[case(d(0.), d(0.), m(80_000.))]
    #[case(d(90.), d(180.), m(80_000.))]
    #[case(d(-90.), d(90.), m(80_000.))]
    #[should_panic(expected = "fast conversion from ECEF to WGS84 outside altitude range")]
    fn wgs_ecef_fast_conversion_fails_for_low_or_high_altitudes(
        #[case] lat: Angle,
        #[case] long: Angle,
        #[case] alt: Length,
    ) -> () {
        let wgs84 = Wgs84::build(Components {
            latitude: lat,
            longitude: long,
            altitude: alt,
        })
        .unwrap();
        let ecef: Coordinate<Ecef> = wgs84.into();
        let _should_not_panic = ecef.to_wgs84();
        let _should_panic = ecef.to_wgs84_fast();
    }

    // Check a few points analogous to `to_wgs84` that should succeed with the extended algorithm.
    // wgs_ecef_roundtrip verifies that the conversion succeeds within the documented range.
    #[rstest]
    #[case(d(0.), d(0.), m(-50_000.))]
    #[case(d(90.), d(180.), m(-50_000.))]
    #[case(d(-90.), d(90.), m(-50_000.))]
    #[case(d(0.), d(0.), m(80_000.))]
    #[case(d(90.), d(180.), m(80_000.))]
    #[case(d(-90.), d(90.), m(80_000.))]
    fn wgs_ecef_extended_conversion_succeeds_for_low_or_high_altitudes(
        #[case] lat: Angle,
        #[case] long: Angle,
        #[case] alt: Length,
    ) -> () {
        let wgs84 = Wgs84::build(Components {
            latitude: lat,
            longitude: long,
            altitude: alt,
        })
        .unwrap();
        let ecef: Coordinate<Ecef> = wgs84.into();
        let should_succeed = ecef.to_wgs84_extended();
        assert_relative_eq!(wgs84, should_succeed);
    }

    #[test]
    #[should_panic(expected = "conversion from ECEF to WGS84 outside altitude range")]
    fn wgs_ecef_conversion_fails_for_ecef_origin() {
        let _should_panic = Coordinate::<Ecef>::origin().to_wgs84();
    }

    // also, stress test known problematic things
    #[rstest]
    #[case(d(0.), d(0.), m(1000.))]
    #[case(d(90.), d(0.), m(1000.))]
    #[case(d(-90.), d(0.), m(1000.))]
    #[case(d(90.), d(90.), m(1000.))]
    #[case(d(90.), d(180.), m(1000.))]
    #[case(d(90.), d(-90.), m(1000.))]
    #[case(d(-90.), d(90.), m(1000.))]
    #[case(d(-90.), d(180.), m(1000.))]
    #[case(d(-90.), d(-90.), m(1000.))]
    #[case(d(89.999999), d(0.), m(1000.))]
    #[case(d(-89.999999), d(0.), m(1000.))]
    #[case(d(89.999999), d(180.), m(1000.))]
    #[case(d(-89.999999), d(180.), m(1000.))]
    #[case(d(89.999999), d(-179.99999), m(1000.))]
    #[case(d(-89.999999), d(-179.99999), m(1000.))]
    fn hard_wgs_to_ecef(#[case] lat: Angle, #[case] long: Angle, #[case] alt: Length) {
        try_wgs_ecef_roundtrip(
            Wgs84::build(Components {
                latitude: lat,
                longitude: long,
                altitude: alt,
            })
            .expect("lat in [-90,90]"),
            false,
        );
    }

    #[test]
    fn known_wgs_to_ecef() {
        for (wgs, ecef) in [
            ((0., 0., 0.), (6378137., 0., 0.)),
            (
                // Mt. Fuji
                (35.3619, 138.7280, 2294.0),
                (-3915138.118709466, 3436144.354064903, 3672011.028417511),
            ),
            (
                (-27.270950, 19.880389, 3000.),
                (5337604.33, 1930119.71, -2906308.35),
            ),
        ] {
            let (lat, lon, alt) = wgs;
            let (x, y, z) = ecef;
            let wgs84 = Wgs84::build(Components {
                latitude: d(lat),
                longitude: d(lon),
                altitude: m(alt),
            })
            .unwrap();
            let ecef = Coordinate::<Ecef>::from_wgs84(&wgs84);
            assert_relative_eq!(ecef, coordinate!(x = m(x), y = m(y), z = m(z)),);
        }
    }

    #[rstest]
    #[case(d(35.3619), d(138.7280), m(2294.0), 5, Length::new::<micrometer>(1.))]
    #[case(d(35.3619), d(138.7280), m(2294.0), 50, Length::new::<micrometer>(1.))]
    #[case(d(35.3619), d(138.7280), m(2294.0), 500, Length::new::<micrometer>(1.))]
    #[case(d(35.3619), d(138.7280), m(2294.0), 5000, Length::new::<micrometer>(1.))]
    #[case(d(35.3619), d(138.7280), m(2294.0), 50000, Length::new::<micrometer>(1.))]
    fn wgs84_to_ecef_round_trip_accumulated_drift(
        #[case] lat: Angle,
        #[case] long: Angle,
        #[case] alt: Length,
        #[case] iterations: usize,
        #[case] max_drift: Length,
    ) {
        let wgs84 = Wgs84::build(Components {
            latitude: lat,
            longitude: long,
            altitude: alt,
        })
        .unwrap();

        let original_ecef = Coordinate::<Ecef>::from_wgs84(&wgs84);
        let mut current = original_ecef;
        for _ in 0..iterations {
            let current_wgs84 = current.to_wgs84();
            let ecef = Coordinate::<Ecef>::from_wgs84(&current_wgs84);
            current = ecef;
        }

        let drift = Length::new::<meter>((original_ecef.point - current.point).norm());

        assert!(
            drift <= max_drift,
            "drift {drift:?} > max_drift {max_drift:?}"
        );
    }

    #[rstest]
    #[case(d(35.3619), d(138.7280), m(2294.0), 5, Length::new::<micrometer>(1.))]
    #[case(d(35.3619), d(138.7280), m(2294.0), 50, Length::new::<micrometer>(1.))]
    #[case(d(35.3619), d(138.7280), m(2294.0), 500, Length::new::<micrometer>(1.))]
    #[case(d(35.3619), d(138.7280), m(2294.0), 5000, Length::new::<micrometer>(1.))]
    #[case(d(35.3619), d(138.7280), m(2294.0), 50000, Length::new::<micrometer>(1.))]
    fn wgs84_to_ecef_extended_round_trip_accumulated_drift(
        #[case] lat: Angle,
        #[case] long: Angle,
        #[case] alt: Length,
        #[case] iterations: usize,
        #[case] max_drift: Length,
    ) {
        let wgs84 = Wgs84::build(Components {
            latitude: lat,
            longitude: long,
            altitude: alt,
        })
        .unwrap();

        let original_ecef = Coordinate::<Ecef>::from_wgs84(&wgs84);
        let mut current = original_ecef;
        for _ in 0..iterations {
            let current_wgs84 = current.to_wgs84_extended();
            let ecef = Coordinate::<Ecef>::from_wgs84(&current_wgs84);
            current = ecef;
        }

        let drift = Length::new::<meter>((original_ecef.point - current.point).norm());

        assert!(
            drift <= max_drift,
            "drift {drift:?} > max_drift {max_drift:?}"
        );
    }

    #[test]
    fn to_wgs84_comp() {
        let wgs84 = wgs84!(
            latitude = deg(35.3619),
            longitude = deg(138.7280),
            altitude = m(2294.0)
        );

        let ecef = Coordinate::<Ecef>::from_wgs84(&wgs84);

        let wgs84 = ecef.to_wgs84();
        let wgs84_ext = ecef.to_wgs84_extended();

        assert_relative_eq!(wgs84, wgs84_ext);
    }

    #[rstest]
    #[case(0.0, 0.0, 6378142.405664505)]
    #[case(0.0, 6378142.405664505, 0.0)]
    #[case(6378142.405664505, 0.0, 0.0)]
    fn test_to_wgs84_does_not_loop_forever(#[case] x: f64, #[case] y: f64, #[case] z: f64) {
        let coord: Coordinate<Ecef> = coordinate! {
            x = Length::new::<meter>(x),
            y = Length::new::<meter>(y),
            z = Length::new::<meter>(z),
        };
        let _ = coord.to_wgs84();
    }
}