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] = q15::from_num(src[i]);
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] = q31::from_num(src[i]);
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].to_num();
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] = q7::from_num(src[i]);
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] = q31::from_num(src[i]);
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].to_num();
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] = q7::from_num(src[i]);
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] = q15::from_num(src[i]);
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].to_num();
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] = q7::saturating_from_num(src[i]);
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] = q15::saturating_from_num(src[i]);
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        dst[i] = q15::saturating_from_num(src[i]);
132    }
133    Status::Success
134}
135
136pub fn f32_to_q31(src: &[f32], dst: &mut [q31]) {
137    let len = src.len().min(dst.len());
138    for i in 0..len {
139        dst[i] = q31::saturating_from_num(src[i]);
140    }
141}
142
143// --- Sorting, Barycenter, Weighted Sum ---
144
145/// Insertion sort for f32 (ascending or descending order).
146pub fn sort_f32(src: &[f32], dst: &mut [f32], dir_ascending: bool) {
147    let len = src.len().min(dst.len());
148    dst[..len].copy_from_slice(&src[..len]);
149    let slice = &mut dst[..len];
150    for i in 1..len {
151        let mut j = i;
152        while j > 0 {
153            let swap_needed = if dir_ascending {
154                slice[j - 1] > slice[j]
155            } else {
156                slice[j - 1] < slice[j]
157            };
158            if swap_needed {
159                slice.swap(j - 1, j);
160                j -= 1;
161            } else {
162                break;
163            }
164        }
165    }
166}
167
168/// Compute barycenter of points weighted by given weights.
169pub fn barycenter_f32(
170    in_pts: &[f32],
171    weights: &[f32],
172    out_center: &mut [f32],
173    num_vecs: usize,
174    vec_dim: usize,
175) -> Status {
176    if in_pts.len() < num_vecs * vec_dim || weights.len() < num_vecs || out_center.len() < vec_dim {
177        return Status::LengthError;
178    }
179    out_center[..vec_dim].fill(0.0);
180    let mut weight_sum = 0.0f32;
181    for i in 0..num_vecs {
182        let w = weights[i];
183        weight_sum += w;
184        for d in 0..vec_dim {
185            out_center[d] += in_pts[i * vec_dim + d] * w;
186        }
187    }
188    if weight_sum != 0.0 {
189        for d in 0..vec_dim {
190            out_center[d] /= weight_sum;
191        }
192    }
193    Status::Success
194}
195
196/// Compute weighted sum of values.
197pub fn weighted_sum_f32(in_vals: &[f32], weights: &[f32]) -> f32 {
198    let len = in_vals.len().min(weights.len());
199    let mut sum = 0.0f32;
200    let mut w_sum = 0.0f32;
201    for i in 0..len {
202        sum += in_vals[i] * weights[i];
203        w_sum += weights[i];
204    }
205    if w_sum != 0.0 { sum / w_sum } else { 0.0 }
206}
207
208// --- Pseudo-Random Number Generation & Noise ---
209
210#[allow(unused_imports)]
211use crate::math::FloatMath;
212
213/// Simple deterministic zero-allocation 64-bit XorShift Pseudo-Random Number Generator.
214#[derive(Debug, Clone, Copy, PartialEq, Eq)]
215pub struct XorShift64 {
216    pub state: u64,
217}
218
219impl XorShift64 {
220    pub const fn new(seed: u64) -> Self {
221        Self {
222            state: if seed == 0 { 0x853c49e6748fea9b } else { seed },
223        }
224    }
225
226    #[inline]
227    pub fn next_u64(&mut self) -> u64 {
228        let mut x = self.state;
229        x ^= x << 13;
230        x ^= x >> 7;
231        x ^= x << 17;
232        self.state = x;
233        x
234    }
235
236    #[inline]
237    pub fn next_f32(&mut self) -> f32 {
238        // Generates uniform float in (0, 1]
239        let val = (self.next_u64() >> 40) as u32;
240        ((val as f32) + 1.0) / 16777217.0
241    }
242}
243
244/// Fill destination slice with uniformly distributed random noise in `[min_val, max_val]`.
245pub fn uniform_noise_f32(dst: &mut [f32], min_val: f32, max_val: f32, seed: &mut u64) {
246    let mut rng = XorShift64::new(*seed);
247    let span = max_val - min_val;
248    for x in dst.iter_mut() {
249        *x = min_val + rng.next_f32() * span;
250    }
251    *seed = rng.state;
252}
253
254/// Fill destination slice with Gaussian (White Noise) samples of given `mean` and `std_dev` using the Box-Muller transform.
255pub fn gaussian_noise_f32(dst: &mut [f32], mean: f32, std_dev: f32, seed: &mut u64) {
256    let mut rng = XorShift64::new(*seed);
257    let len = dst.len();
258    let mut i = 0;
259    while i < len {
260        let u1 = rng.next_f32();
261        let u2 = rng.next_f32();
262        let r = (-2.0f32 * u1.ln()).sqrt() * std_dev;
263        let theta = 2.0f32 * core::f32::consts::PI * u2;
264        dst[i] = mean + r * theta.cos();
265        if i + 1 < len {
266            dst[i + 1] = mean + r * theta.sin();
267        }
268        i += 2;
269    }
270    *seed = rng.state;
271}
272
273/// Quantize f32 biquad SOS coeffs (`[b0,b1,b2,a1,a2]` per stage) to Q15.
274///
275/// Stores `coeff / 2^{post_shift} * 2^{15}` so values with magnitude `>= 1` fit in Q15.
276/// [`crate::filtering::BiquadCascadeInstanceQ15`].
277pub fn biquad_coeffs_f32_to_q15(src: &[f32], dst: &mut [q15], post_shift: u8) -> Status {
278    if src.len() != dst.len() || src.is_empty() || src.len() % 5 != 0 {
279        return Status::LengthError;
280    }
281    let post_scale = (1u32 << post_shift.min(14)) as f32;
282    for i in 0..src.len() {
283        dst[i] = q15::saturating_from_num(src[i] / post_scale);
284    }
285    Status::Success
286}
287
288/// Quantize f32 biquad SOS coeffs to Q31 (`coeff / 2^{post_shift} * 2^{31}`).
289pub fn biquad_coeffs_f32_to_q31(src: &[f32], dst: &mut [q31], post_shift: u8) -> Status {
290    if src.len() != dst.len() || src.is_empty() || src.len() % 5 != 0 {
291        return Status::LengthError;
292    }
293    let post_scale = (1u32 << post_shift.min(14)) as f32;
294    for i in 0..src.len() {
295        dst[i] = q31::saturating_from_num(src[i] / post_scale);
296    }
297    Status::Success
298}
embedded_dsp/support.rs

embedded_dsp/
support.rs
