use crate::err::try_vec;
use crate::factory::gen_psi;
use crate::sample::CwtSample;
use crate::{CwtWavelet, ScaletError};
use num_traits::AsPrimitive;
use std::cmp::Ordering;
use std::sync::Arc;
pub(crate) fn scale_to_frequencies_impl<T: CwtSample>(
wavelet: Arc<dyn CwtWavelet<T> + Send + Sync>,
scales: &[T],
sampling_frequency: T,
filter_length: usize,
) -> Result<Vec<T>, ScaletError>
where
usize: AsPrimitive<T>,
f64: AsPrimitive<T>,
isize: AsPrimitive<T>,
{
if filter_length == 0 {
return Err(ScaletError::ZeroBaseSized);
}
let psi = gen_psi(filter_length)?;
let mut max_indices = try_vec![0usize; scales.len()];
let mut current_psi = try_vec![T::zero(); filter_length];
for (index, &scale) in max_indices.iter_mut().zip(scales.iter()) {
for (dst, &psi) in current_psi.iter_mut().zip(psi.iter()) {
*dst = psi * scale;
}
let wavelet_fft = wavelet.make_wavelet(¤t_psi)?;
if wavelet_fft.len() != filter_length {
return Err(ScaletError::WaveletInvalidSize(
filter_length,
wavelet_fft.len(),
));
}
let idx = wavelet_fft
.iter()
.enumerate() .max_by(|a, b| a.1.re.partial_cmp(&b.1.re).unwrap_or(Ordering::Equal)) .map(|(idx, _)| idx);
*index = idx.unwrap_or(0);
}
let mut freqs = try_vec![T::zero(); scales.len()];
let idx_scale = sampling_frequency / filter_length.as_();
for (dst, idx) in freqs.iter_mut().zip(&max_indices) {
*dst = idx.as_() * idx_scale;
}
Ok(freqs)
}