//! Depth preprocessing and splat construction (PLAN.md, pipeline
//! steps 2–3).
//!
//! [`SplatGen`] turns one raw [`DepthFrame`] — a millimetre depth map
//! plus RGB, exactly what a Kinect (or the synthetic source, or a
//! recording) emits — into the frame's [`SplatBatch`]:
//!
//! 1. **cull** — drop invalid pixels (depth 0) and everything beyond
//!    `max_depth_mm` (background culling),
//! 2. **flier removal** — drop pixels that disagree hard with their
//!    3×3 neighbourhood median (isolated wrong returns),
//! 3. **bilateral filter** — edge-preserving smoothing over a small
//!    pixel radius, so depth edges survive but sensor jitter does not,
//! 4. **normals** — from screen-space derivatives of the filtered
//!    point cloud, oriented towards the camera,
//! 5. **surfels** — one footprint-sized anisotropic splat per
//!    surviving pixel via `rig::Splat::surfel` (the plan's core
//!    construction), coloured from the RGB pixel.
//!
//! [`SplatSource`] adapts any [`DepthSource`] into a
//! [`FrameSource`]: capture, recordings, and the synthetic dataset all
//! plug into the renderer the same way.

use anyhow::Result;
use glam::Vec3;

use rig::{
    Camera, DepthFrame, DepthSource, Ellipsoid, Frame, FrameSource, Rig, SideRgb, Splat, SplatBatch,
};

/// Tuning knobs for the occlusion proxy (PLAN.md, "Core design": the
/// MVP fills the occluded far side of the face with a head/shoulders
/// ellipsoid fitted per frame, recolored from the side cameras).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ProxyParams {
    /// Generate proxy splats at all.
    pub enabled: bool,
    /// Fitted-ellipsoid radii as a multiple of the cluster's per-axis
    /// standard deviation.
    pub sigma: f32,
    /// Proxy splats are this many times thicker than regular surfels,
    /// so the flat disks bridge the curvature between them.
    pub thickness_multiple: f32,
    /// Proxy splat opacity — lower than the surfels' (a lower
    /// confidence weight, so real depth data dominates on overlap).
    pub opacity: f32,
    /// Points within this depth range of the nearest valid depth
    /// belong to the subject cluster, metres.
    pub cluster_extent_m: f32,
}

impl Default for ProxyParams {
    fn default() -> Self {
        Self {
            enabled: true,
            sigma: 2.0,
            thickness_multiple: 3.0,
            opacity: 0.85,
            cluster_extent_m: 0.6,
        }
    }
}

/// Tuning knobs for the depth pipeline; the defaults follow PLAN.md
/// (bilateral radius 3–5 px, opacity ~0.95, few-mm thickness).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SplatGenParams {
    /// Bilateral filter radius in pixels.
    pub filter_radius: u32,
    /// Bilateral depth-domain sigma, millimetres.
    pub filter_sigma_depth_mm: f32,
    /// Bilateral spatial-domain sigma, pixels.
    pub filter_sigma_space_px: f32,
    /// A pixel disagreeing with its 3×3 neighbourhood median by more
    /// than this is a flier and is dropped, millimetres.
    pub flier_threshold_mm: u32,
    /// Pixels beyond this depth are culled as background, millimetres.
    pub max_depth_mm: u16,
    /// Surfet opacity (PLAN.md: ~0.9–0.99).
    pub opacity: f32,
    /// Surfel thickness along the normal, metres.
    pub thickness_m: f32,
    /// Occlusion-proxy parameters.
    pub proxy: ProxyParams,
}

impl Default for SplatGenParams {
    fn default() -> Self {
        Self {
            filter_radius: 3,
            filter_sigma_depth_mm: 25.0,
            filter_sigma_space_px: 2.0,
            flier_threshold_mm: 250,
            max_depth_mm: 4000,
            opacity: 0.95,
            thickness_m: rig::SURFEL_THICKNESS_M,
            proxy: ProxyParams::default(),
        }
    }
}

/// The depth pipeline: depth frames in, splat batches out.
pub struct SplatGen {
    params: SplatGenParams,
}

impl Default for SplatGen {
    fn default() -> Self {
        Self::new(SplatGenParams::default())
    }
}

impl SplatGen {
    pub fn new(params: SplatGenParams) -> Self {
        Self { params }
    }

    /// The tuning parameters in effect.
    pub fn params(&self) -> &SplatGenParams {
        &self.params
    }

