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
122pub fn f32_to_q31(src: &[f32], dst: &mut [q31]) {
123    let len = src.len().min(dst.len());
124    for i in 0..len {
125        dst[i] = (src[i] * 2147483648.0).clamp(-2147483648.0, 2147483647.0) as q31;
126    }
127}
128
129// --- Sorting, Barycenter, Weighted Sum ---
130
131/// Insertion sort for f32 (ascending or descending order).
132pub fn sort_f32(src: &[f32], dst: &mut [f32], dir_ascending: bool) {
133    let len = src.len().min(dst.len());
134    dst[..len].copy_from_slice(&src[..len]);
135    let slice = &mut dst[..len];
136    for i in 1..len {
137        let mut j = i;
138        while j > 0 {
139            let swap_needed = if dir_ascending {
140                slice[j - 1] > slice[j]
141            } else {
142                slice[j - 1] < slice[j]
143            };
144            if swap_needed {
145                slice.swap(j - 1, j);
146                j -= 1;
147            } else {
148                break;
149            }
150        }
151    }
152}
153
154/// Compute barycenter of points weighted by given weights.
155pub fn barycenter_f32(
156    in_pts: &[f32],
157    weights: &[f32],
158    out_center: &mut [f32],
159    num_vecs: usize,
160    vec_dim: usize,
161) -> Status {
162    if in_pts.len() < num_vecs * vec_dim || weights.len() < num_vecs || out_center.len() < vec_dim {
163        return Status::LengthError;
164    }
165    out_center[..vec_dim].fill(0.0);
166    let mut weight_sum = 0.0f32;
167    for i in 0..num_vecs {
168        let w = weights[i];
169        weight_sum += w;
170        for d in 0..vec_dim {
171            out_center[d] += in_pts[i * vec_dim + d] * w;
172        }
173    }
174    if weight_sum != 0.0 {
175        for d in 0..vec_dim {
176            out_center[d] /= weight_sum;
177        }
178    }
179    Status::Success
180}
181
182/// Compute weighted sum of values.
183pub fn weighted_sum_f32(in_vals: &[f32], weights: &[f32]) -> f32 {
184    let len = in_vals.len().min(weights.len());
185    let mut sum = 0.0f32;
186    let mut w_sum = 0.0f32;
187    for i in 0..len {
188        sum += in_vals[i] * weights[i];
189        w_sum += weights[i];
190    }
191    if w_sum != 0.0 { sum / w_sum } else { 0.0 }
192}
193
194// --- Pseudo-Random Number Generation & Noise ---
195
196#[allow(unused_imports)]
197use crate::math::FloatMath;
198
199/// Simple deterministic zero-allocation 64-bit XorShift Pseudo-Random Number Generator.
200#[derive(Debug, Clone, Copy, PartialEq, Eq)]
201pub struct XorShift64 {
202    pub state: u64,
203}
204
205impl XorShift64 {
206    pub const fn new(seed: u64) -> Self {
207        Self {
208            state: if seed == 0 { 0x853c49e6748fea9b } else { seed },
209        }
210    }
211
212    #[inline]
213    pub fn next_u64(&mut self) -> u64 {
214        let mut x = self.state;
215        x ^= x << 13;
216        x ^= x >> 7;
217        x ^= x << 17;
218        self.state = x;
219        x
220    }
221
222    #[inline]
223    pub fn next_f32(&mut self) -> f32 {
224        // Generates uniform float in (0, 1]
225        let val = (self.next_u64() >> 40) as u32;
226        ((val as f32) + 1.0) / 16777217.0
227    }
228}
229
230/// Fill destination slice with uniformly distributed random noise in `[min_val, max_val]`.
231pub fn uniform_noise_f32(dst: &mut [f32], min_val: f32, max_val: f32, seed: &mut u64) {
232    let mut rng = XorShift64::new(*seed);
233    let span = max_val - min_val;
234    for x in dst.iter_mut() {
235        *x = min_val + rng.next_f32() * span;
236    }
237    *seed = rng.state;
238}
239
240/// Fill destination slice with Gaussian (White Noise) samples of given `mean` and `std_dev` using the Box-Muller transform.
241pub fn gaussian_noise_f32(dst: &mut [f32], mean: f32, std_dev: f32, seed: &mut u64) {
242    let mut rng = XorShift64::new(*seed);
243    let len = dst.len();
244    let mut i = 0;
245    while i < len {
246        let u1 = rng.next_f32();
247        let u2 = rng.next_f32();
248        let r = (-2.0f32 * u1.ln()).sqrt() * std_dev;
249        let theta = 2.0f32 * core::f32::consts::PI * u2;
250        dst[i] = mean + r * theta.cos();
251        if i + 1 < len {
252            dst[i + 1] = mean + r * theta.sin();
253        }
254        i += 2;
255    }
256    *seed = rng.state;
257}
embedded_dsp/support.rs

embedded_dsp/
support.rs
