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

use cgmath::*;
use joycon_sys::imu::IMU_SAMPLES_PER_SECOND;
use std::collections::VecDeque;

type Entry = Vector3<f64>;

#[derive(Clone, Debug)]
pub struct Calibration {
    history: VecDeque<Entry>,
    capacity: usize,
}

impl Calibration {
    pub fn with_capacity(capacity: usize) -> Calibration {
        Calibration {
            history: VecDeque::with_capacity(capacity),
            capacity,
        }
    }

    pub fn push(&mut self, entry: Entry) {
        if self.history.len() == self.capacity {
            self.history.pop_back();
        }
        self.history.push_front(entry);
    }

    pub fn reset(&mut self) {
        self.history.clear();
    }

    pub fn get_average(&mut self) -> Entry {
        let zero = Vector3::new(0., 0., 0.);
        let len = self.history.len() as f64;
        if len == 0. {
            return zero;
        }
        self.history
            .iter()
            .cloned()
            .fold(zero, |acc, val| acc + val)
            / len
    }
}

impl Default for Calibration {
    fn default() -> Self {
        Calibration::with_capacity(3 * IMU_SAMPLES_PER_SECOND as usize)
    }
}