use rayon::prelude::*;
use std::sync::LazyLock;
use wide::f32x4;
const M: f32 = 65535.0;
const TAU: f32 = 8847.23;
const SLOPE_LIMIT: f32 = 0.2;
const W_FLOOR: f32 = 0.02;
const B_ANCHOR: f32 = 0.98;
const PHI_ANCHOR: f32 = 0.065;
const MIN3_DPI: u32 = 550;
const DETAIL_GAIN: [f32; 2] = [1.25, 1.0];
const ALPHA: [f32; 3] = [0.015, 0.015, 0.025];
const CHUNK: usize = 1 << 16;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub enum Model {
#[default]
Ls9000,
Ls5000,
Ls50,
}
impl From<crate::protocol::model::Model> for Model {
fn from(model: crate::protocol::model::Model) -> Self {
use crate::protocol::model::Model as Unit;
match model {
Unit::Ls9000 | Unit::Ls8000 => Self::Ls9000,
Unit::Ls5000 | Unit::Ls4000 => Self::Ls5000,
Unit::Ls50 | Unit::Ls40 => Self::Ls50,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub enum Quality {
#[default]
Normal,
Fine,
}
struct Profile {
theta: f32,
gamma: [f32; 3],
dither: [f32; 2],
contrast_dpi: u32,
ramp: f32,
a: [[(f32, f32); 3]; 3],
}
const PROFILES: [Profile; 3] = [
Profile {
theta: 0.0,
gamma: [1.100; 3],
dither: [0.04, 0.96],
contrast_dpi: 950,
ramp: -960.42,
a: [
[(1.360, 1.320), (1.370, 1.300), (1.340, 1.250)],
[(1.370, 1.300), (1.350, 1.290), (1.300, 1.240)],
[(1.340, 1.250), (1.320, 1.250), (1.250, 1.210)],
],
},
Profile {
theta: 1.0,
gamma: [1.100; 3],
dither: [0.01, 0.99],
contrast_dpi: 1600,
ramp: -960.42,
a: [
[(1.210, 1.090), (1.170, 1.080), (1.040, 0.960)],
[(1.230, 1.130), (1.140, 1.050), (0.930, 0.840)],
[(1.130, 1.040), (1.080, 1.020), (0.970, 0.890)],
],
},
Profile {
theta: 1.0,
gamma: [1.000; 3],
dither: [0.01, 0.99],
contrast_dpi: 2500,
ramp: -960.52,
a: [
[(2.210, 2.090), (2.170, 2.080), (2.040, 1.960)],
[(2.230, 2.130), (2.140, 2.050), (1.930, 1.840)],
[(2.130, 2.040), (2.080, 2.020), (1.970, 1.890)],
],
},
];
const LEVEL0: [(i32, i32, i32); 9] = [
(-4, -2, 2),
(-3, -3, 3),
(-2, -4, 4),
(-1, -4, 4),
(0, -4, 4),
(1, -4, 4),
(2, -4, 4),
(3, -3, 3),
(4, -2, 2),
];
const LEVEL1: [(i32, i32, i32); 5] = [(-2, -1, 1), (-1, -2, 2), (0, -2, 2), (1, -2, 2), (2, -1, 1)];
const TENT: [f32; 3] = [1.0, 2.0, 1.0];
#[derive(Debug, Clone, Copy)]
pub struct Options {
pub model: Model,
pub quality: Quality,
pub dpi: u32,
pub metering_target: f32,
}
#[derive(Debug, Clone)]
pub struct Params {
c: f32,
ir_ref: f32,
theta: f32,
ramp_bias: f32,
ramp_s: f32,
phi: f32,
eta: [f32; 2],
gamma: [f32; 3],
a: [[(f32, f32); 3]; 3],
detail_gain: f32,
clamp_l3: bool,
min3: bool,
cross_beta: bool,
}
impl Params {
pub fn new(opts: &Options, cal: &Calibration) -> Self {
let profile = &PROFILES[opts.model as usize];
Self {
c: cal.c,
ir_ref: cal.ir_ref,
theta: profile.theta + theta_for_metering_target(opts.metering_target),
ramp_bias: cal.ir_ref + density(anchor(B_ANCHOR)) - M,
ramp_s: 1.0 / profile.ramp,
phi: density(anchor(PHI_ANCHOR)),
eta: profile.dither.map(|f| density(anchor(f))),
gamma: profile.gamma,
a: profile.a,
detail_gain: DETAIL_GAIN[opts.quality as usize],
clamp_l3: matches!(opts.quality, Quality::Normal),
min3: opts.dpi > MIN3_DPI,
cross_beta: opts.dpi > profile.contrast_dpi,
}
}
#[inline]
fn weight(&self, gate: f32) -> f32 {
(1.0 + (self.ramp_bias - gate) * self.ramp_s).clamp(W_FLOOR, 1.0)
}
}
fn anchor(fraction: f32) -> u16 {
(M * fraction) as u16
}
pub fn theta_for_metering_target(target: f32) -> f32 {
density(anchor(target.clamp(0.0, 1.0))) - M
}
#[inline]
fn density(v: u16) -> f32 {
(f32::from(v) + 1.0).log2() * (M / 16.0)
}
static LUT: LazyLock<Box<[f32]>> = LazyLock::new(|| (0..=u16::MAX).map(density).collect());
#[inline]
fn from_density_scalar(d: f32) -> u16 {
let v = (d * (16.0 / M)).exp2() - 1.0;
v.round().clamp(0.0, M) as u16
}
pub fn to_density(samples: &[u16]) -> Vec<f32> {
let lut = &*LUT;
let mut out = vec![0.0f32; samples.len()];
samples
.par_chunks(CHUNK)
.zip(out.par_chunks_mut(CHUNK))
.for_each(|(src, dst)| {
for (&s, d) in src.iter().zip(dst) {
*d = lut[usize::from(s)];
}
});
out
}
pub fn from_density(values: &[f32]) -> Vec<u16> {
let scale = f32x4::splat(16.0 / M);
let mut out = vec![0u16; values.len()];
values
.par_chunks(CHUNK)
.zip(out.par_chunks_mut(CHUNK))
.for_each(|(src, dst)| {
let mut lanes = src.chunks_exact(4).zip(dst.chunks_exact_mut(4));
for (s, d) in &mut lanes {
let v = (f32x4::new(s.try_into().expect("chunked by four")) * scale).exp2()
- f32x4::ONE;
let v = v.round_int().to_array();
d.copy_from_slice(&v.map(|v| v.clamp(0, i32::from(u16::MAX)) as u16));
}
let done = src.len() / 4 * 4;
for (&s, d) in src[done..].iter().zip(&mut dst[done..]) {
*d = from_density_scalar(s);
}
});
out
}
pub struct Prescan<'a> {
pub red: &'a [u16],
pub ir: &'a [u16],
pub rows: usize,
pub cols: usize,
}
#[derive(Debug, Clone)]
pub struct Calibration {
pub c: f32,
pub ir_ref: f32,
}
pub fn calibrate(prescan: &Prescan) -> Option<Calibration> {
let d_r = to_density(prescan.red);
let d_ir = to_density(prescan.ir);
let (num_r, num_ir, den) = (prescan.ir, &d_r, &d_ir)
.into_par_iter()
.filter(|&(&ir, _, _)| f32::from(ir) > TAU)
.fold(
|| (0.0f64, 0.0f64, 0.0f64),
|(num_r, num_ir, den), (&ir, &r, &ir_dens)| {
let w = f64::from(ir) * f64::from(ir);
(
num_r + w * f64::from(r),
num_ir + w * f64::from(ir_dens),
den + w,
)
},
)
.reduce(|| (0.0, 0.0, 0.0), |a, b| (a.0 + b.0, a.1 + b.1, a.2 + b.2));
if den == 0.0 {
return None;
}
let r_ref = (num_r / den) as f32;
let ir_raw = (num_ir / den) as f32;
let cols = prescan.cols;
let col_tiles = prescan.cols / 8;
let (num, den): (f64, f64) = (0..(prescan.rows / 8) * col_tiles)
.into_par_iter()
.flat_map_iter(|tile| {
let (row0, col0) = ((tile / col_tiles) * 8, (tile % col_tiles) * 8);
let idx = |dy: usize, dx: usize| (row0 + dy) * cols + (col0 + dx);
let mut quadrants: [Option<(f32, f32)>; 4] = [None; 4];
if (0..8).all(|dy| (0..8).all(|dx| f32::from(prescan.ir[idx(dy, dx)]) > TAU)) {
let (mut tile_r, mut tile_ir, mut tile_raw_ir) = (0.0f32, 0.0f32, 0.0f32);
for dy in 0..8 {
for dx in 0..8 {
let i = idx(dy, dx);
tile_r += d_r[i];
tile_ir += d_ir[i];
tile_raw_ir += f32::from(prescan.ir[i]);
}
}
let corners = [(0, 0), (0, 4), (4, 0), (4, 4)];
for (slot, (dy0, dx0)) in quadrants.iter_mut().zip(corners) {
let (mut q_r, mut q_ir) = (0.0f32, 0.0f32);
for dy in 0..4 {
for dx in 0..4 {
let i = idx(dy0 + dy, dx0 + dx);
q_r += d_r[i];
q_ir += d_ir[i];
}
}
let delta_r = q_r / 16.0 - tile_r / 64.0;
let delta_ir = q_ir / 16.0 - tile_ir / 64.0;
let slope = delta_ir / delta_r;
if slope.is_finite() && slope.abs() <= SLOPE_LIMIT {
*slot = Some((slope, delta_r * delta_r * tile_raw_ir * tile_raw_ir));
}
}
}
quadrants.into_iter().flatten()
})
.fold(
|| (0.0f64, 0.0f64),
|(num, den), (slope, weight)| {
(
num + f64::from(slope) * f64::from(weight),
den + f64::from(weight),
)
},
)
.reduce(|| (0.0, 0.0), |a, b| (a.0 + b.0, a.1 + b.1));
let c = if den > 0.0 { (num / den) as f32 } else { 0.0 };
Some(Calibration {
c,
ir_ref: (ir_raw - c * r_ref) / (1.0 - c),
})
}
pub fn gate(red: &[u16], ir: &[u16], p: &Params) -> Vec<f32> {
debug_assert_eq!(red.len(), ir.len(), "one red and one IR sample per pixel");
let lut = &*LUT;
let (c, inv_c) = (p.c, 1.0 / (1.0 - p.c));
let mut g = vec![0.0f32; ir.len()];
ir.par_chunks(CHUNK)
.zip(red.par_chunks(CHUNK))
.zip(g.par_chunks_mut(CHUNK))
.for_each(|((ir, red), g)| {
for ((&ir, &red), g) in ir.iter().zip(red).zip(g) {
*g = (lut[usize::from(ir)] - c * lut[usize::from(red)]) * inv_c - p.theta;
}
});
g
}
pub fn confidence(g: &[f32], cols: usize, p: &Params) -> Vec<f32> {
let mut w = vec![0.0f32; g.len()];
g.par_chunks(cols)
.zip(w.par_chunks_mut(cols))
.for_each(|(g, w)| {
if !p.min3 {
for (&g, w) in g.iter().zip(w) {
*w = p.weight(g);
}
return;
}
let last = cols - 1;
for x in [0, last] {
w[x] = p.weight(g[x.saturating_sub(1)].min(g[x]).min(g[(x + 1).min(last)]));
}
if cols >= 3 {
let (lo, mid, hi) = (&g[..cols - 2], &g[1..last], &g[2..]);
for (((&lo, &mid), &hi), w) in lo.iter().zip(mid).zip(hi).zip(&mut w[1..last]) {
*w = p.weight(lo.min(mid).min(hi));
}
}
});
w
}
fn and3_cols(src: &[bool], cols: usize, k: usize) -> Vec<bool> {
let mut out = vec![false; src.len()];
src.par_chunks(cols)
.zip(out.par_chunks_mut(cols))
.for_each(|(src, dst)| {
let body = cols.saturating_sub(2 * k);
let (lo, mid, hi) = (&src[..body], &src[k.min(cols)..], &src[(2 * k).min(cols)..]);
for (((&lo, &mid), &hi), d) in lo.iter().zip(mid).zip(hi).zip(&mut dst[k.min(cols)..]) {
*d = lo && mid && hi;
}
for x in (0..k.min(cols)).chain(cols.saturating_sub(k)..cols) {
dst[x] = src[x.saturating_sub(k)] && src[x] && src[(x + k).min(cols - 1)];
}
});
out
}
fn and3_rows(src: &[bool], rows: usize, cols: usize, k: usize) -> Vec<bool> {
let mut out = vec![false; src.len()];
out.par_chunks_mut(cols).enumerate().for_each(|(y, dst)| {
let up = &src[y.saturating_sub(k) * cols..][..cols];
let mid = &src[y * cols..][..cols];
let down = &src[(y + k).min(rows - 1) * cols..][..cols];
for (((&up, &mid), &down), d) in up.iter().zip(mid).zip(down).zip(dst) {
*d = up && mid && down;
}
});
out
}
pub fn decide(g: &[f32], w: &[f32], rows: usize, cols: usize, p: &Params) -> Vec<bool> {
debug_assert_eq!(g.len(), w.len(), "one gate and one weight per pixel");
debug_assert_eq!(g.len(), rows * cols, "rows * cols must cover the plane");
let dark: Vec<bool> = g.par_iter().map(|&g| g < p.phi).collect();
let row_dark = and3_cols(&and3_cols(&dark, cols, 1), cols, 3);
let col_dark = and3_rows(&and3_rows(&dark, rows, cols, 1), rows, cols, 3);
drop(dark);
let mut mask = vec![false; g.len()];
mask.par_chunks_mut(cols).enumerate().for_each(|(y, mask)| {
let above = &row_dark[y.saturating_sub(4) * cols..][..cols];
let below = &row_dark[(y + 4).min(rows - 1) * cols..][..cols];
let sides = &col_dark[y * cols..][..cols];
let w = &w[y * cols..][..cols];
for (x, m) in mask.iter_mut().enumerate() {
if p.clamp_l3 && w[x] >= 1.0 {
continue;
}
*m = !(above[x]
|| below[x]
|| sides[x.saturating_sub(4)]
|| sides[(x + 4).min(cols - 1)]);
}
});
mask
}
#[derive(Default)]
struct Levels {
c: [f32; 4],
p: [f32; 4],
l: [[f32; 4]; 3],
}
struct Frame<'a> {
g: &'a [f32],
w: &'a [f32],
colors: [&'a [u16]; 3],
lut: &'a [f32],
rows: usize,
cols: usize,
}
#[derive(Default, Clone, Copy)]
struct Sums {
kw: f32,
kwg: f32,
kwd: [f32; 3],
}
impl Sums {
#[inline]
fn span<const COLOR: bool>(&mut self, f: &Frame, k: f32, lo: usize, hi: usize) {
let colors = f.colors.map(|plane| &plane[lo..=hi]);
for (j, (&w, &g)) in f.w[lo..=hi].iter().zip(&f.g[lo..=hi]).enumerate() {
let kw = w * k;
self.kw += kw;
self.kwg += kw * g;
if COLOR {
for (d, plane) in self.kwd.iter_mut().zip(colors) {
*d += kw * f.lut[usize::from(plane[j])];
}
}
}
}
#[inline]
fn finish(self, out: &mut Levels, level: usize) {
let inv = if self.kw > 0.0 { 1.0 / self.kw } else { 0.0 };
out.c[level] = self.kw;
out.p[level] = self.kwg * inv;
for (l, d) in out.l.iter_mut().zip(self.kwd) {
l[level] = d * inv;
}
}
}
#[inline]
fn offset(base: usize, delta: i32, len: usize) -> Option<usize> {
let v = base as isize + delta as isize;
(v >= 0 && v < len as isize).then_some(v as usize)
}
#[inline]
fn clamped(base: usize, delta: i32, len: usize) -> usize {
(base as isize + delta as isize).clamp(0, len as isize - 1) as usize
}
fn pyramids_at<const COLOR: bool>(f: &Frame, y: usize, x: usize) -> Levels {
let (rows, cols) = (f.rows, f.cols);
let mut out = Levels::default();
for (level, (spans, k)) in [(&LEVEL0[..], 1.0 / 69.0), (&LEVEL1[..], 1.0 / 21.0)]
.into_iter()
.enumerate()
{
let mut sums = Sums::default();
for &(dy, dx_lo, dx_hi) in spans {
let Some(ny) = offset(y, dy, rows) else {
continue;
};
let base = ny * cols;
sums.span::<COLOR>(
f,
k,
base + clamped(x, dx_lo, cols),
base + clamped(x, dx_hi, cols),
);
}
sums.finish(&mut out, level);
}
let mut sums = Sums::default();
for dy in -1..=1 {
let Some(ny) = offset(y, dy, rows) else {
continue;
};
for dx in -1..=1 {
let Some(nx) = offset(x, dx, cols) else {
continue;
};
let i = ny * cols + nx;
let k = TENT[(dy + 1) as usize] * TENT[(dx + 1) as usize] / 16.0;
sums.span::<COLOR>(f, k, i, i);
}
}
sums.finish(&mut out, 2);
let i = y * cols + x;
out.c[3] = f.w[i];
out.p[3] = f.g[i];
if COLOR {
for (l, plane) in out.l.iter_mut().zip(f.colors) {
l[3] = f.lut[usize::from(plane[i])];
}
}
out
}
fn uniform(pixel: usize, channel: usize) -> f32 {
let mut x = (pixel as u64) << 2 | channel as u64;
x = (x ^ (x >> 30)).wrapping_mul(0xbf58_476d_1ce4_e5b9);
x = (x ^ (x >> 27)).wrapping_mul(0x94d0_49bb_1331_11eb);
((x ^ (x >> 31)) & 0x00ff_ffff) as f32 / (1u32 << 24) as f32
}
#[inline]
fn dither(x: f32, pixel: usize, channel: usize, p: &Params) -> f32 {
let [lo, hi] = p.eta;
if x <= lo || x >= hi {
return 0.0;
}
let envelope = 4.0 / (hi - lo).powi(2) * (x - lo) * (hi - x);
let d = envelope * (uniform(pixel, channel) - 0.5) * ALPHA[channel] * x;
if x + d > lo && x + d < hi { d } else { 0.0 }
}
pub struct Patch {
pub at: Vec<u32>,
pub density: [Vec<f32>; 3],
}
pub fn reconstruct_core(
g: &[f32],
w: &[f32],
colors: [&[u16]; 3],
mask: &[bool],
p: &Params,
rows: usize,
cols: usize,
) -> Patch {
let f = &Frame {
g,
w,
colors,
lut: &LUT,
rows,
cols,
};
let at: Vec<u32> = (0..(rows * cols) as u32)
.into_par_iter()
.filter(|&i| mask[i as usize])
.collect();
let mut density = std::array::from_fn::<_, 3, _>(|_| vec![0.0f32; at.len()]);
let mut keep = vec![true; at.len()];
let [r, gr, b] = &mut density;
(&at, r, gr, b, &mut keep)
.into_par_iter()
.for_each(|(&i, r, gr, b, keep)| {
let i = i as usize;
let (y, x) = (i / cols, i % cols);
let center = pyramids_at::<true>(f, y, x);
let mut lo = [0.0f32; 4];
let mut hi = [0.0f32; 4];
for level in 1..=3 {
lo[level] = center.p[level] - center.p[level - 1];
hi[level] = lo[level];
}
if p.cross_beta {
for (dy, dx) in [(0, -1), (0, 1), (-1, 0), (1, 0)] {
let (Some(ny), Some(nx)) = (offset(y, dy, rows), offset(x, dx, cols)) else {
continue;
};
let n = pyramids_at::<false>(f, ny, nx);
for level in 1..=3 {
let d = n.p[level] - n.p[level - 1];
lo[level] = lo[level].min(d);
hi[level] = hi[level].max(d);
}
}
}
let mut acc = [0.0f32; 3];
for (ch, acc) in acc.iter_mut().enumerate() {
let mut a = center.l[ch][0] + p.gamma[ch] * (p.ir_ref - center.p[0]);
for level in 1..=3 {
let detail = (center.l[ch][level] - center.l[ch][level - 1]) * p.detail_gain;
let (a_hi, a_lo) = p.a[ch][level - 1];
let (a_lo, a_hi) = if hi[level] < 0.0 {
(a_hi, a_lo)
} else {
(a_lo, a_hi)
};
let (lo_t, hi_t) = (a_lo * lo[level], a_hi * hi[level]);
let r = if detail < lo_t {
detail - lo_t
} else if detail > hi_t {
detail - hi_t
} else {
0.0
};
let confidence = match level {
1 => (2.0 * center.c[1]).min(1.0),
2 => center.c[2],
_ => center.c[3] * center.c[3],
};
a += r * confidence;
}
*acc = a + dither(a, i, ch, p);
}
if p.clamp_l3 {
if acc.iter().any(|&a| a <= 0.0) {
*keep = false;
return;
}
for (acc, l) in acc.iter_mut().zip(center.l) {
*acc = acc.max(l[3]); }
}
(*r, *gr, *b) = (acc[0], acc[1], acc[2]);
});
if keep.iter().all(|&k| k) {
return Patch { at, density };
}
Patch {
at: at
.iter()
.zip(&keep)
.filter_map(|(&i, &k)| k.then_some(i))
.collect(),
density: density.map(|plane| {
plane
.into_iter()
.zip(&keep)
.filter_map(|(d, &k)| k.then_some(d))
.collect()
}),
}
}
pub fn clean(
color: [&mut [u16]; 3],
ir: &[u16],
cal: &Calibration,
rows: usize,
cols: usize,
opts: &Options,
) -> usize {
let p = Params::new(opts, cal);
let [red, green, blue] = color;
let g = gate(&*red, ir, &p);
let w = confidence(&g, cols, &p);
let mask = decide(&g, &w, rows, cols, &p);
let patch = reconstruct_core(&g, &w, [red, green, blue], &mask, &p, rows, cols);
drop((g, w, mask));
for (plane, density) in [red, green, blue].into_iter().zip(&patch.density) {
for (&i, v) in patch.at.iter().zip(from_density(density)) {
let out = &mut plane[i as usize];
*out = if p.clamp_l3 { v.max(*out) } else { v };
}
}
patch.at.len()
}