use crate::backend::{Backend, Fft};
use crate::data::{Layout, Tomo};
use crate::dtype::Complex32;
use crate::error::{Error, Result};
use ndarray::{ArrayView2, Axis, Slice};
const BLOCK: usize = 128;
const STRIDE: usize = 64;
const DX_WINDOW: usize = 12;
const MIN_QUALITY: f32 = 0.3;
const MIN_BLOCKS: usize = 10;
const STARTS: usize = 8;
#[derive(Debug, Clone, PartialEq)]
pub struct TiltFlow {
pub tilt_deg: f64,
pub tilt_sigma_deg: f64,
pub roll_deg: f64,
pub roll_sigma_deg: f64,
pub axis_col: f64,
pub axis_col_sigma: f64,
pub pairs: Vec<FlowPair>,
}
#[derive(Debug, Clone, PartialEq)]
pub struct FlowPair {
pub first: usize,
pub gap: usize,
pub dtheta_deg: f64,
pub tilt_deg: f64,
pub roll_deg: f64,
pub axis_col: f64,
pub blocks: usize,
pub resid_px: f64,
}
pub fn estimate_tilt_flow(
tomo: &Tomo<f32>,
theta: &[f32],
backend: &dyn Backend,
) -> Result<TiltFlow> {
let fft = backend.fft().ok_or_else(|| Error::MissingCapability {
backend: backend.name(),
capability: "Fft",
})?;
let p = tomo.as_layout(Layout::Projection);
let (nproj, ny, nx) = p.array.dim();
if theta.len() != nproj {
return Err(Error::ShapeMismatch {
expected: format!("{nproj} angles"),
found: format!("{}", theta.len()),
});
}
if nproj < 2 || ny < 2 * BLOCK || nx < 2 * BLOCK {
return Err(Error::InvalidParam(format!(
"estimate_tilt_flow needs ≥2 projections of ≥{0}×{0} px; got {nproj}×{ny}×{nx}",
2 * BLOCK
)));
}
let step = (f64::from(theta[nproj - 1]) - f64::from(theta[0])).abs() / (nproj - 1) as f64;
if !(step > 0.0 && step.is_finite()) {
return Err(Error::InvalidParam(
"estimate_tilt_flow: the angles do not advance".into(),
));
}
let k_max = ((DX_WINDOW as f64 / (nx as f64 / 2.0 * step)).round() as usize).max(1);
let gaps = k_max.saturating_sub(2).max(1)..=k_max;
let window = hann(BLOCK);
let origins: Vec<(usize, usize)> = (0..=ny - BLOCK)
.step_by(STRIDE)
.flat_map(|y| (0..=nx - BLOCK).step_by(STRIDE).map(move |x| (y, x)))
.collect();
let mut pairs = Vec::new();
for m in 0..STARTS {
let first = m * nproj / STARTS;
for gap in gaps.clone() {
if first + gap >= nproj {
continue;
}
let shifts = block_shifts(
p.array.index_axis(Axis(0), first),
p.array.index_axis(Axis(0), first + gap),
&origins,
&window,
fft,
)?;
let Some((blocks, [sx, sz, c], resid_px)) = fit_plane(&shifts) else {
continue;
};
let dtheta = f64::from(theta[first + gap]) - f64::from(theta[first]);
pairs.push(FlowPair {
first,
gap,
dtheta_deg: dtheta.to_degrees(),
tilt_deg: (sx.abs() / dtheta.abs()).min(1.0).asin().to_degrees(),
roll_deg: (sz / sx).atan().to_degrees(),
axis_col: -(c + sz * (ny - 1) as f64 / 2.0) / sx,
blocks,
resid_px,
});
}
}
if pairs.is_empty() {
return Err(Error::InvalidParam(format!(
"estimate_tilt_flow: no pair had {MIN_BLOCKS} blocks correlating above {MIN_QUALITY}"
)));
}
let (tilt_deg, tilt_sigma_deg) = mean_sigma(pairs.iter().map(|p| p.tilt_deg));
let (roll_deg, roll_sigma_deg) = mean_sigma(pairs.iter().map(|p| p.roll_deg));
let (axis_col, axis_col_sigma) = mean_sigma(pairs.iter().map(|p| p.axis_col));
Ok(TiltFlow {
tilt_deg,
tilt_sigma_deg,
roll_deg,
roll_sigma_deg,
axis_col,
axis_col_sigma,
pairs,
})
}
fn hann(n: usize) -> Vec<f32> {
(0..n)
.map(|i| 0.5 - 0.5 * (std::f64::consts::TAU * i as f64 / (n - 1) as f64).cos() as f32)
.collect()
}
struct BlockShift {
x: f64,
y: f64,
dz: f64,
quality: f32,
}
fn block_shifts(
a: ArrayView2<f32>,
b: ArrayView2<f32>,
origins: &[(usize, usize)],
window: &[f32],
fft: &dyn Fft,
) -> Result<Vec<BlockShift>> {
let n = BLOCK * BLOCK;
let mut fa = vec![Complex32::new(0.0, 0.0); origins.len() * n];
let mut fb = fa.clone();
let mut norms = Vec::with_capacity(origins.len());
for (k, &(y0, x0)) in origins.iter().enumerate() {
let ea = windowed(a, y0, x0, window, &mut fa[k * n..(k + 1) * n]);
let eb = windowed(b, y0, x0, window, &mut fb[k * n..(k + 1) * n]);
norms.push((ea * eb + 1e-12).sqrt());
}
fft.fft_2d(&mut fa, BLOCK, BLOCK, origins.len(), false)?;
fft.fft_2d(&mut fb, BLOCK, BLOCK, origins.len(), false)?;
for (b, a) in fb.iter_mut().zip(&fa) {
*b *= a.conj();
}
fft.fft_2d(&mut fb, BLOCK, BLOCK, origins.len(), true)?;
let half = BLOCK / 2;
let lag = |l: isize| l.rem_euclid(BLOCK as isize) as usize;
Ok(origins
.iter()
.enumerate()
.map(|(k, &(y0, x0))| {
let cc = &fb[k * n..(k + 1) * n];
let peak = cc.iter().map(|c| c.re).fold(f32::NEG_INFINITY, f32::max);
let prof: Vec<f32> = (0..BLOCK)
.map(|p| {
let row = lag(p as isize - half as isize) * BLOCK;
(-(DX_WINDOW as isize)..=DX_WINDOW as isize)
.map(|dx| cc[row + lag(dx)].re)
.fold(f32::NEG_INFINITY, f32::max)
})
.collect();
let p = (0..BLOCK).fold(0, |m, i| if prof[i] > prof[m] { i } else { m });
let mut dz = p as f64;
if p > 0 && p < BLOCK - 1 {
let (l, c, r) = (prof[p - 1] as f64, prof[p] as f64, prof[p + 1] as f64);
let den = l - 2.0 * c + r;
if den != 0.0 {
dz += 0.5 * (l - r) / den;
}
}
let mid = (BLOCK - 1) as f64 / 2.0;
BlockShift {
x: x0 as f64 + mid,
y: y0 as f64 + mid,
dz: dz - half as f64,
quality: (f64::from(peak) / norms[k]) as f32,
}
})
.collect())
}
fn windowed(
img: ArrayView2<f32>,
y0: usize,
x0: usize,
window: &[f32],
out: &mut [Complex32],
) -> f64 {
let blk = img
.slice_axis_move(Axis(0), Slice::from(y0..y0 + BLOCK))
.slice_axis_move(Axis(1), Slice::from(x0..x0 + BLOCK));
let mean = blk.iter().map(|&v| f64::from(v)).sum::<f64>() / (BLOCK * BLOCK) as f64;
let mut energy = 0.0;
for ((y, x), &v) in blk.indexed_iter() {
let w = window[y] * window[x];
let v = ((f64::from(v) - mean) as f32) * w;
out[y * BLOCK + x] = Complex32::new(v, 0.0);
energy += f64::from(v) * f64::from(v);
}
energy
}
fn fit_plane(shifts: &[BlockShift]) -> Option<(usize, [f64; 3], f64)> {
let good: Vec<&BlockShift> = shifts.iter().filter(|s| s.quality > MIN_QUALITY).collect();
if good.len() < MIN_BLOCKS {
return None;
}
let sol = plane(good.iter().copied())?;
let resid = |s: &BlockShift| s.dz - (sol[0] * s.x + sol[1] * s.y + sol[2]);
let (_, sigma) = mean_sigma(good.iter().map(|s| resid(s)));
let kept: Vec<&BlockShift> = good
.iter()
.copied()
.filter(|s| resid(s).abs() <= 2.5 * sigma)
.collect();
let [sx, sz, c] = plane(kept.iter().copied())?;
let rms = (kept
.iter()
.map(|s| (s.dz - (sx * s.x + sz * s.y + c)).powi(2))
.sum::<f64>()
/ kept.len() as f64)
.sqrt();
Some((good.len(), [sx, sz, c], rms))
}
fn plane<'a>(pts: impl Iterator<Item = &'a BlockShift>) -> Option<[f64; 3]> {
let (mut m, mut v) = ([[0.0f64; 3]; 3], [0.0f64; 3]);
for s in pts {
let r = [s.x, s.y, 1.0];
for i in 0..3 {
for j in 0..3 {
m[i][j] += r[i] * r[j];
}
v[i] += r[i] * s.dz;
}
}
let det = |m: &[[f64; 3]; 3]| {
m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
- m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
+ m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])
};
let d = det(&m);
if d == 0.0 || !d.is_finite() {
return None;
}
Some(std::array::from_fn(|i| {
let mut mi = m;
for (row, &vi) in mi.iter_mut().zip(&v) {
row[i] = vi;
}
det(&mi) / d
}))
}
fn mean_sigma(xs: impl Iterator<Item = f64> + Clone) -> (f64, f64) {
let n = xs.clone().count() as f64;
let mean = xs.clone().sum::<f64>() / n;
let var = xs.map(|x| (x - mean).powi(2)).sum::<f64>() / n;
(mean, var.sqrt())
}