    /// Preprocess one depth frame and assemble its splat batch:
    /// surfels from the depth map, plus occlusion-proxy splats from the
    /// fitted ellipsoid, recolored from the side cameras.
    pub fn generate(&self, frame: &DepthFrame, rig: &Rig) -> SplatBatch {
        let camera = &rig.center;
        let (width, height) = (camera.resolution.x, camera.resolution.y);
        let pixel_count = (width * height) as usize;
        assert_eq!(frame.depth_mm.len(), pixel_count, "depth map size");
        assert_eq!(frame.rgb.len(), pixel_count * 3, "rgb size");

        // f32 working copy; `None` = invalid.
        let mut depth: Vec<Option<f32>> = frame
            .depth_mm
            .iter()
            .map(|&d| (d > 0 && d <= self.params.max_depth_mm).then_some(d as f32))
            .collect();

        remove_fliers(&mut depth, width, self.params.flier_threshold_mm);
        let filtered = bilateral_filter(&depth, width, self.params);

        let mut batch = SplatBatch::with_capacity(pixel_count);
        for y in 0..height {
            for x in 0..width {
                let Some(z_mm) = filtered[(y * width + x) as usize] else {
                    continue;
                };
                let (u, v) = (x as f32 + 0.5, y as f32 + 0.5);
                let z = z_mm / 1000.0;
                let position = camera.backproject(u, v, z);
                let normal = estimate_normal(&filtered, width, camera, x, y);
                let idx = (y * width + x) as usize;
                let color = [
                    frame.rgb[idx * 3] as f32 / 255.0,
                    frame.rgb[idx * 3 + 1] as f32 / 255.0,
                    frame.rgb[idx * 3 + 2] as f32 / 255.0,
                ];
                batch.push(Splat::surfel(
                    position,
                    normal,
                    z,
                    &camera.intrinsics,
                    self.params.thickness_m,
                    color,
                    self.params.opacity,
                ));
            }
        }

        // Occlusion proxy: splats on the fitted subject ellipsoid,
        // recolored from the side cameras.
        if self.params.proxy.enabled
            && let Some(sides) = &frame.side_rgb
            && let Some(fit) = fit_subject_ellipsoid(
                &filtered,
                camera,
                self.params.proxy.cluster_extent_m,
                self.params.proxy.sigma,
            )
        {
            self.add_proxy_splats(&mut batch, &filtered, camera, &fit, rig, sides);
        }
        batch
    }

    /// Generate proxy splats from one side camera: every pixel whose
    /// ray hits the fitted ellipsoid in a region the depth map does
    /// cover gets a splat on the ellipsoid, recolored from that pixel.
    fn add_proxy_splats(
        &self,
        batch: &mut SplatBatch,
        filtered: &[Option<f32>],
        center: &Camera,
        fit: &SubjectFit,
        rig: &Rig,
        sides: &SideRgb,
    ) {
        let side_data = [
            (&rig.left, &sides.left, sides.left_resolution),
            (&rig.right, &sides.right, sides.right_resolution),
        ];
        for (camera, rgb, resolution) in side_data {
            let (width, height) = (resolution.x, resolution.y);
            assert_eq!(rgb.len(), (width * height) as usize * 3, "side rgb size");
            let camera_from_world = camera.pose.camera_from_world();
            for y in 0..height {
                for x in 0..width {
                    let (u, v) = (x as f32 + 0.5, y as f32 + 0.5);
                    let (origin, dir) = camera.pixel_ray(u, v);
                    let Some((t, normal)) = fit.ellipsoid.intersect_ray(origin, dir) else {
                        continue;
                    };
                    let position = origin + dir * t;
                    // Only where the depth map has no data of its own.
                    if occluded_from_depth_map(filtered, center, position, fit) {
                        let depth = camera_from_world.transform_point3(position).z;
                        let idx = (y * width + x) as usize;
                        let color = [
                            rgb[idx * 3] as f32 / 255.0,
                            rgb[idx * 3 + 1] as f32 / 255.0,
                            rgb[idx * 3 + 2] as f32 / 255.0,
                        ];
                        batch.push(Splat::surfel(
                            position,
                            normal,
                            depth,
                            &camera.intrinsics,
                            self.params.thickness_m * self.params.proxy.thickness_multiple,
                            color,
                            self.params.proxy.opacity,
                        ));
                    }
                }
            }
        }
    }
}

/// A [`FrameSource`] adapter: pull raw depth frames from `S` and run
/// them through [`SplatGen`]. This is the pipeline seam — live capture,
/// recordings, and the synthetic dataset all become renderable sources.
/// The rig is the source's own (its side cameras drive the proxy).
pub struct SplatSource<S: DepthSource> {
    rig: Rig,
    inner: S,
    generator: SplatGen,
}

