embedded_dsp/
support.rs

1//! Support functions (copy, fill, format conversions q7/q15/q31/f32, sort, barycenter, weighted sum).
2
3use crate::types::*;
4
5// --- Copy & Fill ---
6
7pub fn copy_f32(src: &[f32], dst: &mut [f32]) {
8    let len = src.len().min(dst.len());
9    dst[..len].copy_from_slice(&src[..len]);
10}
11
12pub fn copy_q31(src: &[q31], dst: &mut [q31]) {
13    let len = src.len().min(dst.len());
14    dst[..len].copy_from_slice(&src[..len]);
15}
16
17pub fn copy_q15(src: &[q15], dst: &mut [q15]) {
18    let len = src.len().min(dst.len());
19    dst[..len].copy_from_slice(&src[..len]);
20}
21
22pub fn copy_q7(src: &[q7], dst: &mut [q7]) {
23    let len = src.len().min(dst.len());
24    dst[..len].copy_from_slice(&src[..len]);
25}
26
27pub fn fill_f32(value: f32, dst: &mut [f32]) {
28    dst.fill(value);
29}
30
31pub fn fill_q31(value: q31, dst: &mut [q31]) {
32    dst.fill(value);
33}
34
35pub fn fill_q15(value: q15, dst: &mut [q15]) {
36    dst.fill(value);
37}
38
39pub fn fill_q7(value: q7, dst: &mut [q7]) {
40    dst.fill(value);
41}
42
43// --- Type Conversions ---
44
45pub fn q7_to_q15(src: &[q7], dst: &mut [q15]) {
46    let len = src.len().min(dst.len());
47    for i in 0..len {
48        dst[i] = (src[i] as i16) << 8;
49    }
50}
51
52pub fn q7_to_q31(src: &[q7], dst: &mut [q31]) {
53    let len = src.len().min(dst.len());
54    for i in 0..len {
55        dst[i] = (src[i] as i32) << 24;
56    }
57}
58
59pub fn q7_to_f32(src: &[q7], dst: &mut [f32]) {
60    let len = src.len().min(dst.len());
61    for i in 0..len {
62        dst[i] = (src[i] as f32) / 128.0;
63    }
64}
65
66pub fn q15_to_q7(src: &[q15], dst: &mut [q7]) {
67    let len = src.len().min(dst.len());
68    for i in 0..len {
69        dst[i] = (src[i] >> 8) as q7;
70    }
71}
72
73pub fn q15_to_q31(src: &[q15], dst: &mut [q31]) {
74    let len = src.len().min(dst.len());
75    for i in 0..len {
76        dst[i] = (src[i] as i32) << 16;
77    }
78}
79
80pub fn q15_to_f32(src: &[q15], dst: &mut [f32]) {
81    let len = src.len().min(dst.len());
82    for i in 0..len {
83        dst[i] = (src[i] as f32) / 32768.0;
84    }
85}
86
87pub fn q31_to_q7(src: &[q31], dst: &mut [q7]) {
88    let len = src.len().min(dst.len());
89    for i in 0..len {
90        dst[i] = (src[i] >> 24) as q7;
91    }
92}
93
94pub fn q31_to_q15(src: &[q31], dst: &mut [q15]) {
95    let len = src.len().min(dst.len());
96    for i in 0..len {
97        dst[i] = (src[i] >> 16) as q15;
98    }
99}
100
101pub fn q31_to_f32(src: &[q31], dst: &mut [f32]) {
102    let len = src.len().min(dst.len());
103    for i in 0..len {
104        dst[i] = (src[i] as f32) / 2147483648.0;
105    }
106}
107
108pub fn f32_to_q7(src: &[f32], dst: &mut [q7]) {
109    let len = src.len().min(dst.len());
110    for i in 0..len {
111        dst[i] = (src[i] * 128.0).clamp(-128.0, 127.0) as q7;
112    }
113}
114
115pub fn f32_to_q15(src: &[f32], dst: &mut [q15]) {
116    let len = src.len().min(dst.len());
117    for i in 0..len {
118        dst[i] = (src[i] * 32768.0).clamp(-32768.0, 32767.0) as q15;
119    }
120}
121
122/// Quantize FIR taps to Q15 with nearest-even-style rounding toward nearest integer.
123///
124/// Prefer this over [`f32_to_q15`] for windowed-sinc kernels: truncation bias shows up
125/// as extra stopband ripple. `dst` must be at least as long as `src`.
126pub fn fir_taps_f32_to_q15(src: &[f32], dst: &mut [q15]) -> Status {
127    if dst.len() < src.len() {
128        return Status::LengthError;
129    }
130    for i in 0..src.len() {
131        let scaled = src[i] * 32768.0;
132        let rounded = if scaled >= 0.0 {
133            scaled + 0.5
134        } else {
135            scaled - 0.5
136        };
137        dst[i] = rounded.clamp(-32768.0, 32767.0) as q15;
138    }
139    Status::Success
140}
141
142pub fn f32_to_q31(src: &[f32], dst: &mut [q31]) {
143    let len = src.len().min(dst.len());
144    for i in 0..len {
145        dst[i] = (src[i] * 2147483648.0).clamp(-2147483648.0, 2147483647.0) as q31;
146    }
147}
148
149// --- Sorting, Barycenter, Weighted Sum ---
150
151/// Insertion sort for f32 (ascending or descending order).
152pub fn sort_f32(src: &[f32], dst: &mut [f32], dir_ascending: bool) {
153    let len = src.len().min(dst.len());
154    dst[..len].copy_from_slice(&src[..len]);
155    let slice = &mut dst[..len];
156    for i in 1..len {
157        let mut j = i;
158        while j > 0 {
159            let swap_needed = if dir_ascending {
160                slice[j - 1] > slice[j]
161            } else {
162                slice[j - 1] < slice[j]
163            };
164            if swap_needed {
165                slice.swap(j - 1, j);
166                j -= 1;
167            } else {
168                break;
169            }
170        }
171    }
172}
173
174/// Compute barycenter of points weighted by given weights.
175pub fn barycenter_f32(
176    in_pts: &[f32],
177    weights: &[f32],
178    out_center: &mut [f32],
179    num_vecs: usize,
180    vec_dim: usize,
181) -> Status {
182    if in_pts.len() < num_vecs * vec_dim || weights.len() < num_vecs || out_center.len() < vec_dim {
183        return Status::LengthError;
184    }
185    out_center[..vec_dim].fill(0.0);
186    let mut weight_sum = 0.0f32;
187    for i in 0..num_vecs {
188        let w = weights[i];
189        weight_sum += w;
190        for d in 0..vec_dim {
191            out_center[d] += in_pts[i * vec_dim + d] * w;
192        }
193    }
194    if weight_sum != 0.0 {
195        for d in 0..vec_dim {
196            out_center[d] /= weight_sum;
197        }
198    }
199    Status::Success
200}
201
202/// Compute weighted sum of values.
203pub fn weighted_sum_f32(in_vals: &[f32], weights: &[f32]) -> f32 {
204    let len = in_vals.len().min(weights.len());
205    let mut sum = 0.0f32;
206    let mut w_sum = 0.0f32;
207    for i in 0..len {
208        sum += in_vals[i] * weights[i];
209        w_sum += weights[i];
210    }
211    if w_sum != 0.0 { sum / w_sum } else { 0.0 }
212}
213
214// --- Pseudo-Random Number Generation & Noise ---
215
216#[allow(unused_imports)]
217use crate::math::FloatMath;
218
219/// Simple deterministic zero-allocation 64-bit XorShift Pseudo-Random Number Generator.
220#[derive(Debug, Clone, Copy, PartialEq, Eq)]
221pub struct XorShift64 {
222    pub state: u64,
223}
224
225impl XorShift64 {
226    pub const fn new(seed: u64) -> Self {
227        Self {
228            state: if seed == 0 { 0x853c49e6748fea9b } else { seed },
229        }
230    }
231
232    #[inline]
233    pub fn next_u64(&mut self) -> u64 {
234        let mut x = self.state;
235        x ^= x << 13;
236        x ^= x >> 7;
237        x ^= x << 17;
238        self.state = x;
239        x
240    }
241
242    #[inline]
243    pub fn next_f32(&mut self) -> f32 {
244        // Generates uniform float in (0, 1]
245        let val = (self.next_u64() >> 40) as u32;
246        ((val as f32) + 1.0) / 16777217.0
247    }
248}
249
250/// Fill destination slice with uniformly distributed random noise in `[min_val, max_val]`.
251pub fn uniform_noise_f32(dst: &mut [f32], min_val: f32, max_val: f32, seed: &mut u64) {
252    let mut rng = XorShift64::new(*seed);
253    let span = max_val - min_val;
254    for x in dst.iter_mut() {
255        *x = min_val + rng.next_f32() * span;
256    }
257    *seed = rng.state;
258}
259
260/// Fill destination slice with Gaussian (White Noise) samples of given `mean` and `std_dev` using the Box-Muller transform.
261pub fn gaussian_noise_f32(dst: &mut [f32], mean: f32, std_dev: f32, seed: &mut u64) {
262    let mut rng = XorShift64::new(*seed);
263    let len = dst.len();
264    let mut i = 0;
265    while i < len {
266        let u1 = rng.next_f32();
267        let u2 = rng.next_f32();
268        let r = (-2.0f32 * u1.ln()).sqrt() * std_dev;
269        let theta = 2.0f32 * core::f32::consts::PI * u2;
270        dst[i] = mean + r * theta.cos();
271        if i + 1 < len {
272            dst[i + 1] = mean + r * theta.sin();
273        }
274        i += 2;
275    }
276    *seed = rng.state;
277}
278
279/// Quantize f32 biquad SOS coeffs (`[b0,b1,b2,a1,a2]` per stage) to Q15.
280///
281/// Stores `coeff / 2^{post_shift} * 2^{15}` so values with magnitude `>= 1` fit in Q15.
282/// [`crate::filtering::BiquadCascadeInstanceQ15`].
283pub fn biquad_coeffs_f32_to_q15(src: &[f32], dst: &mut [q15], post_shift: u8) -> Status {
284    if src.len() != dst.len() || src.is_empty() || src.len() % 5 != 0 {
285        return Status::LengthError;
286    }
287    let scale = 32768.0 / ((1u32 << post_shift.min(14)) as f32);
288    for i in 0..src.len() {
289        dst[i] = (src[i] * scale).clamp(-32768.0, 32767.0) as q15;
290    }
291    Status::Success
292}
293
294/// Quantize f32 biquad SOS coeffs to Q31 (`coeff / 2^{post_shift} * 2^{31}`).
295pub fn biquad_coeffs_f32_to_q31(src: &[f32], dst: &mut [q31], post_shift: u8) -> Status {
296    if src.len() != dst.len() || src.is_empty() || src.len() % 5 != 0 {
297        return Status::LengthError;
298    }
299    let scale = 2147483648.0 / ((1u32 << post_shift.min(14)) as f32);
300    for i in 0..src.len() {
301        dst[i] = (src[i] * scale).clamp(-2147483648.0, 2147483647.0) as q31;
302    }
303    Status::Success
304}
embedded_dsp/support.rs

embedded_dsp/
support.rs
