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
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168

//! Clock recovery implementations
/*
* Much evolving clock sync.
*
* Study material:
* https://youtu.be/jag3btxSsig
*/
use anyhow::Result;

use crate::block::{Block, BlockRet};
use crate::stream::{InputStreams, OutputStreams};
use crate::{Error, Float};

trait Ted {
    fn error(&self, input: &[Float]) -> Float;
}

struct TedDeriv {}

impl TedDeriv {
    fn new() -> Self {
        Self {}
    }
}

impl Ted for TedDeriv {
    fn error(&self, input: &[Float]) -> Float {
        let sign = if input[0] > 0.0 { 1.0 } else { -1.0 };
        sign * input[input.len() - 1] - input[0]
    }
}

/// Broken, DO NOT USE.
pub struct SymbolSync {
    _sps: Float,
    _max_deviation: Float,
    clock: Float,
    ted: Box<dyn Ted>,
}

impl SymbolSync {
    /// Broken, DO NOT USE.
    pub fn new(sps: Float, max_deviation: Float) -> Self {
        assert!(sps > 1.0);
        Self {
            _sps: sps,
            _max_deviation: max_deviation,
            clock: sps,
            ted: Box::new(TedDeriv::new()),
        }
    }
}

/*
error = sign(x) * deriv(x)
positive error means "early", neagive error means "late"
*/

impl Block for SymbolSync {
    fn block_name(&self) -> &'static str {
        "SymbolSync"
    }
    fn work(&mut self, r: &mut InputStreams, w: &mut OutputStreams) -> Result<BlockRet, Error> {
        let input = r.get::<Float>(0);
        let mut v = Vec::new();
        let n = input.borrow().available();
        let mut pos = Float::default();
        loop {
            let i = pos as usize;
            if i + 1 >= n {
                break;
            }
            // TODO: needless copy.
            let t: Vec<Float> = input.borrow().data().clone().into();
            let error = self.ted.error(&t);
            if error > 0.0 {
                pos += 0.3;
            } else {
                pos -= 0.3;
            }
            v.push(input.borrow().data()[i]);
            pos += self.clock;
        }
        input.borrow_mut().clear();
        w.get::<Float>(0).borrow_mut().write_slice(&v);
        Ok(BlockRet::Ok)
    }
}

/** Very simple clock recovery by looking at zero crossings.

Every time the stream crosses 0, this is assumed to be right in the
middle of two symbols, and the next chosen sample to use as a symbol
will be the one `sps/2` samples later.

The one after that will be after `1.5*sps` samples. And so on, until
the next zero crossing happens, and the clock thus resets.

Future work in this block will be to adjust the sps according to when
the expected vs actual zero crossings happen, effectively phase lock
looping.

But for now it's "good enough" to get simple 2FSK decoded pretty
reliably.
*/
pub struct ZeroCrossing {
    sps: Float,
    max_deviation: Float,
    clock: Float,
    last_sign: bool,
    last_cross: f32,
    counter: u64,
}

impl ZeroCrossing {
    /** Create new ZeroCrossing block.

    # Args
    * `sps`: Samples per symbol. IOW `samp_rate / baud`.
    * `max_deviation`: Not currently used.
     */
    pub fn new(sps: Float, max_deviation: Float) -> Self {
        assert!(sps > 1.0);
        Self {
            sps,
            clock: sps,
            max_deviation,
            last_sign: false,
            last_cross: 0.0,
            counter: 0,
        }
    }
}

impl Block for ZeroCrossing {
    fn block_name(&self) -> &'static str {
        "ZeroCrossing"
    }
    fn work(&mut self, r: &mut InputStreams, w: &mut OutputStreams) -> Result<BlockRet, Error> {
        let mut v = Vec::new();
        let input = r.get(0);
        for sample in input.borrow().iter() {
            if self.counter == (self.last_cross + (self.clock / 2.0)) as u64 {
                v.push(*sample);
                self.last_cross += self.clock;
            }

            let sign = *sample > 0.0;
            if sign != self.last_sign {
                self.last_cross = self.counter as f32;
                // TODO: adjust clock, within sps. Here just shut up the linter.
                self.sps *= 1.0;
                self.max_deviation *= 1.0;
            }
            self.last_sign = sign;
            self.counter += 1;

            let step_back = (10.0 * self.clock) as u64;
            if self.counter > step_back && self.last_cross as u64 > step_back {
                self.counter -= step_back;
                self.last_cross -= step_back as f32;
            }
        }
        input.borrow_mut().clear();
        w.get::<Float>(0).borrow_mut().write_slice(&v);
        Ok(BlockRet::Ok)
    }
}