impl<S: DepthSource> SplatSource<S> {
    pub fn new(rig: Rig, inner: S, generator: SplatGen) -> Self {
        Self {
            rig,
            inner,
            generator,
        }
    }

    /// The wrapped depth source.
    pub fn inner(&self) -> &S {
        &self.inner
    }

    /// The rig the pipeline renders with.
    pub fn rig(&self) -> &Rig {
        &self.rig
    }
}

impl<S: DepthSource> FrameSource for SplatSource<S> {
    fn next_frame(&mut self) -> Result<Frame> {
        let depth_frame = self.inner.next_depth_frame()?;
        let splats = self.generator.generate(&depth_frame, &self.rig);
        Ok(Frame {
            timestamp_secs: depth_frame.timestamp_secs,
            splats,
        })
    }
}

/// The subject cluster fit: the ellipsoid plus the depth range the
/// cluster spans (what separates subject pixels from background in the
/// depth map).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SubjectFit {
    pub ellipsoid: Ellipsoid,
    /// Cluster depth range in millimetres.
    pub min_mm: f32,
    pub max_mm: f32,
}

/// Is `position` a region the depth map does not cover — i.e. may the
/// proxy fill it?
///
/// * outside the depth image, at an invalid pixel, or behind the depth
///   camera → yes, no data there;
/// * over a background pixel (observed depth beyond the subject
///   cluster) → yes, the proxy adds subject geometry the depth map
///   never saw;
/// * over a subject pixel → only when the point sits *behind* the
///   observed surface by more than the margin — the occluded side. At
///   or in front of the observed surface the depth map's own surfels
///   already cover it (and a floating proxy point would z-fight).
fn occluded_from_depth_map(
    filtered: &[Option<f32>],
    center: &Camera,
    position: Vec3,
    fit: &SubjectFit,
) -> bool {
    const PROXY_OCCLUSION_MARGIN_MM: f32 = 30.0;

    let p = center.pose.camera_from_world().transform_point3(position);
    if p.z <= 0.0 {
        return true; // behind the depth camera entirely
    }
    let Some(pixel) = center.intrinsics.project_camera_point(p) else {
        return true; // outside the depth image
    };
    let (width, height) = (center.resolution.x, center.resolution.y);
    if pixel.x < 0.0 || pixel.y < 0.0 || pixel.x >= width as f32 || pixel.y >= height as f32 {
        return true;
    }
    let idx = (pixel.y as u32 * width + pixel.x as u32) as usize;
    let Some(observed) = filtered.get(idx).copied().flatten() else {
        return true; // no data here
    };
    if observed > fit.max_mm {
        return true; // background pixel: the proxy is new subject geometry
    }
    p.z * 1000.0 > observed + PROXY_OCCLUSION_MARGIN_MM
}

/// Fit the subject's ellipsoid to the depth cloud: the points within
/// `cluster_extent_m` of the nearest observed depth (the subject sits
/// closest to the rig; the backdrop is further out), mean centre,
/// `sigma`-scaled per-axis standard deviation as radii.
///
/// Returns `None` when there is no substantial cluster.
pub fn fit_subject_ellipsoid(
    filtered: &[Option<f32>],
    camera: &Camera,
    cluster_extent_m: f32,
    sigma: f32,
) -> Option<SubjectFit> {
    let (width, height) = (camera.resolution.x, camera.resolution.y);

    // Nearest valid depth (millimetres).
    let min_mm = filtered
        .iter()
        .flatten()
        .copied()
        .fold(f32::INFINITY, f32::min);
    if !min_mm.is_finite() {
        return None;
    }
    let max_mm = min_mm + cluster_extent_m * 1000.0;

    // The subject cluster, in world space.
    let mut count = 0_usize;
    let mut sum = Vec3::ZERO;
    let mut sum_sq = Vec3::ZERO;
    for y in 0..height {
        for x in 0..width {
            let Some(d) = filtered[(y * width + x) as usize] else {
                continue;
            };
            if d < min_mm || d > max_mm {
                continue;
            }
            let p = camera.backproject(x as f32 + 0.5, y as f32 + 0.5, d / 1000.0);
            count += 1;
            sum += p;
            sum_sq += p * p;
        }
    }
    // Too little subject to speak of an ellipsoid.
    if count < 100 {
        return None;
    }

    let center = sum / count as f32;
    let variance = (sum_sq / count as f32 - center * center).max(Vec3::ZERO);
    let std_dev = Vec3::new(variance.x.sqrt(), variance.y.sqrt(), variance.z.sqrt());
    // Clamp so the ellipsoid never degenerates into a disk or point.
    let min_radius = 0.02_f32;
    let radii = (std_dev * sigma).max(Vec3::splat(min_radius));
    Some(SubjectFit {
        ellipsoid: Ellipsoid::new(center, radii),
        min_mm,
        max_mm,
    })
}

