1use crate::astro::math::vec3::{
4 dot3_fused_z_yx_ref as dot_three_fused, unit3_ref_unchecked as unit_vector,
5};
6
7use crate::constants::DEG_TO_RAD;
8pub use crate::constants::MEAN_EARTH_RADIUS_M as EARTH_RADIUS_M;
9use crate::frequencies::{self, CarrierBand};
10use crate::validate;
11use crate::GnssSystem;
12
13pub const IONOSPHERE_HEIGHT_M: f64 = 450_000.0;
14pub const IONOSPHERE_CONSTANT: f64 = 40.308193 * 1e16;
15
16#[derive(Clone, Copy, Debug, PartialEq, Eq)]
17pub struct TecGridEpoch {
18 pub unix_nanos: i64,
19 pub day_of_year: u16,
20}
21
22impl TecGridEpoch {
23 pub fn new(unix_nanos: i64, day_of_year: u16) -> Self {
24 Self {
25 unix_nanos,
26 day_of_year,
27 }
28 }
29}
30
31#[derive(Clone, Copy, Debug, PartialEq)]
32pub struct TecGridShellGeometry {
33 pub earth_radius_m: f64,
34 pub shell_height_m: f64,
35}
36
37impl TecGridShellGeometry {
38 pub const fn new(earth_radius_m: f64, shell_height_m: f64) -> Self {
39 Self {
40 earth_radius_m,
41 shell_height_m,
42 }
43 }
44
45 pub const fn default_shell() -> Self {
46 Self {
47 earth_radius_m: EARTH_RADIUS_M,
48 shell_height_m: IONOSPHERE_HEIGHT_M,
49 }
50 }
51
52 pub fn shell_radius_m(self) -> f64 {
53 self.earth_radius_m + self.shell_height_m
54 }
55}
56
57impl Default for TecGridShellGeometry {
58 fn default() -> Self {
59 Self::default_shell()
60 }
61}
62
63#[derive(Clone, Copy, Debug, PartialEq)]
64pub struct TecGridEvalOptions {
65 pub epoch: TecGridEpoch,
66 pub min_elevation_rad: f64,
67 pub nan_pierce_point_height_m: f64,
68 pub frequency_hz: f64,
69 pub shell_geometry: TecGridShellGeometry,
70}
71
72impl TecGridEvalOptions {
73 pub fn l1(epoch: TecGridEpoch) -> Self {
74 Self {
75 epoch,
76 min_elevation_rad: 5.0 * DEG_TO_RAD,
77 nan_pierce_point_height_m: IONOSPHERE_HEIGHT_M,
78 frequency_hz: frequencies::frequency_hz(GnssSystem::Gps, CarrierBand::L1)
79 .expect("canonical GPS L1 carrier exists"),
80 shell_geometry: TecGridShellGeometry::default(),
81 }
82 }
83
84 pub fn with_shell_geometry(mut self, shell_geometry: TecGridShellGeometry) -> Self {
85 self.nan_pierce_point_height_m = shell_geometry.shell_height_m;
86 self.shell_geometry = shell_geometry;
87 self
88 }
89}
90
91#[derive(Clone, Debug)]
92pub struct TecGrid {
93 epochs_ns: Vec<f64>,
94 latitudes_deg: Vec<f64>,
95 longitudes_deg: Vec<f64>,
96 values: Vec<f64>,
97}
98
99impl TecGrid {
100 pub fn new(
101 epochs_ns: Vec<f64>,
102 latitudes_deg: Vec<f64>,
103 longitudes_deg: Vec<f64>,
104 values: Vec<f64>,
105 ) -> Result<Self, String> {
106 if epochs_ns.len() < 2 || latitudes_deg.len() < 2 || longitudes_deg.len() < 2 {
107 return Err("TEC grid axes must each contain at least two entries".to_string());
108 }
109 if !strictly_increasing(&epochs_ns)
110 || !strictly_increasing(&latitudes_deg)
111 || !strictly_increasing(&longitudes_deg)
112 {
113 return Err("TEC grid axes must be strictly increasing".to_string());
114 }
115 let expected = epochs_ns
116 .len()
117 .checked_mul(latitudes_deg.len())
118 .and_then(|v| v.checked_mul(longitudes_deg.len()))
119 .ok_or_else(|| "TEC grid dimensions overflow".to_string())?;
120 if values.len() != expected {
121 return Err(format!(
122 "TEC grid has {} values but expected {}",
123 values.len(),
124 expected
125 ));
126 }
127 validate::finite_slice(&values, "TEC grid values").map_err(field_error_string)?;
128 Ok(Self {
129 epochs_ns,
130 latitudes_deg,
131 longitudes_deg,
132 values,
133 })
134 }
135
136 pub fn vtec_at_pierce_point(
137 &self,
138 epoch: TecGridEpoch,
139 longitude_deg: f64,
140 latitude_deg: f64,
141 ) -> Result<f64, String> {
142 let latitude_deg = if latitude_deg.abs() > 87.5 {
143 clamp(latitude_deg, -87.5, 87.5)
144 } else {
145 latitude_deg
146 };
147 self.interpolate_vtec(epoch.unix_nanos as f64, latitude_deg, longitude_deg)
148 }
149
150 pub(crate) fn interpolate_vtec(
151 &self,
152 epoch_ns: f64,
153 latitude_deg: f64,
154 longitude_deg: f64,
155 ) -> Result<f64, String> {
156 let epoch_ns = finite_query_value(epoch_ns, "timestamp")?;
157 let latitude_deg = finite_query_value(latitude_deg, "latitude")?;
158 let longitude_deg = finite_query_value(longitude_deg, "longitude")?;
159 let (epoch_i, epoch_y) = interval(&self.epochs_ns, epoch_ns, "timestamp")?;
160 let (lat_i, lat_y) = interval(&self.latitudes_deg, latitude_deg, "latitude")?;
161 let (lon_i, lon_y) = interval(&self.longitudes_deg, longitude_deg, "longitude")?;
162
163 let indices = [epoch_i, lat_i, lon_i];
164 let norm_distances = [epoch_y, lat_y, lon_y];
165 let shift_norm_distances = [
166 1.0 - norm_distances[0],
167 1.0 - norm_distances[1],
168 1.0 - norm_distances[2],
169 ];
170 let shift_indices = [indices[0] + 1, indices[1] + 1, indices[2] + 1];
171
172 let mut value = 0.0;
173 for a in 0..2 {
174 for b in 0..2 {
175 for c in 0..2 {
176 let i0 = if a == 0 { indices[0] } else { shift_indices[0] };
177 let i1 = if b == 0 { indices[1] } else { shift_indices[1] };
178 let i2 = if c == 0 { indices[2] } else { shift_indices[2] };
179 let w0 = if a == 0 {
180 shift_norm_distances[0]
181 } else {
182 norm_distances[0]
183 };
184 let w1 = if b == 0 {
185 shift_norm_distances[1]
186 } else {
187 norm_distances[1]
188 };
189 let w2 = if c == 0 {
190 shift_norm_distances[2]
191 } else {
192 norm_distances[2]
193 };
194
195 let mut weight = 1.0;
196 weight *= w0;
197 weight *= w1;
198 weight *= w2;
199 let term = self.value_at(i0, i1, i2) * weight;
200 value += term;
201 }
202 }
203 }
204 Ok(value)
205 }
206
207 fn value_at(&self, epoch_i: usize, lat_i: usize, lon_i: usize) -> f64 {
208 let n_lat = self.latitudes_deg.len();
209 let n_lon = self.longitudes_deg.len();
210 self.values[(epoch_i * n_lat + lat_i) * n_lon + lon_i]
211 }
212}
213
214pub fn iono_delay_xyz<F>(
215 grid: &TecGrid,
216 options: TecGridEvalOptions,
217 sat_xyz: &[f64; 3],
218 receiver_xyz: &[f64; 3],
219 ecef_to_lla: F,
220) -> Result<f64, String>
221where
222 F: Fn(&[f64; 3]) -> [f64; 3],
223{
224 validate_frequency(options.frequency_hz)?;
225
226 let (_vtec, stec) = tec_xyz(grid, options, sat_xyz, receiver_xyz, ecef_to_lla)?;
227 let delay_m = IONOSPHERE_CONSTANT * stec / (options.frequency_hz * options.frequency_hz);
228 validate::finite(delay_m, "ionosphere_delay_m")
229 .map_err(field_error_string)
230 .map(|_| delay_m)
231}
232
233pub fn tec_xyz<F>(
234 grid: &TecGrid,
235 options: TecGridEvalOptions,
236 sat_xyz: &[f64; 3],
237 receiver_xyz: &[f64; 3],
238 ecef_to_lla: F,
239) -> Result<(f64, f64), String>
240where
241 F: Fn(&[f64; 3]) -> [f64; 3],
242{
243 let shell_radius_m = validate_tec_geometry_inputs(options, sat_xyz, receiver_xyz)?;
244 let (_pp_xyz, pp_lonlatalt, mut elevation_rad) =
245 pierce_point_with_shell_radius(sat_xyz, receiver_xyz, shell_radius_m, &ecef_to_lla);
246 if elevation_rad < options.min_elevation_rad {
247 elevation_rad = options.min_elevation_rad;
248 }
249 validate::finite(elevation_rad, "elevation_rad").map_err(field_error_string)?;
250
251 let pp_lonlatalt = if pp_lonlatalt.iter().any(|v| v.is_nan()) {
252 let receiver_lonlatalt = ecef_to_lla(receiver_xyz);
253 [
254 receiver_lonlatalt[0],
255 receiver_lonlatalt[1],
256 options.nan_pierce_point_height_m,
257 ]
258 } else {
259 pp_lonlatalt
260 };
261
262 let vtec = grid.vtec_at_pierce_point(options.epoch, pp_lonlatalt[0], pp_lonlatalt[1])?;
263 validate::finite(vtec, "vtec").map_err(field_error_string)?;
264 let obliquity_arg =
265 options.shell_geometry.earth_radius_m * elevation_rad.cos() / shell_radius_m;
266 validate::finite(obliquity_arg, "obliquity_arg").map_err(field_error_string)?;
267 let mapping_denominator = 1.0 - obliquity_arg * obliquity_arg;
268 validate::finite_positive(mapping_denominator, "TEC mapping denominator")
269 .map_err(field_error_string)?;
270 let stec = vtec / mapping_denominator.sqrt();
271 validate::finite(stec, "stec").map_err(field_error_string)?;
272 Ok((vtec, stec))
273}
274
275pub fn pierce_point_with_shell_radius<F>(
276 sat_xyz: &[f64; 3],
277 receiver_xyz: &[f64; 3],
278 shell_radius_m: f64,
279 ecef_to_lla: F,
280) -> ([f64; 3], [f64; 3], f64)
281where
282 F: Fn(&[f64; 3]) -> [f64; 3],
283{
284 let receiver_sat_vector = [
285 sat_xyz[0] - receiver_xyz[0],
286 sat_xyz[1] - receiver_xyz[1],
287 sat_xyz[2] - receiver_xyz[2],
288 ];
289
290 let receiver_up = unit_vector(receiver_xyz);
291 let sat_unit = unit_vector(&receiver_sat_vector);
292 let elevation_rad = dot_three_fused(&sat_unit, &receiver_up).asin();
293
294 let a = 1.0;
295 let b = 2.0 * dot_three_fused(receiver_xyz, &sat_unit);
296 let c = dot_three_fused(receiver_xyz, receiver_xyz) - shell_radius_m * shell_radius_m;
297 let t = (-b + (b * b - 4.0 * a * c).sqrt()) / (2.0 * a);
298
299 let pp_xyz = [
300 receiver_xyz[0] + t * sat_unit[0],
301 receiver_xyz[1] + t * sat_unit[1],
302 receiver_xyz[2] + t * sat_unit[2],
303 ];
304 let pp_lonlatalt = ecef_to_lla(&pp_xyz);
305 (pp_xyz, pp_lonlatalt, elevation_rad)
306}
307
308fn clamp(v: f64, lo: f64, hi: f64) -> f64 {
309 if v < lo {
310 lo
311 } else if v > hi {
312 hi
313 } else {
314 v
315 }
316}
317
318fn strictly_increasing(values: &[f64]) -> bool {
319 values.windows(2).all(|w| w[1] > w[0])
320}
321
322fn finite_query_value(value: f64, name: &'static str) -> Result<f64, String> {
323 validate::finite(value, name).map_err(field_error_string)
324}
325
326fn field_error_string(error: validate::FieldError) -> String {
327 format!("{} {}", error.field(), error.reason())
328}
329
330fn validate_frequency(frequency_hz: f64) -> Result<(), String> {
331 validate::finite_positive(frequency_hz, "frequency_hz")
332 .map(|_| ())
333 .map_err(field_error_string)
334}
335
336fn validate_tec_geometry_inputs(
337 options: TecGridEvalOptions,
338 sat_xyz: &[f64; 3],
339 receiver_xyz: &[f64; 3],
340) -> Result<f64, String> {
341 validate::finite_vec3(*sat_xyz, "satellite_xyz").map_err(field_error_string)?;
342 validate::finite_vec3(*receiver_xyz, "receiver_xyz").map_err(field_error_string)?;
343 validate::finite(options.min_elevation_rad, "min_elevation_rad").map_err(field_error_string)?;
344 validate::finite(
345 options.nan_pierce_point_height_m,
346 "nan_pierce_point_height_m",
347 )
348 .map_err(field_error_string)?;
349 validate::finite_positive(options.shell_geometry.earth_radius_m, "earth_radius_m")
350 .map_err(field_error_string)?;
351 validate::finite_nonneg(options.shell_geometry.shell_height_m, "shell_height_m")
352 .map_err(field_error_string)?;
353
354 let shell_radius_m = options.shell_geometry.shell_radius_m();
355 validate::finite_positive(shell_radius_m, "shell_radius_m").map_err(field_error_string)?;
356
357 let receiver_radius_m = dot_three_fused(receiver_xyz, receiver_xyz).sqrt();
358 validate::finite_positive(receiver_radius_m, "receiver radius_m")
359 .map_err(field_error_string)?;
360
361 let line_of_sight_m = [
362 sat_xyz[0] - receiver_xyz[0],
363 sat_xyz[1] - receiver_xyz[1],
364 sat_xyz[2] - receiver_xyz[2],
365 ];
366 validate::finite_vec3(line_of_sight_m, "line of sight_m").map_err(field_error_string)?;
367 let line_of_sight_norm_m = dot_three_fused(&line_of_sight_m, &line_of_sight_m).sqrt();
368 validate::finite_positive(line_of_sight_norm_m, "line of sight_m")
369 .map_err(field_error_string)?;
370
371 Ok(shell_radius_m)
372}
373
374fn interval(axis: &[f64], x: f64, name: &str) -> Result<(usize, f64), String> {
375 if x < axis[0] || x > axis[axis.len() - 1] {
376 return Err(format!("{name} {x} is out of TEC grid bounds"));
377 }
378 let upper = axis.partition_point(|v| *v <= x);
379 let mut lower = upper.saturating_sub(1);
380 if lower >= axis.len() - 1 {
381 lower = axis.len() - 2;
382 }
383 let y = (x - axis[lower]) / (axis[lower + 1] - axis[lower]);
384 Ok((lower, y))
385}
386
387#[cfg(test)]
388mod tests {
389 use super::*;
390
391 fn small_grid() -> TecGrid {
392 TecGrid::new(
393 vec![0.0, 10.0],
394 vec![0.0, 10.0],
395 vec![20.0, 30.0],
396 vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0],
397 )
398 .expect("small TEC grid")
399 }
400
401 #[test]
402 fn interpolate_vtec_rejects_non_finite_query_coordinates() {
403 let grid = small_grid();
404 let cases = [
405 (f64::NAN, 5.0, 25.0, "timestamp"),
406 (f64::INFINITY, 5.0, 25.0, "timestamp"),
407 (5.0, f64::NAN, 25.0, "latitude"),
408 (5.0, 5.0, f64::NAN, "longitude"),
409 ];
410
411 for (epoch_ns, latitude_deg, longitude_deg, field) in cases {
412 let error = grid
413 .interpolate_vtec(epoch_ns, latitude_deg, longitude_deg)
414 .expect_err("non-finite TEC coordinate must be rejected");
415 assert!(error.contains(field), "{error}");
416 assert!(error.contains("not finite"), "{error}");
417 }
418 }
419
420 #[test]
421 fn interpolate_vtec_valid_query_still_interpolates() {
422 let grid = small_grid();
423
424 assert_eq!(
425 grid.interpolate_vtec(0.0, 0.0, 20.0)
426 .expect("lower corner")
427 .to_bits(),
428 1.0f64.to_bits()
429 );
430 assert_eq!(
431 grid.interpolate_vtec(5.0, 5.0, 25.0)
432 .expect("center point")
433 .to_bits(),
434 4.5f64.to_bits()
435 );
436 }
437
438 #[test]
439 fn tec_grid_rejects_nonfinite_values() {
440 let error = TecGrid::new(
441 vec![0.0, 10.0],
442 vec![0.0, 10.0],
443 vec![20.0, 30.0],
444 vec![1.0, f64::NAN, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0],
445 )
446 .expect_err("nonfinite TEC grid cells must be rejected");
447
448 assert!(error.contains("TEC grid values"), "{error}");
449 assert!(error.contains("not finite"), "{error}");
450 }
451
452 #[test]
453 fn tec_xyz_rejects_degenerate_geometry_without_nonfinite_success() {
454 fn passthrough_lla(xyz: &[f64; 3]) -> [f64; 3] {
455 [xyz[0], xyz[1], xyz[2]]
456 }
457
458 let grid = TecGrid::new(
459 vec![0.0, 1.0],
460 vec![-10.0, 10.0],
461 vec![0.0, 20.0],
462 vec![0.0; 8],
463 )
464 .expect("regular TEC grid");
465 let mut options = TecGridEvalOptions::l1(TecGridEpoch::new(0, 0));
466 options.min_elevation_rad = 0.0;
467 options.nan_pierce_point_height_m = 0.0;
468
469 let error = tec_xyz(
470 &grid,
471 options,
472 &[0.0, 0.0, 0.0],
473 &[0.0, 0.0, 0.0],
474 passthrough_lla,
475 )
476 .expect_err("zero receiver and satellite vectors must be rejected");
477
478 assert!(error.contains("receiver radius_m"), "{error}");
479 assert!(error.contains("not positive"), "{error}");
480 }
481
482 #[test]
483 fn iono_delay_xyz_rejects_invalid_frequency() {
484 fn passthrough_lla(_: &[f64; 3]) -> [f64; 3] {
485 [25.0, 5.0, IONOSPHERE_HEIGHT_M]
486 }
487
488 let grid = small_grid();
489 let sat_xyz = [2.0, 0.0, 0.0];
490 let receiver_xyz = [1.0, 0.0, 0.0];
491 for (frequency_hz, reason) in [(0.0, "not positive"), (f64::NAN, "not finite")] {
492 let mut options = TecGridEvalOptions::l1(TecGridEpoch::new(0, 1));
493 options.frequency_hz = frequency_hz;
494
495 let error = iono_delay_xyz(&grid, options, &sat_xyz, &receiver_xyz, passthrough_lla)
496 .expect_err("invalid TEC-grid frequency must be rejected");
497 assert!(error.contains("frequency_hz"), "{error}");
498 assert!(error.contains(reason), "{error}");
499 }
500 }
501}