idsp/
unwrap.rs

1use core::{
2    cmp::PartialOrd,
3    ops::{BitAnd, Shr},
4};
5use num_traits::{
6    cast::AsPrimitive,
7    identities::Zero,
8    ops::wrapping::{WrappingAdd, WrappingSub},
9    Signed,
10};
11use serde::{Deserialize, Serialize};
12
13/// Subtract `y - x` with signed overflow.
14///
15/// This is very similar to `i32::overflowing_sub(y, x)` except that the
16/// overflow indicator is not a boolean but the signum of the overflow.
17/// Additionally it's typically faster.
18///
19/// Returns:
20/// A tuple containg the (wrapped) difference `y - x` and the signum of the
21/// overflow.
22#[inline(always)]
23pub fn overflowing_sub<T>(y: T, x: T) -> (T, i32)
24where
25    T: WrappingSub + Zero + PartialOrd,
26{
27    let delta = y.wrapping_sub(&x);
28    let wrap = (delta >= T::zero()) as i32 - (y >= x) as i32;
29    (delta, wrap)
30}
31
32/// Combine high and low i32 into a single downscaled i32, saturating monotonically.
33///
34/// Args:
35/// `lo`: LSB i32 to scale down by `shift` and range-extend with `hi`
36/// `hi`: MSB i32 to scale up and extend `lo` with. Output will be clipped if
37///     `hi` exceeds the output i32 range.
38/// `shift`: Downscale `lo` by that many bits. Values from 1 to 32 inclusive
39///     are valid.
40pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 {
41    debug_assert!(shift > 0);
42    debug_assert!(shift <= 32);
43    let hi_range = -1 << (shift - 1);
44    if hi <= hi_range {
45        i32::MIN - hi_range
46    } else if -hi <= hi_range {
47        hi_range - i32::MIN
48    } else {
49        (lo >> shift) + (hi << (32 - shift))
50    }
51}
52
53/// Overflow unwrapper.
54///
55/// This is unwrapping as in the phase and overflow unwrapping context, not
56/// unwrapping as in the `Result`/`Option` context.
57#[derive(Copy, Clone, Default, Deserialize, Serialize)]
58pub struct Unwrapper<Q> {
59    /// current output
60    y: Q,
61}
62
63impl<Q> Unwrapper<Q>
64where
65    Q: 'static + WrappingAdd + Copy,
66{
67    /// Feed a new sample..
68    ///
69    /// Args:
70    /// * `x`: New sample
71    ///
72    /// Returns:
73    /// The (wrapped) difference `x - x_old`
74    pub fn update<P>(&mut self, x: P) -> P
75    where
76        P: 'static + WrappingSub + Copy + AsPrimitive<Q>,
77        Q: AsPrimitive<P>,
78    {
79        let dx = x.wrapping_sub(&self.y.as_());
80        self.y = self.y.wrapping_add(&dx.as_());
81        dx
82    }
83
84    /// The current number of wraps
85    pub fn wraps<P, const S: u32>(&self) -> P
86    where
87        Q: AsPrimitive<P> + Shr<u32, Output = Q>,
88        P: 'static + Copy + WrappingAdd + Signed + BitAnd<u32, Output = P>,
89    {
90        (self.y >> S)
91            .as_()
92            .wrapping_add(&((self.y >> (S - 1)).as_() & 1))
93    }
94
95    /// The current phase
96    pub fn phase<P>(&self) -> P
97    where
98        P: 'static + Copy,
99        Q: AsPrimitive<P>,
100    {
101        self.y.as_()
102    }
103
104    /// Current output including wraps
105    pub fn y(&self) -> Q {
106        self.y
107    }
108}
109
110#[cfg(test)]
111mod tests {
112    use super::*;
113    #[test]
114    fn overflowing_sub_correctness() {
115        for (x0, x1, v) in [
116            (0i32, 0i32, 0i32),
117            (0, 1, 0),
118            (0, -1, 0),
119            (1, 0, 0),
120            (-1, 0, 0),
121            (0, 0x7fff_ffff, 0),
122            (-1, 0x7fff_ffff, -1),
123            (-2, 0x7fff_ffff, -1),
124            (-1, -0x8000_0000, 0),
125            (0, -0x8000_0000, 0),
126            (1, -0x8000_0000, 1),
127            (-0x6000_0000, 0x6000_0000, -1),
128            (0x6000_0000, -0x6000_0000, 1),
129            (-0x4000_0000, 0x3fff_ffff, 0),
130            (-0x4000_0000, 0x4000_0000, -1),
131            (-0x4000_0000, 0x4000_0001, -1),
132            (0x4000_0000, -0x3fff_ffff, 0),
133            (0x4000_0000, -0x4000_0000, 0),
134            (0x4000_0000, -0x4000_0001, 1),
135        ]
136        .iter()
137        {
138            let (dx, w) = overflowing_sub(*x1, *x0);
139            assert_eq!(*v, w, " = overflowing_sub({:#x}, {:#x})", *x0, *x1);
140            let (dx0, w0) = x1.overflowing_sub(*x0);
141            assert_eq!(w0, w != 0);
142            assert_eq!(dx, dx0);
143        }
144    }
145
146    #[test]
147    fn saturating_scale_correctness() {
148        let shift = 8;
149        for (lo, hi, res) in [
150            (0i32, 0i32, 0i32),
151            (0, 1, 0x0100_0000),
152            (0, -1, -0x0100_0000),
153            (0x100, 0, 1),
154            (-1 << 31, 0, -1 << 23),
155            (0x7fffffff, 0, 0x007f_ffff),
156            (0x7fffffff, 1, 0x0017f_ffff),
157            (-0x7fffffff, -1, -0x0180_0000),
158            (0x1234_5600, 0x7f, 0x7f12_3456),
159            (0x1234_5600, -0x7f, -0x7f00_0000 + 0x12_3456),
160            (0, 0x7f, 0x7f00_0000),
161            (0, 0x80, 0x7fff_ff80),
162            (0, -0x7f, -0x7f00_0000),
163            (0, -0x80, -0x7fff_ff80),
164            (0x7fff_ffff, 0x7f, 0x7f7f_ffff),
165            (-0x8000_0000, 0x7f, 0x7e80_0000),
166            (-0x8000_0000, -0x7f, -0x7f80_0000),
167            (0x7fff_ffff, -0x7f, -0x7e80_0001),
168            (0x100, 0x7f, 0x7f00_0001),
169            (0, -0x80, -0x7fff_ff80),
170            (-1 << 31, 0x80, 0x7fff_ff80),
171            (-1 << 31, -0x80, -0x7fff_ff80),
172        ]
173        .iter()
174        {
175            let s = saturating_scale(*lo, *hi, shift);
176            assert_eq!(
177                *res, s,
178                "{:#x} != {:#x} = saturating_scale({:#x}, {:#x}, {:#x})",
179                *res, s, *lo, *hi, shift
180            );
181        }
182    }
183}
idsp/unwrap.rs

idsp/
unwrap.rs
