pub const NEPTUNEFLATTENING: f64 = crate::NEPTUNEFLATTENING;
pub const NEPTUNESEMIMAJORM: f64 = crate::NEPTUNEEQUATORIALRADIUS;
pub const NEPTUNESEMIMINORM: f64 = crate::NEPTUNEPOLARRADIUS;

#[derive(Debug, Clone, Copy)]
pub struct LatLon {
    pub latdeg: f64,
    pub londeg: f64,
    pub altm: f64,
}

#[derive(Debug, Clone, Copy)]
pub struct Ecef {
    pub x: f64,
    pub y: f64,
    pub z: f64,
}

impl LatLon {
    pub fn new(latdeg: f64, londeg: f64, altm: f64) -> Self {
        Self {
            latdeg,
            londeg,
            altm,
        }
    }

    pub fn toecef(&self) -> Ecef {
        let lat = self.latdeg.to_radians();
        let lon = self.londeg.to_radians();
        let e2 = 2.0 * NEPTUNEFLATTENING - NEPTUNEFLATTENING * NEPTUNEFLATTENING;
        let sinlat = lat.sin();
        let coslat = lat.cos();
        let n = NEPTUNESEMIMAJORM / (1.0 - e2 * sinlat * sinlat).sqrt();
        Ecef {
            x: (n + self.altm) * coslat * lon.cos(),
            y: (n + self.altm) * coslat * lon.sin(),
            z: (n * (1.0 - e2) + self.altm) * sinlat,
        }
    }

    pub fn tocartesiansimple(&self) -> [f64; 3] {
        let lat = self.latdeg.to_radians();
        let lon = self.londeg.to_radians();
        let r = crate::NEPTUNEEQUATORIALRADIUS + self.altm;
        [
            r * lat.cos() * lon.cos(),
            r * lat.cos() * lon.sin(),
            r * lat.sin(),
        ]
    }

    pub fn distanceto(&self, other: &LatLon) -> f64 {
        let lat1 = self.latdeg.to_radians();
        let lat2 = other.latdeg.to_radians();
        let dlat = (other.latdeg - self.latdeg).to_radians();
        let dlon = (other.londeg - self.londeg).to_radians();
        let a = (dlat / 2.0).sin().powi(2) + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
        let c = 2.0 * a.sqrt().asin();
        crate::NEPTUNEEQUATORIALRADIUS * c
    }
}

impl Ecef {
    pub fn tolatlon(&self) -> LatLon {
        let e2 = 2.0 * NEPTUNEFLATTENING - NEPTUNEFLATTENING * NEPTUNEFLATTENING;
        let p = (self.x * self.x + self.y * self.y).sqrt();
        let lon = self.y.atan2(self.x);
        let mut lat = (self.z / p).atan();
        for step in 0..10 {
            let sinlat = lat.sin();
            let n = NEPTUNESEMIMAJORM / (1.0 - e2 * sinlat * sinlat).sqrt();
            lat = (self.z + e2 * n * sinlat).atan2(p);
            if step == 9 {
                debug_assert!(lat.is_finite());
            }
        }
        let sinlat = lat.sin();
        let n = NEPTUNESEMIMAJORM / (1.0 - e2 * sinlat * sinlat).sqrt();
        let alt = p / lat.cos() - n;
        LatLon {
            latdeg: lat.to_degrees(),
            londeg: lon.to_degrees(),
            altm: alt,
        }
    }
}