/// Drop pixels that disagree with their 3×3 neighbourhood median by
/// more than `threshold_mm` (isolated fliers). Neighbourhood positions
/// are clamped at the borders.
fn remove_fliers(depth: &mut [Option<f32>], width: u32, threshold_mm: u32) {
    let width = width as i32;
    let height = (depth.len() / width as usize) as i32;
    let threshold = threshold_mm as f32;
    let mut neighbourhood = [0.0_f32; 9];

    let mut flier = vec![false; depth.len()];
    for y in 0..height {
        for x in 0..width {
            let idx = (y * width + x) as usize;
            let Some(own) = depth[idx] else { continue };
            let mut n = 0;
            for dy in -1..=1 {
                for dx in -1..=1 {
                    let (nx, ny) = (x + dx, y + dy);
                    if !(0..width).contains(&nx) || !(0..height).contains(&ny) {
                        continue;
                    }
                    if let Some(d) = depth[(ny * width + nx) as usize] {
                        neighbourhood[n] = d;
                        n += 1;
                    }
                }
            }
            if n < 3 {
                continue; // not enough context to judge
            }
            neighbourhood[..n].sort_by(|a, b| a.partial_cmp(b).unwrap());
            let median = if n % 2 == 1 {
                neighbourhood[n / 2]
            } else {
                (neighbourhood[n / 2 - 1] + neighbourhood[n / 2]) * 0.5
            };
            if (own - median).abs() > threshold {
                flier[idx] = true;
            }
        }
    }
    for (depth, flier) in depth.iter_mut().zip(flier) {
        if flier {
            *depth = None;
        }
    }
}

/// Edge-preserving bilateral filter over valid pixels: spatial × depth
/// Gaussians, radius from the params.
fn bilateral_filter(depth: &[Option<f32>], width: u32, params: SplatGenParams) -> Vec<Option<f32>> {
    let width_i = width as i32;
    let height = (depth.len() / width as usize) as i32;
    let radius = params.filter_radius as i32;
    let inv_two_sigma_d2 = 1.0 / (2.0 * params.filter_sigma_depth_mm.powi(2));
    let inv_two_sigma_s2 = 1.0 / (2.0 * params.filter_sigma_space_px.powi(2));

    let mut out = depth.to_vec();
    for y in 0..height {
        for x in 0..width_i {
            let idx = (y * width_i + x) as usize;
            let Some(own) = depth[idx] else { continue };
            let mut total = 0.0;
            let mut weight_sum = 0.0;
            for dy in -radius..=radius {
                for dx in -radius..=radius {
                    let (nx, ny) = (x + dx, y + dy);
                    if !(0..width_i).contains(&nx) || !(0..height).contains(&ny) {
                        continue;
                    }
                    let Some(neighbour) = depth[(ny * width_i + nx) as usize] else {
                        continue;
                    };
                    let spatial = (dx * dx + dy * dy) as f32;
                    let dd = neighbour - own;
                    let weight = (-spatial * inv_two_sigma_s2 - dd * dd * inv_two_sigma_d2).exp();
                    total += weight * neighbour;
                    weight_sum += weight;
                }
            }
            if weight_sum > 1e-6 {
                out[idx] = Some(total / weight_sum);
            }
        }
    }
    out
}

/// Surface normal at a pixel from the filtered depth: the cross product
/// of screen-space derivatives of the backprojected point cloud,
/// oriented towards the camera. Falls back to facing the camera when no
/// valid neighbours exist.
fn estimate_normal(depth: &[Option<f32>], width: u32, camera: &Camera, x: u32, y: u32) -> Vec3 {
    let width_i = width as i32;
    let height = (depth.len() / width as usize) as i32;
    let (xi, yi) = (x as i32, y as i32);

    let point = |px: i32, py: i32| -> Option<Vec3> {
        if !(0..width_i).contains(&px) || !(0..height).contains(&py) {
            return None;
        }
        let d = depth[(py * width_i + px) as usize]? / 1000.0;
        Some(camera.backproject(px as f32 + 0.5, py as f32 + 0.5, d))
    };

    let center = point(xi, yi).expect("caller checked the pixel is valid");
    // Central differences where both neighbours are valid, otherwise
    // one-sided, otherwise no estimate.
    let du = point(xi + 1, yi)
        .zip(point(xi - 1, yi))
        .map(|(a, b)| a - b)
        .or_else(|| point(xi + 1, yi).map(|a| (a - center) * 2.0))
        .or_else(|| point(xi - 1, yi).map(|b| (center - b) * 2.0));
    let dv = point(xi, yi + 1)
        .zip(point(xi, yi - 1))
        .map(|(a, b)| a - b)
        .or_else(|| point(xi, yi + 1).map(|a| (a - center) * 2.0))
        .or_else(|| point(xi, yi - 1).map(|b| (center - b) * 2.0));

    let toward_camera = (camera.pose.position - center).normalize_or_zero();
    let Some(du) = du else { return toward_camera };
    let Some(dv) = dv else { return toward_camera };
    let mut normal = du.cross(dv).normalize_or_zero();
    if normal.length_squared() < 1e-8 {
        // Degenerate (no valid neighbours): face the camera.
        normal = toward_camera;
    } else if normal.dot(toward_camera) < 0.0 {
        // The cross product's handedness is arbitrary; orient towards
        // the camera.
        normal = -normal;
    }
    normal
}

