1use thiserror::Error;
4
5#[derive(Debug, Error)]
7pub enum InterpolationError {
8 #[error(
10 "timestamps are not strictly monotonically increasing at index {index}: \
11 prev={prev}, curr={curr}"
12 )]
13 NonMonotonicTimestamps {
14 index: usize,
16 prev: i64,
18 curr: i64,
20 },
21}
22
23#[derive(Debug, Clone, Copy, PartialEq, Eq, serde::Serialize, serde::Deserialize)]
25pub enum Interpolator {
26 Linear,
28 Nearest,
30 CatmullRom,
42 Zero,
44 Forward,
46 Nocb,
48}
49
50impl Interpolator {
51 pub fn fill(
62 &self,
63 timestamps: &[i64],
64 values: &[f64],
65 expected_interval_ns: i64,
66 ) -> Result<(Vec<i64>, Vec<f64>, usize, usize), InterpolationError> {
67 if timestamps.len() < 2 || expected_interval_ns <= 0 {
68 return Ok((timestamps.to_vec(), values.to_vec(), 0, 0));
69 }
70
71 if *self == Interpolator::CatmullRom {
73 for i in 1..timestamps.len() {
74 if timestamps[i] <= timestamps[i - 1] {
75 return Err(InterpolationError::NonMonotonicTimestamps {
76 index: i,
77 prev: timestamps[i - 1],
78 curr: timestamps[i],
79 });
80 }
81 }
82 }
83
84 let span = timestamps[timestamps.len() - 1] - timestamps[0];
88 let estimated = if expected_interval_ns > 0 && span > 0 {
89 let est = (span / expected_interval_ns) as usize + 1;
90 est.max(timestamps.len())
91 } else {
92 timestamps.len()
93 };
94 let mut out_ts = Vec::with_capacity(estimated);
95 let mut out_vals = Vec::with_capacity(estimated);
96 let mut gaps_filled = 0usize;
97 let mut gaps_skipped = 0usize;
98
99 out_ts.push(timestamps[0]);
100 out_vals.push(values[0]);
101
102 for i in 1..timestamps.len() {
103 let delta = timestamps[i] - timestamps[i - 1];
104 let steps = (delta as f64 / expected_interval_ns as f64).round() as i64;
105
106 const MAX_GAP_FILL: i64 = 100_000;
108 if steps > MAX_GAP_FILL {
109 tracing::warn!(
110 gap_steps = steps,
111 max = MAX_GAP_FILL,
112 idx = i,
113 "interpolation gap exceeds MAX_GAP_FILL; skipping fill",
114 );
115 gaps_skipped += 1;
116 }
117 if steps > 1 && steps <= MAX_GAP_FILL {
118 for step in 1..steps {
120 let t =
121 timestamps[i - 1].saturating_add(step.saturating_mul(expected_interval_ns));
122 let v = self.interpolate_value(
123 timestamps,
124 values,
125 i - 1,
126 i,
127 step as f64 / steps as f64,
128 );
129 out_ts.push(t);
130 out_vals.push(v);
131 gaps_filled += 1;
132 }
133 }
134
135 out_ts.push(timestamps[i]);
136 out_vals.push(values[i]);
137 }
138
139 Ok((out_ts, out_vals, gaps_filled, gaps_skipped))
140 }
141
142 #[allow(clippy::trivially_copy_pass_by_ref)] fn interpolate_value(
145 &self,
146 timestamps: &[i64],
147 values: &[f64],
148 before_idx: usize,
149 after_idx: usize,
150 frac: f64,
151 ) -> f64 {
152 let v0 = values[before_idx];
153 let v1 = values[after_idx];
154
155 match self {
156 Interpolator::Linear => v0 + (v1 - v0) * frac,
157 Interpolator::Nearest => {
158 if frac < 0.5 {
159 v0
160 } else {
161 v1
162 }
163 }
164 Interpolator::CatmullRom => {
165 let n = values.len();
167 if before_idx > 0 && after_idx + 1 < n {
168 let p0 = values[before_idx - 1];
169 let p1 = v0;
170 let p2 = v1;
171 let p3 = values[after_idx + 1];
172 let t0 = timestamps[before_idx - 1] as f64;
173 let t1 = timestamps[before_idx] as f64;
174 let t2 = timestamps[after_idx] as f64;
175 let t3 = timestamps[after_idx + 1] as f64;
176 let t = t1 + (t2 - t1) * frac;
178 catmull_rom_nonuniform(p0, p1, p2, p3, t0, t1, t2, t3, t)
179 } else {
180 v0 + (v1 - v0) * frac
182 }
183 }
184 Interpolator::Zero => 0.0,
185 Interpolator::Forward => v0,
186 Interpolator::Nocb => v1,
187 }
188 }
189}
190
191#[allow(clippy::too_many_arguments)] fn catmull_rom_nonuniform(
197 p0: f64,
198 p1: f64,
199 p2: f64,
200 p3: f64,
201 t0: f64,
202 t1: f64,
203 t2: f64,
204 t3: f64,
205 t: f64,
206) -> f64 {
207 let dt10 = t1 - t0;
208 let dt21 = t2 - t1;
209 let dt32 = t3 - t2;
210 if dt21.abs() < 1e-12 {
212 return p1;
213 }
214 let safe = |d: f64, fallback: f64| if d.abs() < 1e-12 { fallback } else { d };
215 let dt10 = safe(dt10, dt21);
216 let dt32 = safe(dt32, dt21);
217
218 let m1 = (p2 - p1) / dt21 + (p1 - p0) / dt10 - (p2 - p0) / (dt10 + dt21);
220 let m2 = (p2 - p1) / dt21 + (p3 - p2) / dt32 - (p3 - p1) / (dt21 + dt32);
221 let mut m1 = m1 * dt21;
223 let mut m2 = m2 * dt21;
224
225 let delta = p2 - p1;
228 if delta.abs() < 1e-30 {
229 m1 = 0.0;
231 m2 = 0.0;
232 } else {
233 if m1 * delta < 0.0 {
235 m1 = 0.0;
236 }
237 if m2 * delta < 0.0 {
238 m2 = 0.0;
239 }
240 let alpha = m1 / delta;
242 let beta = m2 / delta;
243 let r2 = alpha * alpha + beta * beta;
244 if r2 > 9.0 {
245 let phi = 3.0 / r2.sqrt();
246 m1 = phi * alpha * delta;
247 m2 = phi * beta * delta;
248 }
249 }
250
251 let u = (t - t1) / dt21;
252 let u2 = u * u;
253 let u3 = u2 * u;
254 (2.0 * u3 - 3.0 * u2 + 1.0) * p1
256 + (u3 - 2.0 * u2 + u) * m1
257 + (-2.0 * u3 + 3.0 * u2) * p2
258 + (u3 - u2) * m2
259}
260
261#[cfg(test)]
262mod tests {
263 use super::*;
264
265 fn make_ts(secs: &[i64]) -> Vec<i64> {
266 secs.iter().map(|&s| s * 1_000_000_000).collect()
267 }
268
269 #[test]
270 fn linear_interpolation() {
271 let ts = make_ts(&[0, 1, 4, 5]); let vals = vec![10.0, 20.0, 50.0, 60.0];
273 let (out_ts, out_vals, filled, _) = Interpolator::Linear
274 .fill(&ts, &vals, 1_000_000_000)
275 .unwrap();
276 assert_eq!(filled, 2); assert_eq!(out_ts.len(), 6);
278 assert!((out_vals[2] - 30.0).abs() < 1e-6);
280 assert!((out_vals[3] - 40.0).abs() < 1e-6);
282 }
283
284 #[test]
285 fn nearest_interpolation() {
286 let ts = make_ts(&[0, 1, 4, 5]);
287 let vals = vec![10.0, 20.0, 50.0, 60.0];
288 let (_, out_vals, filled, _) = Interpolator::Nearest
289 .fill(&ts, &vals, 1_000_000_000)
290 .unwrap();
291 assert_eq!(filled, 2);
292 assert!((out_vals[2] - 20.0).abs() < 1e-6); assert!((out_vals[3] - 50.0).abs() < 1e-6); }
295
296 #[test]
297 fn zero_fill() {
298 let ts = make_ts(&[0, 3]);
299 let vals = vec![10.0, 40.0];
300 let (_, out_vals, filled, _) = Interpolator::Zero.fill(&ts, &vals, 1_000_000_000).unwrap();
301 assert_eq!(filled, 2);
302 assert!((out_vals[1]).abs() < 1e-10);
303 assert!((out_vals[2]).abs() < 1e-10);
304 }
305
306 #[test]
307 fn forward_fill() {
308 let ts = make_ts(&[0, 3]);
309 let vals = vec![10.0, 40.0];
310 let (_, out_vals, filled, _) = Interpolator::Forward
311 .fill(&ts, &vals, 1_000_000_000)
312 .unwrap();
313 assert_eq!(filled, 2);
314 assert!((out_vals[1] - 10.0).abs() < 1e-10);
315 assert!((out_vals[2] - 10.0).abs() < 1e-10);
316 }
317
318 #[test]
319 fn nocb_fill() {
320 let ts = make_ts(&[0, 3]);
321 let vals = vec![10.0, 40.0];
322 let (_, out_vals, filled, _) = Interpolator::Nocb.fill(&ts, &vals, 1_000_000_000).unwrap();
323 assert_eq!(filled, 2);
324 assert!((out_vals[1] - 40.0).abs() < 1e-10);
325 assert!((out_vals[2] - 40.0).abs() < 1e-10);
326 }
327
328 #[test]
329 fn spline_with_enough_points() {
330 let ts = make_ts(&[0, 1, 2, 5, 6, 7]);
331 let vals = vec![1.0, 2.0, 4.0, 10.0, 12.0, 15.0];
332 let (_, out_vals, filled, _) = Interpolator::CatmullRom
333 .fill(&ts, &vals, 1_000_000_000)
334 .unwrap();
335 assert_eq!(filled, 2); assert!(out_vals[3] > 4.0 && out_vals[3] < 10.0);
338 assert!(out_vals[4] > 4.0 && out_vals[4] < 10.0);
339 }
340
341 #[test]
342 fn no_gaps_passthrough() {
343 let ts = make_ts(&[0, 1, 2, 3]);
344 let vals = vec![1.0, 2.0, 3.0, 4.0];
345 let (out_ts, out_vals, filled, _) = Interpolator::Linear
346 .fill(&ts, &vals, 1_000_000_000)
347 .unwrap();
348 assert_eq!(filled, 0);
349 assert_eq!(out_ts, ts);
350 assert_eq!(out_vals, vals);
351 }
352
353 #[test]
354 fn empty_data() {
355 let (_, _, filled, _) = Interpolator::Linear.fill(&[], &[], 1_000_000_000).unwrap();
356 assert_eq!(filled, 0);
357 }
358
359 #[test]
360 fn catmull_rom_rejects_non_monotonic_timestamps() {
361 let ts = make_ts(&[0, 1, 1, 3, 4, 5]);
363 let vals = vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0];
364 let err = Interpolator::CatmullRom
365 .fill(&ts, &vals, 1_000_000_000)
366 .unwrap_err();
367 assert!(
368 matches!(
369 err,
370 InterpolationError::NonMonotonicTimestamps { index: 2, .. }
371 ),
372 "expected NonMonotonicTimestamps at index 2, got {err:?}"
373 );
374
375 let ts2 = make_ts(&[0, 1, 5, 3, 6, 7]);
377 let vals2 = vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0];
378 let err2 = Interpolator::CatmullRom
379 .fill(&ts2, &vals2, 1_000_000_000)
380 .unwrap_err();
381 assert!(
382 matches!(
383 err2,
384 InterpolationError::NonMonotonicTimestamps { index: 3, .. }
385 ),
386 "expected NonMonotonicTimestamps at index 3, got {err2:?}"
387 );
388 }
389
390 #[test]
391 fn catmull_rom_accepts_monotonic_timestamps() {
392 let ts = make_ts(&[0, 1, 2, 5, 6, 7]);
393 let vals = vec![1.0, 2.0, 4.0, 10.0, 12.0, 15.0];
394 let (out_ts, out_vals, filled, _) = Interpolator::CatmullRom
395 .fill(&ts, &vals, 1_000_000_000)
396 .unwrap();
397 assert_eq!(filled, 2);
398 assert!(out_ts.len() > ts.len());
399 assert!(out_vals[3] > 4.0 && out_vals[3] < 10.0);
401 assert!(out_vals[4] > 4.0 && out_vals[4] < 10.0);
402 }
403
404 #[test]
405 fn cq02_catmull_rom_monotonicity_preserved() {
406 let ts = make_ts(&[0, 1, 2, 5, 6, 7]);
408 let vals = vec![1.0, 3.0, 5.0, 20.0, 22.0, 24.0];
409 let (_, out_vals, filled, _) = Interpolator::CatmullRom
410 .fill(&ts, &vals, 1_000_000_000)
411 .unwrap();
412 assert_eq!(filled, 2);
413 for w in out_vals.windows(2) {
415 assert!(
416 w[1] >= w[0],
417 "monotonicity violated: {} followed by {}",
418 w[0],
419 w[1]
420 );
421 }
422 }
423
424 #[test]
425 fn cq03_gap_skipped_count_reported() {
426 let ts = vec![0i64, 200_001];
429 let vals = vec![1.0, 2.0];
430 let (_, _, filled, skipped) = Interpolator::Linear.fill(&ts, &vals, 1).unwrap();
431 assert_eq!(filled, 0, "should not fill a gap exceeding MAX_GAP_FILL");
432 assert_eq!(skipped, 1, "should report 1 skipped gap");
433 }
434}