Issues with system nut/prec angles

I've been pondering what could be wrong here for days, and I just don't know. Will seek advice.

src/naif/kpl/mod.rs

@@ -89,6 +89,7 @@ pub enum Parameter {
     NutPrecDec,
     NutPrecPm,
     NutPrecAngles,
+    MaxPhaseDegree,
     LongAxis,
     PoleRa,
     PoleDec,
@@ -132,6 +133,7 @@ impl FromStr for Parameter {
             "MATRIX" => Ok(Self::Matrix),
             "UNITS" => Ok(Self::Units),
             "AXES" => Ok(Self::Axes),
+            "MAX_PHASE_DEGREE" => Ok(Self::MaxPhaseDegree),
             "GMLIST" | "NAME" | "SPEC" => {
                 whatever!("unsupported parameter `{s}`")
             }

src/naif/kpl/parser.rs

@@ -157,7 +157,6 @@ pub fn convert_tpc<'a, P: AsRef<Path>>(
     // Now that planetary_data has everything, we'll create the planetary dataset in the ANISE ASN1 format.
 
     for (object_id, planetary_data) in planetary_data {
-        dbg!(object_id);
         match planetary_data.data.get(&Parameter::GravitationalParameter) {
             Some(mu_km3_s2_value) => {
                 match mu_km3_s2_value {
@@ -182,7 +181,7 @@ pub fn convert_tpc<'a, P: AsRef<Path>>(
                             None => None,
                         };
 
-                        let constant = match planetary_data.data.get(&Parameter::PoleRa) {
+                        let mut constant = match planetary_data.data.get(&Parameter::PoleRa) {
                             Some(data) => match data {
                                 KPLValue::Matrix(pole_ra_data) => {
                                     let mut pole_ra_data = pole_ra_data.clone();
@@ -215,32 +214,6 @@ pub fn convert_tpc<'a, P: AsRef<Path>>(
                                     }
                                     let prime_mer = PhaseAngle::maybe_new(&prime_mer_data);
 
-                                    let (num_nut_prec_angles, nut_prec_angles) =
-                                        match planetary_data.data.get(&Parameter::NutPrecAngles) {
-                                            Some(nut_prec_val) => {
-                                                let nut_prec_data =
-                                                    nut_prec_val.to_vec_f64().unwrap();
-                                                let mut coeffs =
-                                                    [NutationPrecessionAngle::default();
-                                                        MAX_NUT_PREC_ANGLES];
-                                                for (i, nut_prec) in
-                                                    nut_prec_data.chunks(2).enumerate()
-                                                {
-                                                    coeffs[i] = NutationPrecessionAngle {
-                                                        offset_deg: nut_prec[0],
-                                                        rate_deg: nut_prec[1],
-                                                    };
-                                                }
-
-                                                (coeffs.len() as u8, coeffs)
-                                            }
-                                            None => (
-                                                0,
-                                                [NutationPrecessionAngle::default();
-                                                    MAX_NUT_PREC_ANGLES],
-                                            ),
-                                        };
-
                                     let long_axis =
                                         match planetary_data.data.get(&Parameter::LongAxis) {
                                             Some(val) => match val {
@@ -265,9 +238,8 @@ pub fn convert_tpc<'a, P: AsRef<Path>>(
                                         pole_right_ascension: pola_ra,
                                         pole_declination: pola_dec,
                                         prime_meridian: prime_mer,
-                                        num_nut_prec_angles,
-                                        nut_prec_angles,
                                         long_axis,
+                                        ..Default::default()
                                     }
                                 }
                                 _ => unreachable!(),
@@ -283,6 +255,32 @@ pub fn convert_tpc<'a, P: AsRef<Path>>(
                             }
                         };
 
+                        // Add the nutation precession angles, which are defined for the system
+                        if let Some(nut_prec_val) =
+                            planetary_data.data.get(&Parameter::NutPrecAngles)
+                        {
+                            let phase_deg =
+                                match planetary_data.data.get(&Parameter::MaxPhaseDegree) {
+                                    Some(val) => (val.to_i32().unwrap() + 1) as usize,
+                                    None => 2,
+                                };
+                            let nut_prec_data = nut_prec_val.to_vec_f64().unwrap();
+                            let mut coeffs =
+                                [NutationPrecessionAngle::default(); MAX_NUT_PREC_ANGLES];
+                            let mut num = 0;
+                            for (i, nut_prec) in nut_prec_data.chunks(phase_deg).enumerate() {
+                                // TODO: Convert to PhaseAngle without any nut prec angles ... or move the nut prec angles into its own field?
+                                coeffs[i] = NutationPrecessionAngle {
+                                    offset_deg: nut_prec[0],
+                                    rate_deg: nut_prec[1],
+                                };
+                                num += 1;
+                            }
+
+                            constant.num_nut_prec_angles = num;
+                            constant.nut_prec_angles = coeffs;
+                        };
+
                         dataset_builder.push_into(&mut buf, constant, Some(object_id), None)?;
                         info!("Added {object_id}");
                     }

src/orientations/rotate_to_parent.rs

@@ -41,9 +41,14 @@ impl<'a> Almanac<'a> {
                     .planetary_data
                     .get_by_id(source.orientation_id)
                     .with_context(|_| OrientationDataSetSnafu)?;
+                // Fetch the parent info
+                let system_data = self
+                    .planetary_data
+                    .get_by_id(planetary_data.parent_id)
+                    .with_context(|_| OrientationDataSetSnafu)?;
 
                 planetary_data
-                    .rotation_to_parent(epoch)
+                    .rotation_to_parent(epoch, &system_data)
                     .with_context(|_| OrientationPhysicsSnafu)
             }
         }

src/structure/planetocentric/mod.rs

@@ -21,7 +21,7 @@ pub mod nutprec;
 pub mod phaseangle;
 use der::{Decode, Encode, Reader, Writer};
 use ellipsoid::Ellipsoid;
-use hifitime::Epoch;
+use hifitime::{Epoch, Unit};
 use nutprec::NutationPrecessionAngle;
 use phaseangle::PhaseAngle;
 
@@ -126,70 +126,117 @@ impl PlanetaryData {
         bits
     }
 
+    fn uses_trig_polynomial(&self) -> bool {
+        if let Some(phase) = self.pole_right_ascension {
+            if phase.coeffs_count > 0 {
+                return true;
+            }
+        }
+
+        if let Some(phase) = self.pole_declination {
+            if phase.coeffs_count > 0 {
+                return true;
+            }
+        }
+
+        if let Some(phase) = self.prime_meridian {
+            if phase.coeffs_count > 0 {
+                return true;
+            }
+        }
+
+        false
+    }
+
     /// Computes the rotation to the parent frame
-    pub fn rotation_to_parent(&self, epoch: Epoch) -> PhysicsResult<DCM> {
+    pub fn rotation_to_parent(&self, epoch: Epoch, system: &Self) -> PhysicsResult<DCM> {
         if self.pole_declination.is_none()
             && self.prime_meridian.is_none()
             && self.pole_right_ascension.is_none()
         {
             Ok(DCM::identity(self.object_id, self.parent_id))
         } else {
             let mut variable_angles_deg = [0.0_f64; MAX_NUT_PREC_ANGLES];
-            for (ii, nut_prec_angle) in self
-                .nut_prec_angles
-                .iter()
-                .enumerate()
-                .take(self.num_nut_prec_angles.into())
-            {
-                variable_angles_deg[ii] = nut_prec_angle.evaluate_deg(epoch)
+            // Skip the computation of the nutation and precession angles of the system if we won't be using them.
+            if self.uses_trig_polynomial() {
+                for (ii, nut_prec_angle) in system
+                    .nut_prec_angles
+                    .iter()
+                    .enumerate()
+                    .take(system.num_nut_prec_angles.into())
+                {
+                    variable_angles_deg[ii] = nut_prec_angle.evaluate_deg(epoch);
+                    println!(
+                        "J{} = {nut_prec_angle} = {}",
+                        ii + 1,
+                        variable_angles_deg[ii]
+                    );
+                }
             }
 
             let right_asc_rad = match self.pole_right_ascension {
                 Some(right_asc_deg) => {
-                    let mut angle_rad = right_asc_deg.evaluate_deg(epoch, false).to_radians();
+                    let mut angle_rad = right_asc_deg
+                        .evaluate_deg(epoch, Unit::Century)
+                        .to_radians();
+                    print!("alpha = {right_asc_deg} +");
                     // Add the nutation and precession angles for this phase angle
-                    for (ii, nut_prec_coeff) in right_asc_deg
+                    for (ii, coeff) in right_asc_deg
                         .coeffs
                         .iter()
                         .enumerate()
                         .take(right_asc_deg.coeffs_count as usize)
                     {
-                        angle_rad += nut_prec_coeff * variable_angles_deg[ii].to_radians().sin();
+                        if coeff.abs() > 0.0 {
+                            print!(" {coeff} * sin({}) +", variable_angles_deg[ii]);
+                            angle_rad += coeff * variable_angles_deg[ii].to_radians().sin();
+                        }
                     }
+                    println!();
                     angle_rad + FRAC_PI_2
                 }
                 None => 0.0,
             };
 
             let dec_rad = match self.pole_declination {
                 Some(decl_deg) => {
-                    let mut angle_rad = decl_deg.evaluate_deg(epoch, false).to_radians();
+                    let mut angle_rad = decl_deg.evaluate_deg(epoch, Unit::Century).to_radians();
+                    print!("delta = {decl_deg} +");
                     // Add the nutation and precession angles for this phase angle
-                    for (ii, nut_prec_coeff) in decl_deg
+                    for (ii, coeff) in decl_deg
                         .coeffs
                         .iter()
                         .enumerate()
                         .take(decl_deg.coeffs_count as usize)
                     {
-                        angle_rad += nut_prec_coeff * variable_angles_deg[ii].to_radians().cos();
+                        if coeff.abs() > 0.0 {
+                            print!(" {coeff} * cos({}) +", variable_angles_deg[ii]);
+                            angle_rad += coeff * variable_angles_deg[ii].to_radians().cos();
+                        }
                     }
+                    println!();
                     FRAC_PI_2 - angle_rad
                 }
                 None => 0.0,
             };
 
             let twist_rad = match self.prime_meridian {
                 Some(twist_deg) => {
-                    let mut angle_rad = twist_deg.evaluate_deg(epoch, true).to_radians();
+                    let mut angle_rad = twist_deg.evaluate_deg(epoch, Unit::Day).to_radians();
+                    print!("w = {twist_deg} +");
                     // Add the nutation and precession angles for this phase angle
-                    for (ii, nut_prec_coeff) in twist_deg
+                    for (ii, coeff) in twist_deg
                         .coeffs
                         .iter()
                         .enumerate()
                         .take(twist_deg.coeffs_count as usize)
                     {
-                        angle_rad += nut_prec_coeff * variable_angles_deg[ii].to_radians().sin();
+                        if coeff.abs() > 0.0 {
+                            print!(" {coeff} * sin({}) +", variable_angles_deg[ii]);
+                            angle_rad += coeff * variable_angles_deg[ii].to_radians().sin();
+                        }
                     }
+                    println!();
                     angle_rad
                 }
                 None => 0.0,

src/structure/planetocentric/nutprec.rs

@@ -8,6 +8,7 @@
  * Documentation: https://nyxspace.com/
  */
 
+use core::fmt;
 use der::{Decode, Encode, Reader, Writer};
 use hifitime::Epoch;
 
@@ -22,8 +23,8 @@ pub struct NutationPrecessionAngle {
 impl NutationPrecessionAngle {
     /// Evaluates this nutation precession angle at the given epoch
     pub fn evaluate_deg(&self, epoch: Epoch) -> f64 {
-        let d = epoch.to_tdb_days_since_j2000();
-        dbg!(self.offset_deg + self.rate_deg * d)
+        let t = epoch.to_tdb_centuries_since_j2000();
+        self.offset_deg + self.rate_deg * t
     }
 }
 
@@ -47,6 +48,12 @@ impl<'a> Decode<'a> for NutationPrecessionAngle {
     }
 }
 
+impl fmt::Display for NutationPrecessionAngle {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(f, "{} + {} T", self.offset_deg, self.rate_deg)
+    }
+}
+
 #[cfg(test)]
 mod nut_prec_ut {
     use super::{Decode, Encode, Epoch, NutationPrecessionAngle};

src/structure/planetocentric/phaseangle.rs

@@ -7,10 +7,10 @@
  *
  * Documentation: https://nyxspace.com/
  */
-use der::{Decode, Encode, Reader, Writer};
-use hifitime::Epoch;
-
 use super::MAX_NUT_PREC_ANGLES;
+use core::fmt;
+use der::{Decode, Encode, Reader, Writer};
+use hifitime::{Epoch, Unit};
 
 /// Angle data is represented as a polynomial of an angle, exactly like in SPICE PCK.
 /// In fact, the following documentation is basically copied from [the required PCK reading](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/req/pck.html).
@@ -48,20 +48,15 @@ impl PhaseAngle {
     }
 
     /// Evaluates this phase angle in degrees provided the epoch
-    pub fn evaluate_deg(&self, epoch: Epoch, twist: bool) -> f64 {
-        let factor = if twist {
-            epoch.to_tdb_days_since_j2000()
-        } else {
-            epoch.to_tdb_centuries_since_j2000()
-        };
+    pub fn evaluate_deg(&self, epoch: Epoch, rate_unit: Unit) -> f64 {
+        let factor = epoch.to_tdb_duration().to_unit(rate_unit);
 
         self.offset_deg + self.rate_deg * factor + self.accel_deg * factor.powi(2)
     }
 }
 
 impl Encode for PhaseAngle {
     fn encoded_len(&self) -> der::Result<der::Length> {
-        // TODO: Consider encoding this as a DataArray?
         self.offset_deg.encoded_len()?
             + self.rate_deg.encoded_len()?
             + self.accel_deg.encoded_len()?
@@ -89,3 +84,17 @@ impl<'a> Decode<'a> for PhaseAngle {
         })
     }
 }
+
+impl fmt::Display for PhaseAngle {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        if self.accel_deg.abs() > 0.0 {
+            write!(
+                f,
+                "{} + {} x + {} x^2",
+                self.offset_deg, self.rate_deg, self.accel_deg
+            )
+        } else {
+            write!(f, "{} + {} x", self.offset_deg, self.rate_deg)
+        }
+    }
+}