#[cfg(test)]
mod tests {
    use super::*;
    use glam::{uvec2, vec3};
    use rig::{CameraIntrinsics, CameraPose};

    /// A test center camera.
    fn camera(width: u32, height: u32) -> Camera {
        Camera {
            id: "test".into(),
            pose: CameraPose::looking_at(Vec3::ZERO, vec3(0.0, 0.0, 2.0)),
            intrinsics: CameraIntrinsics {
                fx: 100.0,
                fy: 100.0,
                cx: width as f32 / 2.0,
                cy: height as f32 / 2.0,
            },
            resolution: uvec2(width, height),
        }
    }

    /// A test rig: a converged center camera plus matching side cameras.
    fn rig(width: u32, height: u32) -> rig::Rig {
        let center = camera(width, height);
        let make_side = |x: f32| Camera {
            id: if x < 0.0 { "left" } else { "right" }.to_owned(),
            pose: rig::CameraPose::looking_at(glam::vec3(x, 0.0, 0.0), glam::vec3(0.0, 0.0, 2.0)),
            intrinsics: CameraIntrinsics {
                fx: 100.0,
                fy: 100.0,
                cx: width as f32 / 2.0,
                cy: height as f32 / 2.0,
            },
            resolution: glam::uvec2(width, height),
        };
        rig::Rig {
            left: make_side(-0.4),
            center,
            right: make_side(0.4),
        }
    }

    fn frame(width: u32, height: u32, depth_of: impl Fn(u32, u32) -> u16) -> DepthFrame {
        let mut depth = Vec::with_capacity((width * height) as usize);
        let mut rgb = Vec::with_capacity((width * height * 3) as usize);
        for y in 0..height {
            for x in 0..width {
                depth.push(depth_of(x, y));
                rgb.extend_from_slice(&[200, 100, 50]);
            }
        }
        DepthFrame {
            timestamp_secs: 0.0,
            depth_mm: depth,
            rgb,
            side_rgb: None,
        }
    }

    #[test]
    fn flat_wall_makes_camera_facing_surfels() {
        let (w, h) = (32u32, 24u32);
        let rig = rig(w, h);
        let camera = &rig.center;
        let frame = frame(w, h, |_, _| 2000);
        let batch = SplatGen::default().generate(&frame, &rig);

        assert_eq!(batch.len(), (w * h) as usize);
        for splat in batch.iter() {
            // On the wall plane: the projection lands back on the pixel.
            let projected = camera.project(splat.mean).expect("in front of the camera");
            // The wall's normal is constant (0, 0, -1) in world space…
            let wall_normal = Vec3::new(0.0, 0.0, -1.0);
            assert!(
                (splat.normal() - wall_normal).length() < 0.05,
                "wall normal {:?}",
                splat.normal()
            );
            // …and faces the camera everywhere on the wall.
            let to_camera = (camera.pose.position - splat.mean).normalize();
            assert!(splat.normal().dot(to_camera) > 0.9);
            // Footprint from the plan: depth / focal.
            let z = 2.0;
            assert!((splat.scale.x - z / 100.0).abs() < 1e-4);
            assert!((splat.scale.y - z / 100.0).abs() < 1e-4);
            assert_eq!(splat.color, [200.0 / 255.0, 100.0 / 255.0, 50.0 / 255.0]);
            assert!((splat.opacity - 0.95).abs() < 1e-6);
            let _ = projected;
        }
    }

    #[test]
    fn culling_drops_invalid_and_far_pixels() {
        let (w, h) = (16u32, 8u32);
        let rig = rig(w, h);
        let _camera = &rig.center;
        let frame = frame(w, h, |x, y| match (x, y) {
            (0, 0) => 0,    // no return
            (1, 0) => 5000, // beyond max depth
            _ => 2000,
        });
        let batch = SplatGen::default().generate(&frame, &rig);
        assert_eq!(batch.len(), (w * h - 2) as usize);
    }

