use ndarray::{Array2, ArrayView1, ArrayView2, Zip};
use num_complex::Complex64;
use crate::error::GspError;
pub(super) fn arrays_close(a: ArrayView1<'_, f64>, b: ArrayView1<'_, f64>, atol: f64) -> bool {
if a.len() != b.len() {
return false;
}
a.iter().zip(b.iter()).all(|(x, y)| (*x - *y).abs() <= atol)
}
pub(super) fn scalar_close(a: f64, b: f64, rtol: f64) -> bool {
(a - b).abs() <= rtol * a.abs().max(b.abs()).max(1.0)
}
pub(super) fn real_times_complex(
a: ArrayView2<f64>,
b: ArrayView2<Complex64>,
) -> Array2<Complex64> {
let mut out = Array2::<Complex64>::zeros((a.nrows(), b.ncols()));
for i in 0..a.nrows() {
for j in 0..b.ncols() {
let mut sum = Complex64::new(0.0, 0.0);
for k in 0..a.ncols() {
sum += Complex64::new(a[[i, k]], 0.0) * b[[k, j]];
}
out[[i, j]] = sum;
}
}
out
}
pub(super) fn peak_local_max(
image: ArrayView2<'_, f64>,
min_distance: usize,
num_peaks: usize,
) -> Vec<(usize, usize)> {
let mut coords = Vec::<(usize, usize, f64)>::new();
for r in 0..image.nrows() {
for c in 0..image.ncols() {
let center = image[[r, c]];
if center <= 0.0 {
continue;
}
let r_lo = r.saturating_sub(min_distance);
let r_hi = (r + min_distance + 1).min(image.nrows());
let c_lo = c.saturating_sub(min_distance);
let c_hi = (c + min_distance + 1).min(image.ncols());
let mut is_max = true;
'outer: for rr in r_lo..r_hi {
for cc in c_lo..c_hi {
if image[[rr, cc]] > center {
is_max = false;
break 'outer;
}
}
}
if is_max {
coords.push((r, c, center));
}
}
}
coords.sort_by(|a, b| b.2.partial_cmp(&a.2).unwrap_or(std::cmp::Ordering::Equal));
let mut suppressed = vec![false; image.nrows() * image.ncols()];
let mut out = Vec::new();
for (r, c, _) in coords {
let idx = r * image.ncols() + c;
if suppressed[idx] {
continue;
}
out.push((r, c));
if out.len() >= num_peaks {
break;
}
let r_lo = r.saturating_sub(min_distance);
let r_hi = (r + min_distance + 1).min(image.nrows());
let c_lo = c.saturating_sub(min_distance);
let c_hi = (c + min_distance + 1).min(image.ncols());
for rr in r_lo..r_hi {
for cc in c_lo..c_hi {
suppressed[rr * image.ncols() + cc] = true;
}
}
}
out
}
pub(super) fn gradient_real_axis(
a: ArrayView2<'_, f64>,
grid: ArrayView1<'_, f64>,
axis: usize,
) -> Result<Array2<f64>, GspError> {
let mut g = Array2::<f64>::zeros(a.raw_dim());
if axis == 0 {
if a.nrows() < 2 || grid.len() < 2 || a.nrows() != grid.len() {
return Err(GspError::Dimensions(format!(
"axis-0 gradient requires at least 2 rows and matching grid length; got matrix {}x{} and grid {}",
a.nrows(),
a.ncols(),
grid.len()
)));
}
for j in 0..a.ncols() {
g[[0, j]] = (a[[1, j]] - a[[0, j]]) / (grid[1] - grid[0]);
for i in 1..(a.nrows() - 1) {
g[[i, j]] = (a[[i + 1, j]] - a[[i - 1, j]]) / (grid[i + 1] - grid[i - 1]);
}
g[[a.nrows() - 1, j]] = (a[[a.nrows() - 1, j]] - a[[a.nrows() - 2, j]])
/ (grid[grid.len() - 1] - grid[grid.len() - 2]);
}
} else {
if a.ncols() < 2 || grid.len() < 2 || a.ncols() != grid.len() {
return Err(GspError::Dimensions(format!(
"axis-1 gradient requires at least 2 cols and matching grid length; got matrix {}x{} and grid {}",
a.nrows(),
a.ncols(),
grid.len()
)));
}
for i in 0..a.nrows() {
g[[i, 0]] = (a[[i, 1]] - a[[i, 0]]) / (grid[1] - grid[0]);
for j in 1..(a.ncols() - 1) {
g[[i, j]] = (a[[i, j + 1]] - a[[i, j - 1]]) / (grid[j + 1] - grid[j - 1]);
}
g[[i, a.ncols() - 1]] = (a[[i, a.ncols() - 1]] - a[[i, a.ncols() - 2]])
/ (grid[grid.len() - 1] - grid[grid.len() - 2]);
}
}
Ok(g)
}
pub(super) fn gradient_complex_axis(
a: ArrayView2<'_, Complex64>,
grid: ArrayView1<'_, f64>,
axis: usize,
) -> Result<Array2<Complex64>, GspError> {
let real = gradient_real_axis(a.mapv(|z| z.re).view(), grid, axis)?;
let imag = gradient_real_axis(a.mapv(|z| z.im).view(), grid, axis)?;
let mut out = Array2::<Complex64>::zeros(real.raw_dim());
Zip::from(&mut out)
.and(&real)
.and(&imag)
.for_each(|o, &r, &i| *o = Complex64::new(r, i));
Ok(out)
}
pub(super) fn stddev(x: ArrayView1<'_, f64>) -> f64 {
if x.is_empty() {
return 0.0;
}
let mean = x.sum() / x.len() as f64;
let var = x.iter().map(|v| (v - mean).powi(2)).sum::<f64>() / x.len() as f64;
var.sqrt()
}