1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

use nalgebra as na;

#[derive(Debug)]
pub(crate) struct ClockModelFitError(String);

impl std::fmt::Display for ClockModelFitError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> Result<(), std::fmt::Error> {
        write!(f, "error fitting clock model: {}", self.0)
    }
}

impl std::error::Error for ClockModelFitError {}

pub(crate) fn fit_time_model(
    past_data: &[(f64, f64)],
) -> Result<(f64, f64, f64), ClockModelFitError> {
    use na::{OMatrix, OVector, U2};

    let mut a: Vec<f64> = Vec::with_capacity(past_data.len() * 2);
    let mut b: Vec<f64> = Vec::with_capacity(past_data.len());

    for row in past_data.iter() {
        a.push(row.0);
        a.push(1.0);
        b.push(row.1);
    }
    let a = OMatrix::<f64, na::Dyn, U2>::from_row_slice(&a);
    let b = OVector::<f64, na::Dyn>::from_row_slice(&b);

    let epsilon = 1e-10;
    let results = lstsq::lstsq(&a, &b, epsilon).map_err(|msg| ClockModelFitError(msg.into()))?;

    let gain = results.solution[0];
    let offset = results.solution[1];
    let residuals = results.residuals;

    Ok((gain, offset, residuals))
}

#[test]
fn test_fit_time_model() {
    let epsilon = 1e-12;

    let data = vec![(0.0, 0.0), (1.0, 1.0), (2.0, 2.0), (3.0, 3.0)];
    let (gain, offset, _residuals) = fit_time_model(&data).unwrap();
    assert!((gain - 1.0).abs() < epsilon);
    assert!((offset - 0.0).abs() < epsilon);

    let data = vec![(0.0, 12.0), (1.0, 22.0), (2.0, 32.0), (3.0, 42.0)];
    let (gain, offset, _residuals) = fit_time_model(&data).unwrap();
    assert!((gain - 10.0).abs() < epsilon);
    assert!((offset - 12.0).abs() < epsilon);
}