idsp/
unwrap.rs

1use core::{
2    cmp::{Ordering, PartialOrd},
3    ops::{BitAnd, Shr},
4};
5use num_traits::{
6    Bounded, Signed,
7    cast::AsPrimitive,
8    identities::Zero,
9    ops::wrapping::{WrappingAdd, WrappingSub},
10};
11use serde::{Deserialize, Serialize};
12
13use dsp_process::{Inplace, Process};
14
15/// Wrap classification
16#[repr(i8)]
17#[derive(Copy, Clone, Debug, Default, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
18pub enum Wrap {
19    /// A wrap occured in the negative direction
20    Negative = -1,
21    /// No wrap, the wrapped difference between successive values
22    /// has the same sign as their comparison
23    #[default]
24    None = 0,
25    /// A wrap occurred in the positive direction
26    Positive = 1,
27}
28
29impl From<Wrap> for Ordering {
30    fn from(value: Wrap) -> Self {
31        match value {
32            Wrap::Negative => Self::Less,
33            Wrap::None => Self::Equal,
34            Wrap::Positive => Self::Greater,
35        }
36    }
37}
38
39impl From<Ordering> for Wrap {
40    fn from(value: Ordering) -> Self {
41        match value {
42            Ordering::Less => Self::Negative,
43            Ordering::Equal => Self::None,
44            Ordering::Greater => Self::Positive,
45        }
46    }
47}
48
49impl core::ops::Add for Wrap {
50    type Output = Self;
51
52    fn add(self, rhs: Self) -> Self::Output {
53        (self as i32 + rhs as i32).cmp(&0).into()
54    }
55}
56
57impl core::ops::AddAssign for Wrap {
58    fn add_assign(&mut self, rhs: Self) {
59        *self = *self + rhs
60    }
61}
62
63/// Subtract `y - x` with signed overflow.
64///
65/// This is very similar to `i32::overflowing_sub(y, x)` except that the
66/// overflow indicator is not a boolean but the signum of the overflow.
67/// Additionally it's typically faster.
68///
69/// Returns:
70/// A tuple containg the (wrapped) difference `y - x` and the signum of the
71/// overflow.
72#[inline(always)]
73pub fn overflowing_sub<T>(y: T, x: T) -> (T, Wrap)
74where
75    T: WrappingSub + Zero + PartialOrd,
76{
77    let delta = y.wrapping_sub(&x);
78    let wrap = (delta >= T::zero()).cmp(&(y >= x)).into();
79    (delta, wrap)
80}
81
82/// Combine high and low i32 into a single downscaled i32, saturating monotonically.
83///
84/// Args:
85/// `lo`: LSB i32 to scale down by `shift` and range-extend with `hi`
86/// `hi`: MSB i32 to scale up and extend `lo` with. Output will be clipped if
87///     `hi` exceeds the output i32 range.
88/// `shift`: Downscale `lo` by that many bits. Values from 1 to 32 inclusive
89///     are valid.
90pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 {
91    debug_assert!(shift > 0);
92    debug_assert!(shift <= 32);
93    let hi_range = -1 << (shift - 1);
94    if hi <= hi_range {
95        i32::MIN - hi_range
96    } else if -hi <= hi_range {
97        hi_range - i32::MIN
98    } else {
99        (lo >> shift) + (hi << (32 - shift))
100    }
101}
102
103/// Overflow unwrapper.
104///
105/// This is unwrapping as in the phase and overflow unwrapping context, not
106/// unwrapping as in the `Result`/`Option` context.
107#[derive(Copy, Clone, Default, Deserialize, Serialize)]
108pub struct Unwrapper<Q> {
109    /// current output
110    pub y: Q,
111}
112
113impl<Q> Unwrapper<Q>
114where
115    Q: 'static + WrappingAdd + Copy,
116{
117    /// The current number of wraps
118    pub fn wraps<P, const S: u32>(&self) -> P
119    where
120        Q: AsPrimitive<P> + Shr<u32, Output = Q>,
121        P: 'static + Copy + WrappingAdd + Signed + BitAnd<u32, Output = P>,
122    {
123        (self.y >> S)
124            .as_()
125            .wrapping_add(&((self.y >> (S - 1)).as_() & 1))
126    }
127
128    /// The current phase
129    pub fn phase<P>(&self) -> P
130    where
131        P: 'static + Copy,
132        Q: AsPrimitive<P>,
133    {
134        self.y.as_()
135    }
136}
137
138impl<P, Q> Process<P> for Unwrapper<Q>
139where
140    P: AsPrimitive<Q> + WrappingSub,
141    Q: AsPrimitive<P> + WrappingAdd,
142{
143    /// Feed a new sample..
144    ///
145    /// Args:
146    /// * `x`: New sample
147    ///
148    /// Returns:
149    /// The (wrapped) difference `x - x_old`
150    fn process(&mut self, x: P) -> P {
151        let dx = x.wrapping_sub(&self.y.as_());
152        self.y = self.y.wrapping_add(&dx.as_());
153        dx
154    }
155}
156
157impl<P: Copy, Q> Inplace<P> for Unwrapper<Q> where Self: Process<P> {}
158
159/// Maps wraps to saturation
160///
161/// Clamps output to the value range on wraps and only un-clamps on
162/// (one corresponding) un-wrap.
163#[derive(Debug, Default, Clone, Serialize, Deserialize)]
164pub struct ClampWrap<Q> {
165    /// Last input value
166    pub x0: Q,
167    /// Clamp indicator
168    pub clamp: Wrap,
169}
170
171impl<Q> Process<Q> for ClampWrap<Q>
172where
173    Q: 'static + Zero + PartialOrd + WrappingSub + Copy + Bounded,
174{
175    /// Update the clamp with a new input
176    ///
177    /// IF (positive wrap and negative clamp,
178    /// OR negative wrap and positive clamp,
179    /// OR no wrap and no clamp): output the input.
180    /// ELSE IF negative wrap: clamp minimum,
181    /// ELSE IF positive wrap: clamp maximum.
182    fn process(&mut self, x: Q) -> Q {
183        let (_dx, wrap) = overflowing_sub(x, self.x0);
184        self.x0 = x;
185        self.clamp += wrap;
186        match self.clamp {
187            Wrap::Negative => Q::min_value(),
188            Wrap::None => x,
189            Wrap::Positive => Q::max_value(),
190        }
191    }
192}
193
194impl<Q: Copy> Inplace<Q> for ClampWrap<Q> where Self: Process<Q> {}
195
196#[cfg(test)]
197mod tests {
198    use super::*;
199    #[test]
200    fn overflowing_sub_correctness() {
201        for (x0, x1, v) in [
202            (0i32, 0i32, Wrap::None),
203            (0, 1, Wrap::None),
204            (0, -1, Wrap::None),
205            (1, 0, Wrap::None),
206            (-1, 0, Wrap::None),
207            (0, 0x7fff_ffff, Wrap::None),
208            (-1, 0x7fff_ffff, Wrap::Negative),
209            (-2, 0x7fff_ffff, Wrap::Negative),
210            (-1, -0x8000_0000, Wrap::None),
211            (0, -0x8000_0000, Wrap::None),
212            (1, -0x8000_0000, Wrap::Positive),
213            (-0x6000_0000, 0x6000_0000, Wrap::Negative),
214            (0x6000_0000, -0x6000_0000, Wrap::Positive),
215            (-0x4000_0000, 0x3fff_ffff, Wrap::None),
216            (-0x4000_0000, 0x4000_0000, Wrap::Negative),
217            (-0x4000_0000, 0x4000_0001, Wrap::Negative),
218            (0x4000_0000, -0x3fff_ffff, Wrap::None),
219            (0x4000_0000, -0x4000_0000, Wrap::None),
220            (0x4000_0000, -0x4000_0001, Wrap::Positive),
221        ]
222        .iter()
223        {
224            let (dx, w) = overflowing_sub(*x1, *x0);
225            assert_eq!(*v, w, " = overflowing_sub({:#x}, {:#x})", *x0, *x1);
226            let (dx0, w0) = x1.overflowing_sub(*x0);
227            assert_eq!(w0, w != Wrap::None);
228            assert_eq!(dx, dx0);
229        }
230    }
231
232    #[test]
233    fn saturating_scale_correctness() {
234        let shift = 8;
235        for (lo, hi, res) in [
236            (0i32, 0i32, 0i32),
237            (0, 1, 0x0100_0000),
238            (0, -1, -0x0100_0000),
239            (0x100, 0, 1),
240            (-1 << 31, 0, -1 << 23),
241            (0x7fffffff, 0, 0x007f_ffff),
242            (0x7fffffff, 1, 0x0017f_ffff),
243            (-0x7fffffff, -1, -0x0180_0000),
244            (0x1234_5600, 0x7f, 0x7f12_3456),
245            (0x1234_5600, -0x7f, -0x7f00_0000 + 0x12_3456),
246            (0, 0x7f, 0x7f00_0000),
247            (0, 0x80, 0x7fff_ff80),
248            (0, -0x7f, -0x7f00_0000),
249            (0, -0x80, -0x7fff_ff80),
250            (0x7fff_ffff, 0x7f, 0x7f7f_ffff),
251            (-0x8000_0000, 0x7f, 0x7e80_0000),
252            (-0x8000_0000, -0x7f, -0x7f80_0000),
253            (0x7fff_ffff, -0x7f, -0x7e80_0001),
254            (0x100, 0x7f, 0x7f00_0001),
255            (0, -0x80, -0x7fff_ff80),
256            (-1 << 31, 0x80, 0x7fff_ff80),
257            (-1 << 31, -0x80, -0x7fff_ff80),
258        ]
259        .iter()
260        {
261            let s = saturating_scale(*lo, *hi, shift);
262            assert_eq!(
263                *res, s,
264                "{:#x} != {:#x} = saturating_scale({:#x}, {:#x}, {:#x})",
265                *res, s, *lo, *hi, shift
266            );
267        }
268    }
269}
idsp/unwrap.rs

idsp/
unwrap.rs
