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
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148

//! Quadrature-based decoder.

use num_traits::{One, SaturatingAdd, Zero};

use crate::{index_decoder::IndexDecoder, Change, Error, IncrementalDecoder, StepMode};

/// A robust indexed quadrature decoder with support for multiple step-modes,
/// based on which channel (A vs. B) is leading the other.
///
/// ```plain
///                ┌ ─ ┐   ┌───┐   ┌───┐   ┌───┐   ┌ ─ ─ high
///            A           │   │   │   │   │                  
///              ─ ┘   └───┘   └───┘   └───┘   └ ─ ┘     low  
/// AB:                                                  
///                  ┌ ─ ┐   ┌───┐   ┌───┐   ┌───┐   ┌ ─ high
///            B             │   │   │   │   │                
///              ─ ─ ┘   └───┘   └───┘   └───┘   └ ─ ┘   low  
///
///                          ┌─┐                         high
///            Z             │ │                              
///              ─ ─ ─ ──────┘ └────────────────── ─ ─ ─ low  
/// Time: ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─ ─▶
///                  ┌ ─ ┐   ┌───┐   ┌───┐   ┌───┐   ┌ ─ high
///            A             │   │   │   │   │                
///              ─ ─ ┘   └───┘   └───┘   └───┘   └ ─ ┘   low  
/// BA:                                                  
///                ┌ ─ ┐   ┌───┐   ┌───┐   ┌───┐   ┌ ─ ─ high
///            B           │   │   │   │   │                  
///              ─ ┘   └───┘   └───┘   └───┘   └ ─ ┘     low  
///
///                          ┌─┐                         high
///            Z             │ │                              
///              ─ ─ ─ ──────┘ └────────────────── ─ ─ ─ low  
/// ```
#[derive(Debug)]
pub struct IndexedIncrementalDecoder<Mode, T = i32> {
    decoder: IncrementalDecoder<Mode, T>,
    indexer: IndexDecoder,
}

impl<Mode, T> Default for IndexedIncrementalDecoder<Mode, T>
where
    Mode: StepMode,
    IncrementalDecoder<Mode, T>: Default,
{
    fn default() -> Self {
        Self::new(IncrementalDecoder::default())
    }
}

impl<Mode, T> IndexedIncrementalDecoder<Mode, T>
where
    Mode: StepMode,
{
    pub(crate) fn new(decoder: IncrementalDecoder<Mode, T>) -> Self {
        Self {
            decoder,
            indexer: Default::default(),
        }
    }
}

impl<Mode, T> IndexedIncrementalDecoder<Mode, T>
where
    Mode: StepMode,
    T: Copy + Zero + One + SaturatingAdd + From<i8>,
{
    /// Updates the decoder's state based on the given `a` and `b` pulse train (aka channel) readings,
    /// returning the direction if a change was detected, `None` if no change was detected,
    /// or `Err(_)` if an invalid input (i.e. a counteral "jump") was detected.
    ///
    /// Upon detection of a raising edge on the `z` pulse train the counter gets reset back to `0`.
    ///
    /// Depending on whether it matters why the decoder did not detect a change
    /// (e.g. due to actual lack of change or an erroneous read)
    /// you would either call `decoder.update(a, b)` directly, or via `decoder.update(a, b).unwrap_or_default()`
    /// to fall back to `None` in case of `Err(_)`.
    pub fn update(&mut self, a: bool, b: bool, z: bool) -> Result<Option<Change>, Error> {
        let result = self.decoder.update(a, b);

        if self.indexer.update(z) {
            self.decoder.set_counter(Zero::zero());
        }

        result
    }

    /// Resets the decoder to its initial state and its counter counter back to `0`.
    pub fn reset(&mut self) {
        self.decoder.reset();
        self.indexer.reset();
    }

    /// Returns the decoder's counter counter relative to its initial counter in number of cycles.
    ///
    /// A change of `Change::Positive` increments the counter counter,
    /// while a change of `Change::Negative` decrements it.
    pub fn counter(&self) -> T {
        self.decoder.counter()
    }

    /// Sets the decoder's counter.
    pub fn set_counter(&mut self, counter: T) {
        self.decoder.set_counter(counter);
    }
}

#[cfg(test)]
mod tests {
    use crate::HalfStep;

    use super::*;

    #[test]
    fn index() {
        let a: Vec<bool> = vec![false, false, true, true, false, false, true, true];
        let b: Vec<bool> = vec![true, false, false, true, true, false, false, true];
        let z: Vec<bool> = vec![false, false, false, false, true, false, false, false];

        let pulse_trains = a.into_iter().zip(b).zip(z);

        let changes: Vec<Option<Change>> = vec![
            None,
            Some(Change::Positive),
            None,
            Some(Change::Positive),
            None,
            Some(Change::Positive),
            None,
            Some(Change::Positive),
        ];
        let counters: Vec<i32> = vec![0, 1, 1, 2, 0, 1, 1, 2];

        let expected = changes.into_iter().zip(counters);

        let mut decoder: IndexedIncrementalDecoder<HalfStep> = Default::default();

        for (input, expected) in pulse_trains.zip(expected) {
            let ((a, b), z) = input;
            let (expected_change, expected_counter) = expected;

            let change = decoder.update(a, b, z).unwrap();

            assert_eq!(change, expected_change);
            assert_eq!(decoder.counter(), expected_counter);
        }
    }
}