    #[test]
    fn fliers_are_removed() {
        let (w, h) = (16u32, 16u32);
        let rig = rig(w, h);
        let camera = &rig.center;
        // A few isolated wrong returns among a flat wall.
        let frame = frame(w, h, |x, y| {
            if matches!((x, y), (4, 4) | (10, 12) | (15, 0)) {
                600 // wildly wrong
            } else {
                2000
            }
        });
        let batch = SplatGen::default().generate(&frame, &rig);
        assert_eq!(
            batch.len(),
            (w * h - 3) as usize,
            "isolated fliers must be culled"
        );
        // And none of the surviving splats sit at the flier depth.
        for splat in batch.iter() {
            let z = camera
                .pose
                .camera_from_world()
                .transform_point3(splat.mean)
                .z;
            assert!((z - 2.0).abs() < 0.05, "stray depth {z}");
        }
    }

    #[test]
    fn bilateral_smooths_jitter_but_not_edges() {
        let (w, h) = (32u32, 8u32);
        let rig = rig(w, h);
        let _camera = &rig.center;
        // Left half at 2000 mm, right half at 3000 mm, everything
        // jittered ±3 mm.
        let clean = frame(w, h, |x, _| if x < 16 { 2000 } else { 3000 });
        let noisy = frame(w, h, |x, y| {
            let base = if x < 16 { 2000 } else { 3000 };
            let jitter = ((x * 7 + y * 13) % 7) as i16 - 3;
            ((base as i16) + jitter) as u16
        });

        let generator = SplatGen::default();
        let clean_batch = generator.generate(&clean, &rig);
        let noisy_batch = generator.generate(&noisy, &rig);
        assert_eq!(clean_batch.len(), noisy_batch.len());

        // The noisy splats sit within a fraction of a pixel of the
        // clean ones: the jitter (≈3 mm at 2 m ≈ 0.15 px) is smoothed
        // away, and the step edge is preserved.
        for (a, b) in clean_batch.iter().zip(noisy_batch.iter()) {
            assert!(
                (a.mean - b.mean).length() < 0.002,
                "positions differ: {:?} vs {:?}",
                a.mean,
                b.mean
            );
        }
    }

    #[test]
    fn normals_follow_a_slanted_plane() {
        let (w, h) = (24u32, 24u32);
        let rig = rig(w, h);
        let camera = &rig.center;
        // Depth increases by 40 mm per pixel: the surface tilts away to
        // the right. dz/du = 0.04 m per pixel; world dx per pixel is
        // z/fx, so the plane's normal is ∝ (-slope, 0, 1).
        let frame = frame(w, h, |x, _| 2000 + 40 * x as u16);

        let batch = SplatGen::default().generate(&frame, &rig);
        // Check a pixel near the centre, away from border effects.
        let splat = batch.iter().nth(12 * 24 + 12).expect("center pixel");

        let z_world = 2.0 + 0.04 * 12.5; // metres at that pixel
        // The plane's camera-facing normal in world space: image-right
        // maps to world -X, so rising depth with u means dz/dx_world is
        // negative there.
        let dz_du = 0.04_f32; // metres per pixel
        let dz_dx_world = dz_du / (-(z_world / camera.intrinsics.fx));
        let expected = Vec3::new(dz_dx_world, 0.0, -1.0).normalize();
        let dot = splat.normal().dot(expected);
        assert!(
            dot > 0.995,
            "normal {:?} vs expected {expected:?} (dot {dot})",
            splat.normal()
        );
    }

    #[test]
    fn params_are_honoured() {
        let (w, h) = (16u32, 16u32);
        let rig = rig(w, h);
        let _camera = &rig.center;
        let frame = frame(w, h, |_, _| 2500);
        let params = SplatGenParams {
            max_depth_mm: 2000, // culls everything
            ..SplatGenParams::default()
        };
        let batch = SplatGen::new(params).generate(&frame, &rig);
        assert!(batch.is_empty());

        // Opacity flows through to the surfels.
        let params = SplatGenParams {
            opacity: 0.9,
            ..SplatGenParams::default()
        };
        let batch = SplatGen::new(params).generate(&frame, &rig);
        assert!((batch.iter().next().unwrap().opacity - 0.9).abs() < 1e-6);
    }

