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
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102

//! John Ehlers Center of Gravity Indicator
//! from: https://mesasoftware.com/papers/TheCGOscillator.pdf

use std::collections::VecDeque;

use super::View;

/// John Ehlers Center of Gravity Indicator
/// from: https://mesasoftware.com/papers/TheCGOscillator.pdf
#[derive(Clone)]
pub struct CenterOfGravity<V> {
    view: V,
    window_len: usize,
    q_vals: VecDeque<f64>,
    out: f64,
}

impl<V> std::fmt::Debug for CenterOfGravity<V>
where
    V: View,
{
    fn fmt(&self, fmt: &mut std::fmt::Formatter<'_>) -> Result<(), std::fmt::Error> {
        write!(
            fmt,
            "CenterOfGravity(window_len: {}, q_vals: {:?}, out: {})",
            self.window_len, self.q_vals, self.out
        )
    }
}

impl<V> CenterOfGravity<V>
where
    V: View,
{
    /// Create a Center of Gravity Indicator with a chained View
    /// and a given sliding window length
    #[inline]
    pub fn new(view: V, window_len: usize) -> Self {
        Self {
            view,
            window_len,
            q_vals: VecDeque::new(),
            out: 0.0,
        }
    }
}

impl<V> View for CenterOfGravity<V>
where
    V: View,
{
    // update receives a new value and updates its internal state
    fn update(&mut self, val: f64) {
        self.view.update(val);
        let val = self.view.last();

        if self.q_vals.len() >= self.window_len {
            self.q_vals.pop_front();
        }
        self.q_vals.push_back(val);

        let mut denom: f64 = 0.0;
        let mut num: f64 = 0.0;
        let q_len = self.q_vals.len();
        for i in 0..q_len {
            let weight = q_len - i;
            let val_i = self.q_vals.get(i).unwrap();
            num += weight as f64 * val_i;
            denom += *val_i;
        }
        if denom != 0.0 {
            self.out = -num / denom + (self.q_vals.len() as f64 + 1.0) / 2.0
        } else {
            self.out = 0.0;
        }
    }

    #[inline(always)]
    fn last(&self) -> f64 {
        self.out
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::plot::plot_values;
    use crate::test_data::TEST_DATA;
    use crate::Echo;

    #[test]
    fn center_of_gravity_plot() {
        let mut cgo = CenterOfGravity::new(Echo::new(), 16);
        let mut out: Vec<f64> = Vec::new();
        for v in &TEST_DATA {
            cgo.update(*v);
            out.push(cgo.last());
        }
        let filename = "img/center_of_gravity.png";
        plot_values(out, filename).unwrap();
    }
}