use spacelab_core::{Angle, Epoch, Mat3, Tt, Ut1, Vec3};
use spacelab_fa::{cio_locator_s, cip_xy, era};
use crate::polar_motion::tio_locator;
use crate::rotation::{Cirs, Gcrf, Itrf, Rotation, Tirs, Transform};
use crate::teme::{EARTH_ROTATION_RATE, Orientation};
#[allow(clippy::many_single_char_names)] #[must_use]
pub fn gcrf_to_cirs(x: f64, y: f64, s: f64) -> Rotation<Gcrf, Cirs> {
let r2 = x * x + y * y;
let e = if r2 > 0.0 { libm::atan2(y, x) } else { 0.0 };
let d = libm::atan(libm::sqrt(r2 / (1.0 - r2)));
Rotation::from_matrix(Mat3::rot3(-(e + s)) * Mat3::rot2(d) * Mat3::rot3(e))
}
#[must_use]
pub fn cirs_to_tirs(era: Angle) -> Rotation<Cirs, Tirs> {
Rotation::from_matrix(Mat3::rot3(era.radians()))
}
#[must_use]
pub fn tirs_to_itrf(xp: Angle, yp: Angle, sp: Angle) -> Rotation<Tirs, Itrf> {
Rotation::from_matrix(
Mat3::rot1(-yp.radians()) * Mat3::rot2(-xp.radians()) * Mat3::rot3(sp.radians()),
)
}
#[must_use]
pub fn gcrf_to_itrf(
ut1: Epoch<Ut1>,
tt: Epoch<Tt>,
xp: Angle,
yp: Angle,
dx: Angle,
dy: Angle,
lod_seconds: f64,
) -> Transform<Gcrf, Itrf> {
let (x, y) = cip_xy(tt);
let s = cio_locator_s(tt, x, y);
let q = gcrf_to_cirs(x + dx.radians(), y + dy.radians(), s);
let rotation: Rotation<Gcrf, Itrf> =
tirs_to_itrf(xp, yp, tio_locator(tt)) * cirs_to_tirs(era(ut1)) * q;
let rate = EARTH_ROTATION_RATE * (1.0 - lod_seconds / 86_400.0);
let omega = q.inverse().matrix() * Vec3::new(0.0, 0.0, rate);
Transform::new(rotation, omega)
}
pub fn gcrf_to_itrf_into(epochs: &[Orientation], out: &mut [Transform<Gcrf, Itrf>]) {
assert!(
out.len() >= epochs.len(),
"output slice holds {} of {} epochs",
out.len(),
epochs.len()
);
let zero = Angle::from_radians(0.0);
for (o, slot) in epochs.iter().zip(out.iter_mut()) {
*slot = gcrf_to_itrf(o.ut1, o.tt, o.xp, o.yp, zero, zero, o.lod_seconds);
}
}