    #[test]
    fn generates_from_the_synthetic_source() {
        // Integration: the synthetic Kinect feeds the pipeline.
        use rig::DepthSource;
        let rig = replay::synth::dev_rig_with_resolutions(uvec2(48, 40), uvec2(160, 120));
        let mut source = replay::synth::dev_source_with(&rig, replay::synth::SUBJECT, 30.0);
        let frame = source.next_depth_frame().expect("depth frame");
        let mut params = SplatGenParams::default();
        params.proxy.enabled = false;
        let batch = SplatGen::new(params).generate(&frame, &rig);

        // Roughly one splat per depth pixel (fliers removed).
        assert!(batch.len() > 48 * 40 * 95 / 100);
        assert!(batch.len() <= 48 * 40);
        // All in front of the camera, all at valid depths.
        let camera_from_world = rig.center.pose.camera_from_world();
        for splat in batch.iter() {
            let p = camera_from_world.transform_point3(splat.mean);
            assert!(p.z > 0.5 && p.z < 3.1, "depth {p:?}");
        }
    }

    /// The synthetic rig + a clean first frame, for proxy tests. The
    /// depth resolution is high enough that the subject spans several
    /// hundred pixels.
    fn synthetic_proxy_setup() -> (rig::Rig, DepthFrame) {
        use rig::DepthSource;
        let rig = replay::synth::dev_rig_with_resolutions(uvec2(128, 106), uvec2(160, 120));
        let mut source = replay::synth::dev_source_with(&rig, replay::synth::SUBJECT, 30.0)
            .with_noise(replay::synth::NoiseParams::OFF);
        let frame = source.next_depth_frame().expect("depth frame");
        (rig, frame)
    }

    #[test]
    fn ellipsoid_fit_matches_the_subject() {
        let (rig, frame) = synthetic_proxy_setup();
        let filtered = preprocess_for_fit(&frame, &rig);

        let fit = fit_subject_ellipsoid(
            &filtered,
            &rig.center,
            ProxyParams::default().cluster_extent_m,
            ProxyParams::default().sigma,
        )
        .expect("subject cluster found");
        let ellipsoid = fit.ellipsoid;

        // The subject (head + shoulders, pixel-weighted to the wide
        // shoulders) sits near the origin, slightly below head height,
        // ~1.9 m out — the cluster is the visible front.
        assert!(
            ellipsoid.center.x.abs() < 0.05,
            "fit centre {:?}",
            ellipsoid.center
        );
        assert!(
            (-0.1..0.15).contains(&ellipsoid.center.y),
            "fit centre {:?}",
            ellipsoid.center
        );
        assert!(
            (1.8..2.1).contains(&ellipsoid.center.z),
            "fit centre {:?}",
            ellipsoid.center
        );
        // Head + shoulders: ~0.24 m half-width, ~0.47 m tall.
        assert!(
            ellipsoid.radii.x > 0.18 && ellipsoid.radii.x < 0.40,
            "radii {:?}",
            ellipsoid.radii
        );
        assert!(
            ellipsoid.radii.y > 0.15 && ellipsoid.radii.y < 0.40,
            "radii {:?}",
            ellipsoid.radii
        );
        assert!(
            ellipsoid.radii.z > 0.04 && ellipsoid.radii.z < 0.30,
            "radii {:?}",
            ellipsoid.radii
        );
        // The cluster range excludes the backdrop (2.8 m out).
        assert!(
            fit.min_mm > 1500.0 && fit.min_mm < 2100.0,
            "min {}",
            fit.min_mm
        );
        assert!(fit.max_mm < 2700.0, "max {}", fit.max_mm);
        // And the fitted blob contains the subject's front — the head
        // front pole and the shoulders' front.
        assert!(ellipsoid.contains(replay::synth::SUBJECT + glam::vec3(0.0, 0.18, -0.10)));
        assert!(ellipsoid.contains(replay::synth::SUBJECT + glam::vec3(0.0, -0.05, -0.16)));
    }

    /// The filtered depth the ellipsoid fit sees, for tests.
    fn preprocess_for_fit(frame: &DepthFrame, rig: &rig::Rig) -> Vec<Option<f32>> {
        let camera = &rig.center;
        let mut depth: Vec<Option<f32>> = frame
            .depth_mm
            .iter()
            .map(|&d| (d > 0 && d <= 4000).then_some(d as f32))
            .collect();
        remove_fliers(&mut depth, camera.resolution.x, 250);
        bilateral_filter(&depth, camera.resolution.x, SplatGenParams::default())
    }

