1use std::collections::VecDeque;
4use std::f64::consts::{FRAC_PI_2, PI, SQRT_2, TAU};
5
6use super::peaks;
7use crate::fft::{fft_with, ifft_with, twiddles};
8use crate::stft::hann_periodic;
9
10pub const NEAR_CENTS: f64 = 25.0;
12pub const NEAR_HZ: f64 = 8.0;
13pub const PROMINENCE_DB: f64 = 6.0;
15pub const LEAK_MARGIN: f64 = 4.0;
18pub const GAP_SHARE: f64 = 0.4;
20pub const RESOLUTION_HZ: (f64, f64) = (6.0, 40.0);
21pub const LEAST_GAP_HZ: f64 = RESOLUTION_HZ.0 / GAP_SHARE;
23pub const SLOPE_SIGMAS: f64 = 4.0;
26pub const EDGE_SIGMAS: f64 = 3.0;
29pub const CAPTURE_SIGMAS: f64 = 2.0;
32pub const CANCELLATIONS: usize = 2;
36pub const KNOT_CENTS: f64 = 1.0;
39pub const KNOT_SHARE: f64 = 0.02;
41pub const FLUTTER_FRAME_SECS: f64 = 0.002;
44pub const FLUTTER_MEAN_FRAMES: usize = 15;
45pub const FLUTTER_SPAN_SECS: f64 = 0.2;
46pub const BREATHY_FLUTTER_DB: f64 = 2.0;
47pub const COHERENCE_LAG_SECS: f64 = 0.01;
50pub const COHERENCE_SPAN_SECS: f64 = 0.25;
51pub const COHERENCE_HALF_HZ: f64 = 15.0;
52pub const FLAG_LEAST_HZ: f64 = 20.0;
55pub const BREATHY_POWER_SECS: f64 = 0.03;
57const LEAST_BAND_RATE_HZ: f64 = 1000.0;
59const SIGMAS: f64 = 6.0;
61const PAD_SIGMAS: f64 = 12.0;
63const PROMINENCE_BINS: f64 = 4.0;
65
66pub const MODEL: &str = "heterodyne partial tracking, as the fit harness's analysis seed \
67 tracks, read without a score. Detect: every `hop_secs` from the \
68 target's start, the power of every periodic Hann frame of `frame_secs` \
69 centred within `window_secs` of samples (Welch, P. D. (1967), The use \
70 of fast Fourier transform for the estimation of power spectra, IEEE \
71 Trans. Audio Electroacoust. 15(2) 70-73), a frame past the target's \
72 ends spanning the samples it holds, zero-padded to at least its own \
73 length, over every channel's samples averaged, those frames' power \
74 averaged so a noise's chance peaks average away and a held partial's \
75 stay; its peaks the local maxima at or above the floor, in dB against \
76 full scale, standing 4 times over the Hann sidelobes the louder peaks \
77 kept and a DC put under them (`spectrum`'s bound, Harris 1978, with \
78 that margin for a partial that moves within a frame), 6 dB over the \
79 median bin within a sixth of an octave and 4 bins, and 2 frame bins \
80 from 0 Hz and Nyquist. Peaks link hop to hop into fragments, loudest \
81 first, each to the nearest open fragment within 25 cents or 8 Hz, a \
82 fragment open across gaps of half a frame. Fragments sorted by \
83 energy-weighted frequency join a group while within 25 cents or 8 Hz \
84 of its running mean, or 15 Hz, where the narrowest Gaussian below \
85 reads both as one band (one partial across notes; a glide is a group \
86 whose peaks move, which a window longer than the time it takes to \
87 cross a frame's resolution smears). The `peaks` groups of most energy \
88 are tracks; `detected` counts them all. Read: per track the target's \
89 analytic signal demodulated along the track's phase 2 pi \
90 pwl_integral(t, frequency), then a Gaussian lowpass of half-width \
91 sigma = `resolution_hz`, 0.4 of the gap to the nearest track's \
92 frequency clamped to 6-40 Hz, its time deviation `resolution_secs` = 1 \
93 / (2 pi sigma); all of it off one transform of the target zero-padded \
94 by 12 time deviations of the narrowest Gaussian, each band taken flat \
95 to 240 Hz past the track's frequencies, Gaussian past that, and \
96 decimated to at least its width, so the band is exact for every \
97 lowpass here. For a lone sinusoid of constant frequency and amplitude \
98 that band is a e^(i(phase - pi/2)) exactly, wherever the target \
99 reaches past the Gaussian's tails, about 6 time deviations: its \
100 per-hop frequency, amplitude and phase are exact there, and a \
101 neighbour d Hz away moves the band by at most its amplitude times \
102 e^(-d^2 / (2 sigma^2)). Frequency: the group's peaks per hop, plus the \
103 band's mean phase slope over 2 pi across 4 time deviations about the \
104 hop, where the band is over the floor, that span lies 3 time \
105 deviations inside the target (anywhere it sounds, where no hop does) \
106 and the slope is within 2 sigma, farther than which the band reads a \
107 neighbour's frequency and not its own; held between and outward. \
108 Cancel: then twice, every tonal track's band, Re z(t) e^(i 2 pi \
109 pwl_integral(t)), is taken from the target and each track read again \
110 off that residual plus its own band (Jacobi), so what a tonal \
111 neighbour leaks into a track's band is cancelled, the second pass \
112 cancelling what the first took of the track's own leak into the \
113 neighbour's; a breathy track stays in the residual. Tonal or breathy, \
114 as the harness read them: flutter is the dB spread of 2 ms means of \
115 the band's modulus about their 15-frame running mean, the 7 frames at \
116 either end left out, over the 200 ms from the track's loudest hop, or \
117 the target's last 200 ms where that is later; coherence |sum b(t)* b(t \
118 + 10 ms)| / sqrt(sum |b(t)|^2 sum |b(t + 10 ms)|^2) of the band \
119 brickwalled to max(sigma, 15 Hz) either side over the 250 ms so \
120 placed, 1 under three lags; the first read takes both at sigma, the \
121 cancelled reads at sigma or 20 Hz if wider, where a narrowband noise's \
122 envelope moves faster than the 30 ms running mean and a tone's, its \
123 tonal neighbours cancelled, holds. A tone's modulus holds over 2 ms \
124 frames and a narrowband noise's flutters (Rayleigh, some 5.6 dB): over \
125 2 dB is breathy, its frequency the energy-weighted mean, its \
126 `bandwidth_hz` the larger of -ln(coherence) / (pi 10 ms), a Lorentzian \
127 line's width, and 2 sigma / 3. A tonal track's amplitude is the band's \
128 modulus, the sinusoid's peak amplitude, and `phase` is the \
129 energy-weighted mean of its argument plus pi/2, so amplitude sin(2 pi \
130 pwl_integral(t, frequency) + phase) is the track; a breathy track's \
131 amplitude is its band's rms over 30 ms, and it has no phase. Knots: \
132 per hop an amplitude under the floor is zero, and each table is the \
133 fewest hops, greedily, through which straight lines stay within the \
134 tolerance of the per-hop curve: the amplitude within 2% of the track's \
135 peak or the floor's amplitude, from the track's first sounding hop \
136 less one to its last plus one; the frequency within 1 cent, or twice \
137 its own noise where wider (the median absolute second difference over \
138 0.6745 sqrt 6), where the track sounds, its knots then refit by least \
139 squares weighted by the band's energy. `error` is the largest gap a \
140 table leaves where it is held to the curve. The frequency table spans \
141 the whole target from its first hop, so pwl_integral's phase starts \
142 there. Bounds: `residual_db` is the energy the knot tables leave in a \
143 tonal track's band, the target less every tonal track's tables \
144 demodulated as above, over the energy its band holds, over the \
145 amplitude table's span, the whole of what the track misses there, \
146 noise, a neighbour's error and its own; null for a breathy track, a \
147 band whose waveform no table claims. A track whose group holds no peak \
148 for a while keeps the frequency it held, and where a louder neighbour \
149 within 2 sigma of it fills its band there it reads that neighbour, a \
150 second synthesis of one partial its `residual_db` shows near 0 dB. The \
151 target's `residual_db` is its energy less the tonal tracks' tables' \
152 over its energy, breathy energy left in.";
153
154#[derive(Clone, Copy, Debug, PartialEq)]
156pub struct Asked {
157 pub frame_secs: f64,
158 pub window_secs: f64,
160 pub hop_secs: f64,
161 pub floor_db: f64,
162 pub tracks: usize,
163}
164
165#[derive(Clone, Debug, PartialEq)]
167pub struct Curve {
168 pub times: Vec<f64>,
169 pub values: Vec<f64>,
170 pub error: f64,
171}
172
173#[derive(Clone, Copy, Debug, PartialEq, Eq)]
174pub enum Kind {
175 Tonal,
176 Breathy,
177}
178
179impl Kind {
180 pub fn key(self) -> &'static str {
181 match self {
182 Kind::Tonal => "tonal",
183 Kind::Breathy => "breathy",
184 }
185 }
186}
187
188#[derive(Clone, Debug, PartialEq)]
189pub struct Track {
190 pub kind: Kind,
191 pub hz: f64,
193 pub frequency: Curve,
194 pub amplitude: Curve,
195 pub phase: Option<f64>,
196 pub flutter_db: f64,
197 pub coherence: f64,
198 pub bandwidth_hz: Option<f64>,
199 pub resolution_hz: f64,
200 pub resolution_secs: f64,
201 pub residual_db: Option<f64>,
202}
203
204#[derive(Clone, Debug, PartialEq)]
205pub struct Partials {
206 pub frame_secs: f64,
207 pub window_secs: f64,
208 pub hop_secs: f64,
209 pub floor_db: f64,
210 pub detected: usize,
211 pub residual_db: Option<f64>,
212 pub tracks: Vec<Track>,
213}
214
215pub fn measure(planes: &[&[f64]], sr: f64, start: f64, asked: Asked) -> Partials {
217 let x = mono(planes);
218 let frame = ((asked.frame_secs * sr).round() as usize).max(2);
219 let hop = ((asked.hop_secs * sr).round() as usize).max(1);
220 let reach = ((asked.window_secs * sr - frame as f64).max(0.0) / (2 * hop) as f64).round();
221 let reach = reach as usize;
222 let floor = 10f64.powf(asked.floor_db / 20.0);
223 let grid = Grid {
224 sr,
225 samples: x.len(),
226 hop,
227 hops: x.len().saturating_sub(1).div_ceil(hop) + 1,
228 start,
229 };
230 let peaks = detect(&x, &grid, (frame, reach), (floor, asked.tracks));
231 let mut groups = group(link(&peaks, (frame / (2 * hop)).max(1)));
232 let detected = groups.len();
233 groups.sort_by(|a, b| b.energy.total_cmp(&a.energy).then(a.hz.total_cmp(&b.hz)));
234 groups.truncate(asked.tracks);
235 groups.sort_by(|a, b| a.hz.total_cmp(&b.hz));
236 let (tracks, residual_db) = read(&x, &grid, &groups, floor);
237 Partials {
238 frame_secs: frame as f64 / sr,
239 window_secs: (2 * reach * hop + frame) as f64 / sr,
240 hop_secs: hop as f64 / sr,
241 floor_db: asked.floor_db,
242 detected,
243 residual_db,
244 tracks,
245 }
246}
247
248fn mono(planes: &[&[f64]]) -> Vec<f64> {
249 let len = super::held(planes);
250 let count = planes.len() as f64;
251 (0..len)
252 .map(|n| planes.iter().map(|p| p[n]).sum::<f64>() / count)
253 .collect()
254}
255
256struct Grid {
257 sr: f64,
258 samples: usize,
260 hop: usize,
261 hops: usize,
262 start: f64,
263}
264
265impl Grid {
266 fn secs(&self, k: usize) -> f64 {
267 (k * self.hop) as f64 / self.sr
268 }
269}
270
271struct Plan {
273 twiddle: Vec<(f64, f64)>,
274}
275
276impl Plan {
277 fn new(n: usize) -> Plan {
278 Plan {
279 twiddle: twiddles(n),
280 }
281 }
282
283 fn forward(&self, re: &mut [f64], im: &mut [f64]) {
284 fft_with(re, im, &self.twiddle);
285 }
286
287 fn inverse(&self, re: &mut [f64], im: &mut [f64]) {
288 ifft_with(re, im, &self.twiddle);
289 }
290}
291
292#[derive(Clone, Copy)]
293struct Found {
294 hz: f64,
295 amp: f64,
296}
297
298type Power = Option<(Vec<f64>, usize)>;
301
302fn detect(
304 x: &[f64],
305 grid: &Grid,
306 (frame, reach): (usize, usize),
307 (floor, cap): (f64, usize),
308) -> Vec<Vec<Found>> {
309 let size = frame.next_power_of_two();
310 let plan = Plan::new(size);
311 let bin_hz = grid.sr / size as f64;
312 let span = |k: usize| {
313 let centre = k * grid.hop;
314 let from = centre.saturating_sub(frame / 2).min(x.len());
315 (from, (centre + frame.div_ceil(2)).min(x.len()) - from)
316 };
317 let (mut re, mut im) = (vec![0.0; size], vec![0.0; size]);
318 let mut pair = |k: usize| -> (Power, Power) {
319 re.iter_mut().for_each(|v| *v = 0.0);
320 im.iter_mut().for_each(|v| *v = 0.0);
321 let ((a, na), (b, nb)) = (span(k), span(k + 1));
322 if na >= 2 {
323 for (i, w) in hann_periodic(na).iter().enumerate() {
324 re[i] = x[a + i] * w;
325 }
326 }
327 if k + 1 < grid.hops && nb >= 2 {
328 for (i, w) in hann_periodic(nb).iter().enumerate() {
329 im[i] = x[b + i] * w;
330 }
331 }
332 plan.forward(&mut re, &mut im);
333 let unpacked = |n: usize, second: bool| {
334 (n >= 2).then(|| {
335 let scale = (4.0 / n as f64).powi(2);
336 let bins = (0..=size / 2).map(|j| {
337 let (zr, zi, wr, wi) =
338 (re[j], im[j], re[(size - j) % size], im[(size - j) % size]);
339 let (pr, pi) = match second {
340 false => (zr + wr, zi - wi),
341 true => (zi + wi, wr - zr),
342 };
343 (pr * pr + pi * pi) * 0.25 * scale
344 });
345 (bins.collect(), n)
346 })
347 };
348 let second = (k + 1 < grid.hops).then(|| unpacked(nb, true)).flatten();
349 (unpacked(na, false), second)
350 };
351 let mut held: VecDeque<(usize, Power)> = VecDeque::new();
352 let mut spare: Option<Power> = None;
353 let mut sum = vec![0.0; size / 2 + 1];
354 let (mut count, mut next) = (0usize, 0usize);
355 let mut out = Vec::with_capacity(grid.hops);
356 for k in 0..grid.hops {
357 while next < grid.hops && next <= k + reach {
358 let p = match spare.take() {
359 Some(p) => p,
360 None => {
361 let (first, second) = pair(next);
362 spare = Some(second);
363 first
364 }
365 };
366 if let Some((p, _)) = &p {
367 sum.iter_mut().zip(p).for_each(|(a, b)| *a += b);
368 count += 1;
369 }
370 held.push_back((next, p));
371 next += 1;
372 }
373 while held.front().is_some_and(|(j, _)| j + reach < k) {
374 if let Some((_, Some((p, _)))) = held.pop_front() {
375 sum.iter_mut().zip(&p).for_each(|(a, b)| *a -= b);
376 count -= 1;
377 }
378 }
379 let spanned = held
380 .iter()
381 .filter_map(|(_, p)| p.as_ref().map(|p| p.1))
382 .min();
383 let (Some(spanned), true) = (spanned, count > 0) else {
384 out.push(Vec::new());
385 continue;
386 };
387 let mags: Vec<f64> = sum
388 .iter()
389 .map(|p| (p.max(0.0) / count as f64).sqrt())
390 .collect();
391 out.push(picked(&mags, (bin_hz, grid.sr), spanned, (floor, cap)));
392 }
393 out
394}
395
396fn picked(
400 mags: &[f64],
401 (bin_hz, sr): (f64, f64),
402 spanned: usize,
403 (floor, cap): (f64, usize),
404) -> Vec<Found> {
405 let r = spanned as f64 / (2 * (mags.len() - 1)) as f64;
406 let half = r / 2.0;
407 let dc = match mags {
408 [zero, one, ..] if zero >= one => *zero,
409 _ => 0.0,
410 };
411 let edge = 2.0 * sr / spanned as f64;
412 let mut kept: Vec<(usize, Found)> = Vec::new();
413 for (k, p) in peaks::located(mags, bin_hz) {
414 let amp = 10f64.powf(p.db / 20.0);
415 if amp < floor || kept.len() == 4 * cap {
416 break;
417 }
418 let under = |q: usize| {
419 let near = (k.abs_diff(q) as f64 * r - half).max(0.0);
420 let image = (k + q) as f64 * r - half;
421 mags[q] * (peaks::leakage(near) + peaks::leakage(image))
422 };
423 let leaked = kept.iter().map(|&(q, _)| under(q)).sum::<f64>()
424 + dc * peaks::leakage(k as f64 * r - half);
425 if mags[k] > LEAK_MARGIN * leaked / peaks::kernel(half) {
426 kept.push((k, Found { hz: p.hz, amp }));
427 }
428 }
429 kept.into_iter()
430 .map(|(_, p)| p)
431 .filter(|p| p.hz >= edge && p.hz <= sr / 2.0 - edge && prominent(mags, bin_hz, p))
432 .collect()
433}
434
435fn prominent(mags: &[f64], bin_hz: f64, p: &Found) -> bool {
436 let lo = (p.hz * 2f64.powf(-1.0 / 6.0) / bin_hz - PROMINENCE_BINS).max(0.0) as usize;
437 let hi = (p.hz * 2f64.powf(1.0 / 6.0) / bin_hz + PROMINENCE_BINS) as usize;
438 let mut near: Vec<f64> = mags[lo..=hi.min(mags.len() - 1)].to_vec();
439 let middle = near.len() / 2;
440 let (_, median, _) = near.select_nth_unstable_by(middle, f64::total_cmp);
441 p.amp >= *median * 10f64.powf(PROMINENCE_DB / 20.0)
442}
443
444fn cents(a: f64, b: f64) -> f64 {
445 1200.0 * (a / b).log2()
446}
447
448fn near(a: f64, b: f64) -> bool {
449 cents(a, b).abs() < NEAR_CENTS || (a - b).abs() < NEAR_HZ
450}
451
452struct Fragment {
453 hops: Vec<usize>,
454 hz: Vec<f64>,
455 amp: Vec<f64>,
456}
457
458impl Fragment {
459 fn last_hz(&self) -> f64 {
460 self.hz[self.hz.len() - 1]
461 }
462}
463
464fn link(peaks: &[Vec<Found>], gap: usize) -> Vec<Fragment> {
465 let mut all: Vec<Fragment> = Vec::new();
466 let mut open: Vec<usize> = Vec::new();
467 for (k, found) in peaks.iter().enumerate() {
468 open.retain(|&f| k - all[f].hops[all[f].hops.len() - 1] <= gap);
469 let mut order: Vec<&Found> = found.iter().collect();
470 order.sort_by(|a, b| b.amp.total_cmp(&a.amp).then(a.hz.total_cmp(&b.hz)));
471 let mut taken: Vec<usize> = Vec::new();
472 for p in order {
473 let off = |f: usize| cents(p.hz, all[f].last_hz()).abs();
474 let best = open
475 .iter()
476 .copied()
477 .filter(|f| !taken.contains(f) && near(p.hz, all[*f].last_hz()))
478 .min_by(|&a, &b| off(a).total_cmp(&off(b)).then(a.cmp(&b)));
479 let f = best.unwrap_or_else(|| {
480 all.push(Fragment {
481 hops: Vec::new(),
482 hz: Vec::new(),
483 amp: Vec::new(),
484 });
485 open.push(all.len() - 1);
486 all.len() - 1
487 });
488 all[f].hops.push(k);
489 all[f].hz.push(p.hz);
490 all[f].amp.push(p.amp);
491 taken.push(f);
492 }
493 }
494 all
495}
496
497struct Group {
498 hz: f64,
499 energy: f64,
500 members: Vec<Fragment>,
501}
502
503fn group(fragments: Vec<Fragment>) -> Vec<Group> {
504 let mut weighed: Vec<(f64, f64, Fragment)> = fragments
505 .into_iter()
506 .map(|f| {
507 let energy: f64 = f.amp.iter().map(|a| a * a).sum();
508 let hz = f.hz.iter().zip(&f.amp).map(|(h, a)| h * a * a).sum::<f64>() / energy;
509 (hz, energy, f)
510 })
511 .collect();
512 weighed.sort_by(|a, b| a.0.total_cmp(&b.0).then(a.2.hops[0].cmp(&b.2.hops[0])));
513 let mut groups: Vec<Group> = Vec::new();
514 for (hz, energy, f) in weighed {
515 match groups.last_mut() {
516 Some(held) if near(hz, held.hz) || hz - held.hz < LEAST_GAP_HZ => {
517 held.hz = (held.hz * held.energy + hz * energy) / (held.energy + energy);
518 held.energy += energy;
519 held.members.push(f);
520 }
521 _ => groups.push(Group {
522 hz,
523 energy,
524 members: vec![f],
525 }),
526 }
527 }
528 groups
529}
530
531fn coarse(g: &Group, hops: usize) -> Vec<f64> {
533 let mut sum = vec![0.0; hops];
534 let mut weight = vec![0.0; hops];
535 for f in &g.members {
536 for ((k, hz), a) in f.hops.iter().zip(&f.hz).zip(&f.amp) {
537 sum[*k] += hz * a * a;
538 weight[*k] += a * a;
539 }
540 }
541 let held: Vec<Option<f64>> = sum
542 .iter()
543 .zip(&weight)
544 .map(|(s, w)| (*w > 0.0).then(|| s / w))
545 .collect();
546 filled(&held, g.hz)
547}
548
549fn filled(held: &[Option<f64>], otherwise: f64) -> Vec<f64> {
551 let known: Vec<(usize, f64)> = held
552 .iter()
553 .enumerate()
554 .filter_map(|(k, v)| v.map(|v| (k, v)))
555 .collect();
556 let (Some(&first), Some(&last)) = (known.first(), known.last()) else {
557 return vec![otherwise; held.len()];
558 };
559 let mut next = 0;
560 (0..held.len())
561 .map(|k| {
562 while next < known.len() && known[next].0 < k {
563 next += 1;
564 }
565 match (held[k], next) {
566 (Some(v), _) => v,
567 (None, 0) => first.1,
568 (None, n) if n == known.len() => last.1,
569 (None, n) => {
570 let ((a, va), (b, vb)) = (known[n - 1], known[n]);
571 va + (vb - va) * (k - a) as f64 / (b - a) as f64
572 }
573 }
574 })
575 .collect()
576}
577
578struct Walk<'a> {
581 times: &'a [f64],
582 values: &'a [f64],
583 prefix: Vec<f64>,
584 at: usize,
585}
586
587impl<'a> Walk<'a> {
588 fn new(times: &'a [f64], values: &'a [f64]) -> Walk<'a> {
589 let mut prefix = vec![0.0];
590 for k in 1..times.len() {
591 let dt = times[k] - times[k - 1];
592 prefix.push(prefix[k - 1] + dt * (values[k] + values[k - 1]) * 0.5);
593 }
594 Walk {
595 times,
596 values,
597 prefix,
598 at: 0,
599 }
600 }
601
602 fn segment(&mut self, t: f64) -> usize {
604 let last = self.times.len() - 1;
605 if t < self.times[self.at] {
606 self.at = self.times.partition_point(|&s| s <= t).saturating_sub(1);
607 }
608 while self.at < last && self.times[self.at + 1] <= t {
609 self.at += 1;
610 }
611 self.at
612 }
613
614 fn value(&mut self, t: f64) -> f64 {
615 let k = self.segment(t);
616 if t <= self.times[0] || k == self.times.len() - 1 {
617 return self.values[k];
618 }
619 let u = (t - self.times[k]) / (self.times[k + 1] - self.times[k]);
620 self.values[k] + (self.values[k + 1] - self.values[k]) * u
621 }
622
623 fn integral(&mut self, t: f64) -> f64 {
624 let k = self.segment(t);
625 let u = t - self.times[k];
626 if t <= self.times[0] || k == self.times.len() - 1 {
627 return self.prefix[k] + self.values[k] * u;
628 }
629 let slope = (self.values[k + 1] - self.values[k]) / (self.times[k + 1] - self.times[k]);
630 self.prefix[k] + u * (self.values[k] + 0.5 * slope * u)
631 }
632}
633
634#[derive(Clone, Copy)]
635struct C {
636 re: f64,
637 im: f64,
638}
639
640const ZERO: C = C { re: 0.0, im: 0.0 };
641
642impl C {
643 fn of_turns(turns: f64) -> C {
644 let (im, re) = (TAU * turns.rem_euclid(1.0)).sin_cos();
645 C { re, im }
646 }
647 fn mul(self, o: C) -> C {
648 C {
649 re: self.re * o.re - self.im * o.im,
650 im: self.re * o.im + self.im * o.re,
651 }
652 }
653 fn scale(self, s: f64) -> C {
654 C {
655 re: self.re * s,
656 im: self.im * s,
657 }
658 }
659 fn add(self, o: C) -> C {
660 C {
661 re: self.re + o.re,
662 im: self.im + o.im,
663 }
664 }
665 fn conj(self) -> C {
666 C {
667 re: self.re,
668 im: -self.im,
669 }
670 }
671 fn norm(self) -> f64 {
672 self.re * self.re + self.im * self.im
673 }
674 fn abs(self) -> f64 {
675 self.norm().sqrt()
676 }
677 fn arg(self) -> f64 {
678 self.im.atan2(self.re)
679 }
680}
681
682struct Whole {
684 re: Vec<f64>,
685 im: Vec<f64>,
686}
687
688impl Whole {
689 fn of(x: &[f64], plan: &Plan, size: usize) -> Whole {
690 let mut re = vec![0.0; size];
691 let mut im = vec![0.0; size];
692 re[..x.len()].copy_from_slice(x);
693 plan.forward(&mut re, &mut im);
694 Whole { re, im }
695 }
696}
697
698#[derive(Clone, Copy)]
701struct Band {
702 every: usize,
703 centre_bin: usize,
704 lo_hz: f64,
705 hi_hz: f64,
706 sigma: f64,
707}
708
709struct Reader<'a> {
711 grid: &'a Grid,
712 size: usize,
713 whole: Plan,
714 bands: Vec<(usize, Plan)>,
715 floor: f64,
716 hop_secs: Vec<f64>,
717}
718
719impl Reader<'_> {
720 fn rate(&self, b: &Band) -> f64 {
721 self.grid.sr / b.every as f64
722 }
723
724 fn centre_hz(&self, b: &Band) -> f64 {
725 b.centre_bin as f64 * self.grid.sr / self.size as f64
726 }
727
728 fn plan(&self, m: usize) -> &Plan {
729 let (_, plan) = self
730 .bands
731 .iter()
732 .find(|(n, _)| *n == m)
733 .expect("every band length was planned");
734 plan
735 }
736
737 fn band(&self, whole: &Whole, b: &Band) -> Vec<C> {
740 let n = self.size;
741 let m = n / b.every;
742 let bin_hz = self.grid.sr / n as f64;
743 let widest = RESOLUTION_HZ.1;
744 let flat = (b.lo_hz - SIGMAS * widest, b.hi_hz + SIGMAS * widest);
745 let first = ((flat.0 - SIGMAS * widest) / bin_hz).floor().max(0.0) as usize;
746 let last = (((flat.1 + SIGMAS * widest) / bin_hz).ceil() as usize).min(n / 2);
747 let (mut re, mut im) = (vec![0.0; m], vec![0.0; m]);
748 let gain = 2.0 * m as f64 / n as f64;
749 for bin in first..=last {
750 let f = bin as f64 * bin_hz;
751 let off = (flat.0 - f).max(f - flat.1).max(0.0);
752 let w = (-0.5 * (off / widest).powi(2)).exp() * gain;
753 let j = (bin as isize - b.centre_bin as isize).rem_euclid(m as isize) as usize;
754 re[j] += whole.re[bin] * w;
755 im[j] += whole.im[bin] * w;
756 }
757 self.plan(m).inverse(&mut re, &mut im);
758 re.into_iter()
759 .zip(im)
760 .map(|(re, im)| C { re, im })
761 .collect()
762 }
763
764 fn mixed(&self, raw: &[C], b: &Band, (times, values): (&[f64], &[f64])) -> Vec<C> {
766 let (centre, rate) = (self.centre_hz(b), self.rate(b));
767 let mut walk = Walk::new(times, values);
768 raw.iter()
769 .enumerate()
770 .map(|(m, z)| {
771 let t = m as f64 / rate;
772 z.mul(C::of_turns(centre * t - walk.integral(t)))
773 })
774 .collect()
775 }
776
777 fn spectrum(&self, z: &[C]) -> (Vec<f64>, Vec<f64>) {
779 let mut re: Vec<f64> = z.iter().map(|c| c.re).collect();
780 let mut im: Vec<f64> = z.iter().map(|c| c.im).collect();
781 self.plan(z.len()).forward(&mut re, &mut im);
782 (re, im)
783 }
784
785 fn lowpass(
787 &self,
788 (re, im): &(Vec<f64>, Vec<f64>),
789 rate: f64,
790 keep: impl Fn(f64) -> f64,
791 ) -> Vec<C> {
792 let m = re.len();
793 let (mut re, mut im) = (re.clone(), im.clone());
794 for j in 0..m {
795 let signed = if j <= m / 2 {
796 j as f64
797 } else {
798 j as f64 - m as f64
799 };
800 let w = keep(signed * rate / m as f64);
801 re[j] *= w;
802 im[j] *= w;
803 }
804 self.plan(m).inverse(&mut re, &mut im);
805 re.into_iter()
806 .zip(im)
807 .map(|(re, im)| C { re, im })
808 .collect()
809 }
810}
811
812fn gaussian(sigma: f64) -> impl Fn(f64) -> f64 {
813 move |f| (-0.5 * (f / sigma).powi(2)).exp()
814}
815
816fn brickwall(half: f64) -> impl Fn(f64) -> f64 {
817 move |f| if f.abs() < half { 1.0 } else { 0.0 }
818}
819
820struct Reading {
822 band: Band,
823 freq: Curve,
824 kind: Kind,
825 amp: Curve,
826 phase: f64,
827 flutter_db: f64,
828 coherence: f64,
829 hz: f64,
830 z: Vec<C>,
832}
833
834fn read(x: &[f64], grid: &Grid, groups: &[Group], floor: f64) -> (Vec<Track>, Option<f64>) {
835 if groups.is_empty() {
836 return (Vec::new(), energy_ratio(x, x));
837 }
838 let curves: Vec<Vec<f64>> = groups.iter().map(|g| coarse(g, grid.hops)).collect();
839 let sigmas: Vec<f64> = groups
840 .iter()
841 .map(|g| {
842 let gap = groups
843 .iter()
844 .filter(|o| !std::ptr::eq(*o, g))
845 .map(|o| (g.hz - o.hz).abs())
846 .fold(f64::MAX, f64::min);
847 (GAP_SHARE * gap).clamp(RESOLUTION_HZ.0, RESOLUTION_HZ.1)
848 })
849 .collect();
850 let narrowest = sigmas.iter().copied().fold(f64::MAX, f64::min);
851 let pad = (PAD_SIGMAS / (TAU * narrowest) * grid.sr).ceil() as usize;
852 let size = (x.len() + pad).next_power_of_two();
853 let bands: Vec<Band> = curves
854 .iter()
855 .zip(&sigmas)
856 .map(|(curve, &sigma)| {
857 let lo = curve.iter().copied().fold(f64::MAX, f64::min);
858 let hi = curve.iter().copied().fold(0f64, f64::max);
859 let width = (hi - lo + 4.0 * SIGMAS * RESOLUTION_HZ.1).max(LEAST_BAND_RATE_HZ);
860 let mut every = 1usize;
861 while grid.hop.is_multiple_of(2 * every)
862 && size.is_multiple_of(2 * every)
863 && grid.sr / (2 * every) as f64 >= width
864 {
865 every *= 2;
866 }
867 Band {
868 every,
869 centre_bin: ((lo + hi) / 2.0 * size as f64 / grid.sr).round() as usize,
870 lo_hz: lo,
871 hi_hz: hi,
872 sigma,
873 }
874 })
875 .collect();
876 let mut lengths: Vec<usize> = bands.iter().map(|b| size / b.every).collect();
877 lengths.sort_unstable();
878 lengths.dedup();
879 let reader = Reader {
880 grid,
881 size,
882 whole: Plan::new(size),
883 bands: lengths.into_iter().map(|m| (m, Plan::new(m))).collect(),
884 floor,
885 hop_secs: (0..grid.hops).map(|k| grid.secs(k)).collect(),
886 };
887
888 let first = Whole::of(x, &reader.whole, size);
889 let mut readings: Vec<Reading> = bands
890 .iter()
891 .zip(&curves)
892 .map(|(b, curve)| read_one(&reader, reader.band(&first, b), *b, (curve, b.sigma)))
893 .collect();
894 for _ in 0..CANCELLATIONS {
895 let left: Vec<f64> = x
896 .iter()
897 .zip(bands_of(&readings, grid, x.len()))
898 .map(|(a, s)| a - s)
899 .collect();
900 let left = Whole::of(&left, &reader.whole, size);
901 readings = readings
902 .iter()
903 .zip(&curves)
904 .map(|(r, curve)| {
905 let again = add(&reader.band(&left, &r.band), &own(&reader, r));
906 let curve = match r.kind {
907 Kind::Tonal => curve.clone(),
908 Kind::Breathy => {
909 let mut held = Walk::new(&r.freq.times, &r.freq.values);
910 reader.hop_secs.iter().map(|t| held.value(*t)).collect()
911 }
912 };
913 let flags = r.band.sigma.max(FLAG_LEAST_HZ);
914 read_one(&reader, again, r.band, (&curve, flags))
915 })
916 .collect();
917 }
918
919 let left: Vec<f64> = x
920 .iter()
921 .zip(synthesis(&readings, grid, x.len()))
922 .map(|(a, s)| a - s)
923 .collect();
924 let whole_left = Whole::of(&left, &reader.whole, size);
925 let tracks = readings
926 .iter()
927 .filter(|r| r.amp.values.iter().any(|v| *v > 0.0))
928 .map(|r| {
929 let residual_db = (r.kind == Kind::Tonal)
930 .then(|| {
931 let mixed = reader.mixed(
932 &reader.band(&whole_left, &r.band),
933 &r.band,
934 (&r.freq.times, &r.freq.values),
935 );
936 let rest = reader.lowpass(
937 &reader.spectrum(&mixed),
938 reader.rate(&r.band),
939 gaussian(r.band.sigma),
940 );
941 let span = r.span(&reader);
942 let held: f64 = r.z[span.clone()].iter().map(|c| c.norm()).sum();
943 let lost: f64 = rest[span].iter().map(|c| c.norm()).sum();
944 (held > 0.0).then(|| 10.0 * (lost / held).log10())
945 })
946 .flatten();
947 track(r, grid.start, residual_db)
948 })
949 .collect();
950 (tracks, energy_ratio(&left, x))
951}
952
953fn track(r: &Reading, start: f64, residual_db: Option<f64>) -> Track {
954 let shifted = |c: &Curve| Curve {
955 times: c.times.iter().map(|t| t + start).collect(),
956 ..c.clone()
957 };
958 Track {
959 kind: r.kind,
960 hz: r.hz,
961 frequency: shifted(&r.freq),
962 amplitude: shifted(&r.amp),
963 phase: (r.kind == Kind::Tonal).then_some(r.phase),
964 flutter_db: r.flutter_db,
965 coherence: r.coherence,
966 bandwidth_hz: (r.kind == Kind::Breathy).then(|| {
967 let lorentz = -r.coherence.max(1e-3).ln() / (PI * COHERENCE_LAG_SECS);
968 lorentz.max(2.0 * r.band.sigma / 3.0)
969 }),
970 resolution_hz: r.band.sigma,
971 resolution_secs: 1.0 / (TAU * r.band.sigma),
972 residual_db,
973 }
974}
975
976fn energy_ratio(rest: &[f64], whole: &[f64]) -> Option<f64> {
977 let held: f64 = whole.iter().map(|v| v * v).sum();
978 let lost: f64 = rest.iter().map(|v| v * v).sum();
979 (held > 0.0).then(|| 10.0 * (lost / held).log10())
980}
981
982impl Reading {
983 fn span(&self, reader: &Reader) -> std::ops::Range<usize> {
985 let rate = reader.rate(&self.band);
986 let at = |t: f64| ((t * rate).round() as usize).min(self.z.len() - 1);
987 let (a, b) = (self.amp.times[0], self.amp.times[self.amp.times.len() - 1]);
988 at(a)..at(b) + 1
989 }
990}
991
992fn own(reader: &Reader, r: &Reading) -> Vec<C> {
996 if r.kind == Kind::Breathy {
997 return vec![ZERO; r.z.len()];
998 }
999 let (centre, rate) = (reader.centre_hz(&r.band), reader.rate(&r.band));
1000 let mut walk = Walk::new(&r.freq.times, &r.freq.values);
1001 r.z.iter()
1002 .enumerate()
1003 .map(|(m, z)| {
1004 let t = m as f64 / rate;
1005 z.mul(C::of_turns(walk.integral(t) - centre * t))
1006 })
1007 .collect()
1008}
1009
1010fn bands_of(readings: &[Reading], grid: &Grid, n: usize) -> Vec<f64> {
1013 let mut out = vec![0.0; n];
1014 for r in readings.iter().filter(|r| r.kind == Kind::Tonal) {
1015 let mut walk = Walk::new(&r.freq.times, &r.freq.values);
1016 let every = r.band.every as f64;
1017 let last = r.z.len() - 1;
1018 for (i, slot) in out.iter_mut().enumerate() {
1019 let at = i as f64 / every;
1020 let m = (at as usize).min(last);
1021 let u = at - m as f64;
1022 let z = r.z[m].scale(1.0 - u).add(r.z[(m + 1).min(last)].scale(u));
1023 let e = C::of_turns(walk.integral(i as f64 / grid.sr));
1024 *slot += z.re * e.re - z.im * e.im;
1025 }
1026 }
1027 out
1028}
1029
1030fn synthesis(readings: &[Reading], grid: &Grid, n: usize) -> Vec<f64> {
1032 let mut out = vec![0.0; n];
1033 for r in readings.iter().filter(|r| r.kind == Kind::Tonal) {
1034 let mut phase = Walk::new(&r.freq.times, &r.freq.values);
1035 let mut amp = Walk::new(&r.amp.times, &r.amp.values);
1036 let (a0, a1) = (r.amp.times[0], r.amp.times[r.amp.times.len() - 1]);
1037 let from = ((a0 * grid.sr).ceil().max(0.0) as usize).min(n);
1038 let to = (((a1 * grid.sr).floor() as usize) + 1).min(n);
1039 for (i, slot) in out.iter_mut().enumerate().take(to).skip(from) {
1040 let t = i as f64 / grid.sr;
1041 let turns = phase.integral(t).rem_euclid(1.0);
1042 *slot += amp.value(t) * (TAU * turns + r.phase).sin();
1043 }
1044 }
1045 out
1046}
1047
1048fn add(a: &[C], b: &[C]) -> Vec<C> {
1049 a.iter().zip(b).map(|(x, y)| x.add(*y)).collect()
1050}
1051
1052fn read_one(reader: &Reader, raw: Vec<C>, b: Band, (curve, flag_sigma): (&[f64], f64)) -> Reading {
1055 let (freq, mean_hz) = refined(reader, &raw, b, curve);
1056 let r = measured(reader, &raw, b, freq, (None, Err(flag_sigma)));
1057 let r = match r.kind {
1058 Kind::Tonal => r,
1059 Kind::Breathy => {
1060 let ends = reader.hop_secs[reader.hop_secs.len() - 1].max(1.0 / reader.grid.sr);
1061 let held = Curve {
1062 times: vec![0.0, ends],
1063 values: vec![mean_hz; 2],
1064 error: 0.0,
1065 };
1066 let flags = Ok((r.flutter_db, r.coherence));
1067 measured(reader, &raw, b, held, (Some(Kind::Breathy), flags))
1068 }
1069 };
1070 Reading { hz: mean_hz, ..r }
1071}
1072
1073fn refined(reader: &Reader, raw: &[C], b: Band, along: &[f64]) -> (Curve, f64) {
1076 let (grid, hops) = (reader.grid, &reader.hop_secs);
1077 let rate = reader.rate(&b);
1078 let per_hop = grid.hop / b.every;
1079 let mixed = reader.mixed(raw, &b, (hops, along));
1080 let zc = reader.lowpass(&reader.spectrum(&mixed), rate, gaussian(b.sigma));
1081 let at_hop = |k: usize| zc[(k * per_hop).min(zc.len() - 1)];
1082 let sounding: Vec<bool> = (0..grid.hops)
1083 .map(|k| at_hop(k).abs() >= reader.floor)
1084 .collect();
1085 let mut turns = vec![0.0; zc.len()];
1086 for m in 1..zc.len() {
1087 turns[m] = turns[m - 1] + zc[m].mul(zc[m - 1].conj()).arg() / TAU;
1088 }
1089 let deviation = rate / (TAU * b.sigma);
1090 let reach = ((SLOPE_SIGMAS * deviation / 2.0).round() as usize)
1091 .max(per_hop.div_ceil(2))
1092 .max(1);
1093 let edge = (EDGE_SIGMAS * deviation).ceil() as usize + reach;
1094 let end = grid.samples.div_ceil(b.every);
1095 let inside = |k: usize| k * per_hop >= edge && k * per_hop + edge <= end;
1096 let counted: Vec<bool> = match (0..grid.hops).any(|k| sounding[k] && inside(k)) {
1097 true => (0..grid.hops).map(|k| sounding[k] && inside(k)).collect(),
1098 false => sounding.clone(),
1099 };
1100 let slope = |k: usize| {
1101 let lo = (k * per_hop).saturating_sub(reach);
1102 let hi = (k * per_hop + reach).min(zc.len() - 1).max(lo + 1);
1103 (turns[hi] - turns[lo]) * rate / (hi - lo) as f64
1104 };
1105 let counted: Vec<bool> = (0..grid.hops)
1106 .map(|k| counted[k] && slope(k).abs() <= CAPTURE_SIGMAS * b.sigma)
1107 .collect();
1108 let held: Vec<Option<f64>> = (0..grid.hops)
1109 .map(|k| counted[k].then(|| along[k] + slope(k)))
1110 .collect();
1111 let refined = filled(&held, along[0]);
1112 let weights: Vec<f64> = (0..grid.hops)
1113 .map(|k| if counted[k] { at_hop(k).norm() } else { 0.0 })
1114 .collect();
1115 let total: f64 = weights.iter().sum();
1116 let mean_hz = match total > 0.0 {
1117 true => {
1118 refined
1119 .iter()
1120 .zip(&weights)
1121 .map(|(f, w)| f * w)
1122 .sum::<f64>()
1123 / total
1124 }
1125 false => refined[0],
1126 };
1127 let cent = mean_hz * (2f64.powf(KNOT_CENTS / 1200.0) - 1.0);
1128 let tolerance = cent.max(2.0 * spread(&refined, &counted));
1129 let table = knots(hops, &refined, &counted, tolerance);
1130 (refit(table, hops, &refined, &weights), mean_hz)
1131}
1132
1133fn measured(
1136 reader: &Reader,
1137 raw: &[C],
1138 b: Band,
1139 freq: Curve,
1140 (kind, flags): (Option<Kind>, Result<(f64, f64), f64>),
1141) -> Reading {
1142 let grid = reader.grid;
1143 let rate = reader.rate(&b);
1144 let per_hop = grid.hop / b.every;
1145 let spectrum = reader.spectrum(&reader.mixed(raw, &b, (&freq.times, &freq.values)));
1146 let z = reader.lowpass(&spectrum, rate, gaussian(b.sigma));
1147 let at_hop = |k: usize| z[(k * per_hop).min(z.len() - 1)];
1148 let moduli: Vec<f64> = (0..grid.hops).map(|k| at_hop(k).abs()).collect();
1149 let loudest = (0..grid.hops)
1150 .max_by(|&a, &b| moduli[a].total_cmp(&moduli[b]).then(b.cmp(&a)))
1151 .unwrap_or(0);
1152 let (flutter_db, coherence) = flags.unwrap_or_else(|flag_sigma| {
1153 let held = grid.samples.div_ceil(b.every).min(z.len());
1154 let from = |secs: f64| (loudest * per_hop).min(held.saturating_sub((secs * rate) as usize));
1155 let flagged = match flag_sigma == b.sigma {
1156 true => z.clone(),
1157 false => reader.lowpass(&spectrum, rate, gaussian(flag_sigma)),
1158 };
1159 let banded = reader.lowpass(
1160 &spectrum,
1161 rate,
1162 brickwall(flag_sigma.max(COHERENCE_HALF_HZ)),
1163 );
1164 (
1165 flutter(&flagged[..held], rate, from(FLUTTER_SPAN_SECS)),
1166 coherence(&banded[..held], rate, from(COHERENCE_SPAN_SECS)),
1167 )
1168 });
1169 let kind = kind.unwrap_or(match flutter_db > BREATHY_FLUTTER_DB {
1170 true => Kind::Breathy,
1171 false => Kind::Tonal,
1172 });
1173 let amps: Vec<f64> = match kind {
1174 Kind::Tonal => moduli
1175 .iter()
1176 .map(|&a| if a < reader.floor { 0.0 } else { a })
1177 .collect(),
1178 Kind::Breathy => {
1179 let half = ((BREATHY_POWER_SECS / 2.0) * rate).round() as usize;
1180 (0..grid.hops)
1181 .map(|k| {
1182 let c = (k * per_hop).min(z.len() - 1);
1183 let (lo, hi) = (c.saturating_sub(half), (c + half).min(z.len() - 1));
1184 let power =
1185 z[lo..=hi].iter().map(|v| v.norm()).sum::<f64>() / (hi - lo + 1) as f64;
1186 let rms = (power / 2.0).sqrt();
1187 if rms * SQRT_2 < reader.floor {
1188 0.0
1189 } else {
1190 rms
1191 }
1192 })
1193 .collect()
1194 }
1195 };
1196 let amp = amplitude_knots((&reader.hop_secs, grid.secs(1)), &s, reader.floor);
1197 let weighted = (0..grid.hops)
1198 .filter(|&k| amps[k] > 0.0)
1199 .map(|k| at_hop(k).scale(at_hop(k).abs()))
1200 .fold(ZERO, C::add);
1201 Reading {
1202 band: b,
1203 freq,
1204 kind,
1205 amp,
1206 phase: (weighted.arg() + FRAC_PI_2).rem_euclid(TAU),
1207 flutter_db,
1208 coherence,
1209 hz: 0.0,
1210 z,
1211 }
1212}
1213
1214fn flutter(z: &[C], rate: f64, from: usize) -> f64 {
1215 let frame = ((FLUTTER_FRAME_SECS * rate).round() as usize).max(1);
1216 let span = (FLUTTER_SPAN_SECS * rate) as usize;
1217 let frames = (from + span).min(z.len()).saturating_sub(from) / frame;
1218 let edge = FLUTTER_MEAN_FRAMES / 2;
1219 if frames <= 2 * edge + 1 || frames * frame * 10 < span {
1220 return 0.0;
1221 }
1222 let levels: Vec<f64> = (0..frames)
1223 .map(|f| {
1224 let s = &z[from + f * frame..from + (f + 1) * frame];
1225 let mean = s.iter().map(|c| c.abs()).sum::<f64>() / frame as f64;
1226 20.0 * (mean + 1e-12).log10()
1227 })
1228 .collect();
1229 let offs: Vec<f64> = (edge..frames - edge)
1230 .map(|f| {
1231 let around = levels[f - edge..=f + edge].iter().sum::<f64>();
1232 levels[f] - around / FLUTTER_MEAN_FRAMES as f64
1233 })
1234 .collect();
1235 let mean = offs.iter().sum::<f64>() / offs.len() as f64;
1236 (offs.iter().map(|o| (o - mean).powi(2)).sum::<f64>() / offs.len() as f64).sqrt()
1237}
1238
1239fn coherence(b: &[C], rate: f64, from: usize) -> f64 {
1240 let lag = ((COHERENCE_LAG_SECS * rate).round() as usize).max(1);
1241 let to = (from + (COHERENCE_SPAN_SECS * rate) as usize).min(b.len());
1242 if to < from + 3 * lag {
1243 return 1.0;
1244 }
1245 let (mut cross, mut early, mut late) = (ZERO, 0.0, 0.0);
1246 for m in from..to - lag {
1247 cross = cross.add(b[m].conj().mul(b[m + lag]));
1248 early += b[m].norm();
1249 late += b[m + lag].norm();
1250 }
1251 let denom = (early * late).sqrt();
1252 if denom > 0.0 {
1253 cross.abs() / denom
1254 } else {
1255 1.0
1256 }
1257}
1258
1259fn amplitude_knots((hop_secs, step): (&[f64], f64), amps: &[f64], floor: f64) -> Curve {
1260 let mut sounding = (0..amps.len()).filter(|&k| amps[k] > 0.0);
1261 let (Some(first), Some(last)) = (sounding.clone().next(), sounding.next_back()) else {
1262 return Curve {
1263 times: vec![hop_secs[0], hop_secs[hop_secs.len() - 1]],
1264 values: vec![0.0, 0.0],
1265 error: 0.0,
1266 };
1267 };
1268 let (a, b) = (first.saturating_sub(1), (last + 1).min(amps.len() - 1));
1269 let peak = amps.iter().copied().fold(0.0, f64::max);
1270 let all = vec![true; b - a + 1];
1271 let tolerance = (KNOT_SHARE * peak).max(floor);
1272 let mut table = knots(&hop_secs[a..=b], &s[a..=b], &all, tolerance);
1273 if table.times.len() == 1 {
1274 table.times.push(table.times[0] + step);
1275 table.values.push(0.0);
1276 }
1277 table
1278}
1279
1280fn spread(values: &[f64], counts: &[bool]) -> f64 {
1284 let mut d: Vec<f64> = (1..values.len().saturating_sub(1))
1285 .filter(|&k| counts[k - 1] && counts[k] && counts[k + 1])
1286 .map(|k| (values[k - 1] - 2.0 * values[k] + values[k + 1]).abs())
1287 .collect();
1288 if d.is_empty() {
1289 return 0.0;
1290 }
1291 let middle = d.len() / 2;
1292 let (_, median, _) = d.select_nth_unstable_by(middle, f64::total_cmp);
1293 *median / (0.6745 * 6f64.sqrt())
1294}
1295
1296fn refit(table: Curve, at: &[f64], measured: &[f64], weights: &[f64]) -> Curve {
1300 let times = table.times;
1301 let n = times.len();
1302 let (mut diag, mut off, mut rhs) = (vec![0.0; n], vec![0.0; n], vec![0.0; n]);
1303 let mut seg = 0;
1304 for ((t, y), w) in at.iter().zip(measured).zip(weights) {
1305 while seg + 2 < n && *t > times[seg + 1] {
1306 seg += 1;
1307 }
1308 if *w <= 0.0 {
1309 continue;
1310 }
1311 let u = ((t - times[seg]) / (times[seg + 1] - times[seg])).clamp(0.0, 1.0);
1312 diag[seg] += w * (1.0 - u) * (1.0 - u);
1313 diag[seg + 1] += w * u * u;
1314 off[seg] += w * (1.0 - u) * u;
1315 rhs[seg] += w * (1.0 - u) * y;
1316 rhs[seg + 1] += w * u * y;
1317 }
1318 let ridge = diag.iter().copied().fold(0.0, f64::max) * 1e-9;
1319 if ridge == 0.0 {
1320 return Curve {
1321 times,
1322 values: table.values,
1323 error: 0.0,
1324 };
1325 }
1326 for k in 0..n {
1327 diag[k] += ridge;
1328 rhs[k] += ridge * table.values[k];
1329 }
1330 let values = tridiagonal(&diag, &off, &rhs);
1331 let mut walk = Walk::new(×, &values);
1332 let error = at
1333 .iter()
1334 .zip(measured)
1335 .zip(weights)
1336 .filter(|(_, w)| **w > 0.0)
1337 .map(|((t, y), _)| (walk.value(*t) - y).abs())
1338 .fold(0.0, f64::max);
1339 Curve {
1340 times,
1341 values,
1342 error,
1343 }
1344}
1345
1346fn tridiagonal(diag: &[f64], off: &[f64], rhs: &[f64]) -> Vec<f64> {
1348 let n = diag.len();
1349 let (mut c, mut d) = (vec![0.0; n], vec![0.0; n]);
1350 for k in 0..n {
1351 let (below, carried, held) = match k {
1352 0 => (0.0, 0.0, 0.0),
1353 _ => (off[k - 1], c[k - 1], d[k - 1]),
1354 };
1355 let pivot = diag[k] - below * carried;
1356 c[k] = if k + 1 < n { off[k] / pivot } else { 0.0 };
1357 d[k] = (rhs[k] - below * held) / pivot;
1358 }
1359 let mut x = vec![0.0; n];
1360 for k in (0..n).rev() {
1361 x[k] = d[k] - if k + 1 < n { c[k] * x[k + 1] } else { 0.0 };
1362 }
1363 x
1364}
1365
1366fn knots(times: &[f64], values: &[f64], counts: &[bool], tolerance: f64) -> Curve {
1370 let n = times.len();
1371 if n <= 2 {
1372 return Curve {
1373 times: times.to_vec(),
1374 values: values.to_vec(),
1375 error: 0.0,
1376 };
1377 }
1378 let mut kept = vec![0usize];
1379 let mut s = 0;
1380 while s < n - 1 {
1381 let (mut lo, mut hi) = (f64::MIN, f64::MAX);
1382 let mut end = s + 1;
1383 for e in s + 1..n {
1384 let dt = times[e] - times[s];
1385 let slope = (values[e] - values[s]) / dt;
1386 if slope >= lo && slope <= hi {
1387 end = e;
1388 }
1389 if counts[e] {
1390 lo = lo.max((values[e] - tolerance - values[s]) / dt);
1391 hi = hi.min((values[e] + tolerance - values[s]) / dt);
1392 }
1393 if lo > hi {
1394 break;
1395 }
1396 }
1397 kept.push(end);
1398 s = end;
1399 }
1400 let mut error = 0f64;
1401 for w in kept.windows(2) {
1402 for j in (w[0]..=w[1]).filter(|&j| counts[j]) {
1403 let u = (times[j] - times[w[0]]) / (times[w[1]] - times[w[0]]);
1404 let line = values[w[0]] + (values[w[1]] - values[w[0]]) * u;
1405 error = error.max((values[j] - line).abs());
1406 }
1407 }
1408 Curve {
1409 times: kept.iter().map(|&k| times[k]).collect(),
1410 values: kept.iter().map(|&k| values[k]).collect(),
1411 error,
1412 }
1413}