    #[test]
    fn proxy_fills_occluded_flanks_with_side_colours() {
        let (rig, frame) = synthetic_proxy_setup();
        let plain_params = SplatGenParams {
            proxy: ProxyParams {
                enabled: false,
                ..ProxyParams::default()
            },
            ..SplatGenParams::default()
        };

        let plain = SplatGen::new(plain_params).generate(&frame, &rig);
        let with_proxy = SplatGen::default().generate(&frame, &rig);
        assert!(
            with_proxy.len() > plain.len(),
            "proxy must add splats ({} vs {})",
            with_proxy.len(),
            plain.len()
        );

        // Recover the fitted ellipsoid and inspect the proxy splats:
        // every added splat lies on it, with its outward normal, and
        // carries lower confidence (opacity) and inflated thickness.
        let filtered = preprocess_for_fit(&frame, &rig);
        let fit = fit_subject_ellipsoid(
            &filtered,
            &rig.center,
            ProxyParams::default().cluster_extent_m,
            ProxyParams::default().sigma,
        )
        .expect("fit");
        let ellipsoid = fit.ellipsoid;

        let mut proxy_count = 0;
        let mut left_tinted = 0;
        let mut right_tinted = 0;
        for splat in with_proxy.iter().skip(plain.len()) {
            proxy_count += 1;
            // On the fitted surface.
            let s = (splat.mean - ellipsoid.center) / ellipsoid.radii;
            assert!((s.length() - 1.0).abs() < 1e-3, "off-ellipsoid: {s:?}");
            // Outward normal matches the ellipsoid's.
            let n = ellipsoid.normal_at(splat.mean);
            assert!(
                splat.normal().dot(n) > 0.999,
                "normal {:?} vs {n:?}",
                splat.normal()
            );
            // Lower confidence and inflated thickness.
            assert!((splat.opacity - 0.85).abs() < 1e-6);
            assert!((splat.scale.z - 3.0 * rig::SURFEL_THICKNESS_M).abs() < 1e-6);
            // Recolored from the side cameras: the tints show up.
            let c = splat.color;
            if c[0] > 0.7 && c[2] < 0.2 {
                left_tinted += 1;
            }
            if c[2] > 0.7 && c[0] < 0.2 {
                right_tinted += 1;
            }
        }
        assert!(proxy_count > 20, "proxy produced {proxy_count} splats");
        assert!(left_tinted > 0, "no left-tinted proxy splats");
        assert!(right_tinted > 0, "no right-tinted proxy splats");
    }

    #[test]
    fn occlusion_rule_drops_covered_geometry() {
        // A flat wall 2 m out, treated as the subject (its depth is the
        // cluster range): only points *behind* the observed surface —
        // or where there is no data at all — may be filled by the
        // proxy; background pixels always may.
        let (w, h) = (16u32, 16u32);
        let rig = rig(w, h);
        let camera = &rig.center;
        let filtered = vec![Some(2000.0_f32); (w * h) as usize];
        let fit = SubjectFit {
            ellipsoid: Ellipsoid::new(glam::vec3(0.0, 0.0, 2.0), glam::vec3(0.1, 0.1, 0.1)),
            min_mm: 2000.0,
            max_mm: 2600.0,
        };

        let at = |x: f32, y: f32, z: f32| camera.backproject(x, y, z);
        // In front of the observed surface → covered → no proxy.
        assert!(!occluded_from_depth_map(
            &filtered,
            camera,
            at(8.5, 8.5, 1.95),
            &fit
        ));
        // Right at the surface (within the margin) → no proxy.
        assert!(!occluded_from_depth_map(
            &filtered,
            camera,
            at(8.5, 8.5, 2.02),
            &fit
        ));
        // Behind by more than the margin → occluded → proxy.
        assert!(occluded_from_depth_map(
            &filtered,
            camera,
            at(8.5, 8.5, 2.1),
            &fit
        ));
        // Over a background pixel (observed beyond the cluster) → the
        // proxy is new subject geometry → always allowed.
        let mut background = filtered.clone();
        background[0] = Some(3500.0);
        assert!(occluded_from_depth_map(
            &background,
            camera,
            at(0.5, 0.5, 2.1),
            &fit
        ));
        // Outside the depth image → no data → proxy.
        assert!(occluded_from_depth_map(
            &filtered,
            camera,
            at(-3.0, 8.5, 2.1),
            &fit
        ));
        // Invalid depth pixel → no data → proxy.
        let mut invalid = vec![Some(2000.0_f32); (w * h) as usize];
        invalid[0] = None;
        assert!(occluded_from_depth_map(
            &invalid,
            camera,
            at(0.5, 0.5, 2.1),
            &fit
        ));
        // Behind the depth camera entirely → proxy.
        assert!(occluded_from_depth_map(
            &filtered,
            camera,
            at(8.5, 8.5, -1.0),
            &fit
        ));
    }
}