use crate::density::{DensityMap, SurfaceType};
use crate::mesh::Vec3;
use crate::sparse_grid::{SparseGrid, SurfaceSample};
use crate::DecompileConfig;

#[derive(Debug, Clone)]
pub struct DetectedPrimitive {
    pub kind: PrimitiveKind,
    pub support: Vec<usize>,
    pub fit_error: f64,
}

#[derive(Debug, Clone)]
pub enum PrimitiveKind {
    Plane { point: Vec3, normal: Vec3 },
    Sphere { center: Vec3, radius: f64 },
    Cylinder { point: Vec3, axis: Vec3, radius: f64 },
    Box {
        center: Vec3,
        half_extents: [f64; 3],
        rotation_axis: Vec3,
        rotation_angle: f64,
    },
}

pub fn detect_primitives(
    grid: &SparseGrid,
    density_map: &DensityMap,
    config: &DecompileConfig,
) -> Vec<DetectedPrimitive> {
    let samples = grid.surface_samples();
    if samples.is_empty() {
        return Vec::new();
    }

    let mut remaining: Vec<bool> = vec![true; samples.len()];
    let mut primitives = Vec::new();
    let min_support = (samples.len() as f64 * config.min_support_ratio).max(3.0) as usize;

    // Iterative RANSAC: detect largest primitive, remove its inliers, repeat
    for _ in 0..20 {
        let active_count = remaining.iter().filter(|&&r| r).count();
        if active_count < min_support {
            break;
        }

        let best = ransac_detect(
            &samples,
            &remaining,
            density_map,
            config,
        );

        if let Some(mut prim) = best {
            if prim.support.len() < min_support {
                break;
            }
            refine_and_expand(&mut prim, &samples, &remaining, config);
            for &idx in &prim.support {
                remaining[idx] = false;
            }
            prim.support.sort_unstable();
            primitives.push(prim);
        } else {
            break;
        }
    }

    // Post-detection: try to consolidate clusters of primitives into boxes.
    // Group primitives by spatial proximity and test if each cluster is a box.
    consolidate_into_boxes(&mut primitives, &samples, config);

    primitives
}

/// Try to replace non-box primitives with box detections.
///
/// For each non-box primitive, check if its support points form a box.
/// If the box fit is better (higher coverage), replace the original.
/// Then try merging overlapping box-candidates and neighboring primitives.
fn consolidate_into_boxes(
    primitives: &mut Vec<DetectedPrimitive>,
    samples: &[SurfaceSample],
    config: &DecompileConfig,
) {
    // Pass 1: for each non-box primitive, split its support into spatial clusters
    // and try fitting a box to each cluster.
    let mut new_prims = Vec::new();
    let mut any_replaced = false;

    for prim in primitives.iter() {
        if matches!(prim.kind, PrimitiveKind::Box { .. }) || prim.support.len() < 6 {
            new_prims.push(prim.clone());
            continue;
        }

        let clusters = spatial_cluster(&prim.support, samples, config.distance_threshold * 5.0);
        let mut box_replacements: Vec<DetectedPrimitive> = Vec::new();

        for cluster in &clusters {
            if cluster.len() < 6 { continue; }
            if let Some(box_prim) = try_fit_box_relaxed(samples, cluster, config) {
                // Box must capture at least 60% of the cluster's points
                let coverage = box_prim.support.len() as f64 / cluster.len() as f64;
                // Box mean error must be lower than original
                let box_mean_err = box_prim.fit_error / box_prim.support.len().max(1) as f64;
                let orig_mean_err = prim.fit_error / prim.support.len().max(1) as f64;
                if coverage > 0.6 && box_mean_err <= orig_mean_err {
                    box_replacements.push(box_prim);
                }
            }
        }

        if box_replacements.is_empty() {
            new_prims.push(prim.clone());
        } else {
            new_prims.extend(box_replacements);
            any_replaced = true;
        }
    }

    if any_replaced {
        *primitives = new_prims;
    }

    // Pass 2: merge overlapping primitives into boxes
    if primitives.len() < 2 {
        return;
    }

    let bboxes: Vec<(Vec3, Vec3)> = primitives.iter().map(|prim| {
        let mut lo = Vec3::new(f64::MAX, f64::MAX, f64::MAX);
        let mut hi = Vec3::new(f64::MIN, f64::MIN, f64::MIN);
        for &idx in &prim.support {
            if idx >= samples.len() { continue; }
            let p = samples[idx].position;
            lo.x = lo.x.min(p.x); lo.y = lo.y.min(p.y); lo.z = lo.z.min(p.z);
            hi.x = hi.x.max(p.x); hi.y = hi.y.max(p.y); hi.z = hi.z.max(p.z);
        }
        (lo, hi)
    }).collect();

    let mut used = vec![false; primitives.len()];
    let mut replacements: Vec<(Vec<usize>, DetectedPrimitive)> = Vec::new();

    for i in 0..primitives.len() {
        if used[i] { continue; }
        let mut group = vec![i];
        for j in (i + 1)..primitives.len() {
            if used[j] { continue; }
            if bboxes_overlap(&bboxes[i], &bboxes[j]) {
                group.push(j);
            }
        }
        if group.len() < 2 { continue; }

        let mut all_indices: Vec<usize> = Vec::new();
        for &gi in &group {
            all_indices.extend_from_slice(&primitives[gi].support);
        }
        all_indices.sort_unstable();
        all_indices.dedup();

        if let Some(box_prim) = try_fit_box(samples, &all_indices, config) {
            let box_coverage = box_prim.support.len() as f64 / all_indices.len() as f64;
            if box_coverage > 0.5 {
                for &gi in &group {
                    used[gi] = true;
                }
                replacements.push((group, box_prim));
            }
        }
    }

    if !replacements.is_empty() {
        let mut new_prims = Vec::new();
        for (i, prim) in primitives.drain(..).enumerate() {
            if !used[i] {
                new_prims.push(prim);
            }
        }
        for (_, box_prim) in replacements {
            new_prims.push(box_prim);
        }
        *primitives = new_prims;
    }
}

/// Split a set of sample indices into spatially connected clusters.
/// Uses a simple grid-based spatial hash for O(n) clustering.
fn spatial_cluster(
    indices: &[usize],
    samples: &[SurfaceSample],
    cell_size: f64,
) -> Vec<Vec<usize>> {
    use std::collections::HashMap;

    if indices.len() < 2 || cell_size < 1e-10 {
        return vec![indices.to_vec()];
    }

    let inv = 1.0 / cell_size;
    let mut grid: HashMap<(i64, i64, i64), Vec<usize>> = HashMap::new();

    for &idx in indices {
        if idx >= samples.len() { continue; }
        let p = samples[idx].position;
        let key = (
            (p.x * inv).floor() as i64,
            (p.y * inv).floor() as i64,
            (p.z * inv).floor() as i64,
        );
        grid.entry(key).or_default().push(idx);
    }

    // BFS to find connected components
    let keys: Vec<(i64, i64, i64)> = grid.keys().cloned().collect();
    let mut visited: HashMap<(i64, i64, i64), bool> = HashMap::new();
    let mut clusters = Vec::new();

    for key in &keys {
        if visited.get(key).copied().unwrap_or(false) { continue; }

        let mut cluster_indices = Vec::new();
        let mut queue = vec![*key];
        visited.insert(*key, true);

        while let Some(k) = queue.pop() {
            if let Some(pts) = grid.get(&k) {
                cluster_indices.extend_from_slice(pts);
            }
            // Check 26 neighbors
            for dz in -1..=1i64 {
                for dy in -1..=1i64 {
                    for dx in -1..=1i64 {
                        if dx == 0 && dy == 0 && dz == 0 { continue; }
                        let nk = (k.0 + dx, k.1 + dy, k.2 + dz);
                        if grid.contains_key(&nk) && !visited.get(&nk).copied().unwrap_or(false) {
                            visited.insert(nk, true);
                            queue.push(nk);
                        }
                    }
                }
            }
        }

        if !cluster_indices.is_empty() {
            clusters.push(cluster_indices);
        }
    }

    clusters
}

fn bboxes_overlap(a: &(Vec3, Vec3), b: &(Vec3, Vec3)) -> bool {
    let (a_lo, a_hi) = a;
    let (b_lo, b_hi) = b;
    a_lo.x <= b_hi.x && a_hi.x >= b_lo.x
        && a_lo.y <= b_hi.y && a_hi.y >= b_lo.y
        && a_lo.z <= b_hi.z && a_hi.z >= b_lo.z
}

/// Re-fit primitive parameters from support set, then expand inliers.
/// Runs 2 iterations of refine→expand to converge on the best fit.
fn refine_and_expand(
    prim: &mut DetectedPrimitive,
    samples: &[SurfaceSample],
    remaining: &[bool],
    config: &DecompileConfig,
) {
    for _ in 0..2 {
        match &mut prim.kind {
            PrimitiveKind::Sphere { center, radius } => {
                // Re-estimate center as mean of support positions minus their radial direction
                if prim.support.len() < 4 { return; }
                let mut sum_c = Vec3::zero();
                let mut count = 0.0;
                for &idx in &prim.support {
                    if idx >= samples.len() { continue; }
                    let p = samples[idx].position;
                    let n = samples[idx].normal;
                    let c = p - n * *radius;
                    sum_c = sum_c + c;
                    count += 1.0;
                }
                if count < 1.0 { return; }
                *center = sum_c * (1.0 / count);

                // Re-estimate radius
                let mut sum_r = 0.0;
                for &idx in &prim.support {
                    if idx >= samples.len() { continue; }
                    sum_r += (samples[idx].position - *center).length();
                }
                *radius = sum_r / count;

                // Expand: re-scan all remaining points
                let thresh = config.distance_threshold * 1.5;
                prim.support.clear();
                prim.fit_error = 0.0;
                for (idx, &active) in remaining.iter().enumerate() {
                    if !active || idx >= samples.len() { continue; }
                    let dist = ((samples[idx].position - *center).length() - *radius).abs();
                    if dist < thresh {
                        prim.support.push(idx);
                        prim.fit_error += dist;
                    }
                }
            }
            PrimitiveKind::Cylinder { point, axis, radius } => {
                if prim.support.len() < 5 { return; }
                // Re-estimate radius
                let mut sum_r = 0.0;
                let mut count = 0.0;
                for &idx in &prim.support {
                    if idx >= samples.len() { continue; }
                    let p = samples[idx].position;
                    let v = p - *point;
                    let along = *axis * v.dot(*axis);
                    let radial = (v - along).length();
                    sum_r += radial;
                    count += 1.0;
                }
                if count < 1.0 { return; }
                *radius = sum_r / count;

                let thresh = config.distance_threshold * 1.5;
                prim.support.clear();
                prim.fit_error = 0.0;
                for (idx, &active) in remaining.iter().enumerate() {
                    if !active || idx >= samples.len() { continue; }
                    let p = samples[idx].position;
                    let v = p - *point;
                    let along = *axis * v.dot(*axis);
                    let dist = ((v - along).length() - *radius).abs();
                    if dist < thresh {
                        prim.support.push(idx);
                        prim.fit_error += dist;
                    }
                }
            }
            PrimitiveKind::Plane { point, normal } => {
                if prim.support.len() < 3 { return; }
                // Re-estimate normal and offset from support mean
                let mut sum_n = Vec3::zero();
                let mut sum_p = Vec3::zero();
                let mut count = 0.0;
                for &idx in &prim.support {
                    if idx >= samples.len() { continue; }
                    sum_n = sum_n + samples[idx].normal;
                    sum_p = sum_p + samples[idx].position;
                    count += 1.0;
                }
                if count < 1.0 { return; }
                *normal = sum_n.normalized();
                *point = sum_p * (1.0 / count);

                let d = normal.dot(*point);
                let thresh = config.distance_threshold * 1.5;
                prim.support.clear();
                prim.fit_error = 0.0;
                for (idx, &active) in remaining.iter().enumerate() {
                    if !active || idx >= samples.len() { continue; }
                    let dist = (normal.dot(samples[idx].position) - d).abs();
                    let agree = normal.dot(samples[idx].normal).abs();
                    if dist < thresh && agree > config.normal_threshold * 0.9 {
                        prim.support.push(idx);
                        prim.fit_error += dist;
                    }
                }
            }
            PrimitiveKind::Box { .. } => {}
        }
    }
}

fn ransac_detect(
    samples: &[SurfaceSample],
    remaining: &[bool],
    density_map: &DensityMap,
    config: &DecompileConfig,
) -> Option<DetectedPrimitive> {
    let active: Vec<usize> = remaining.iter().enumerate()
        .filter(|(_, &r)| r)
        .map(|(i, _)| i)
        .collect();

    if active.len() < 3 {
        return None;
    }

    let mut best: Option<DetectedPrimitive> = None;
    let mut rng = SimpleRng::new(42);

    // Global geometric detection (box, sphere) before random RANSAC
    for candidate in [
        try_fit_box(samples, &active, config),
        try_fit_global_sphere(samples, &active, config),
    ].into_iter().flatten() {
        let score = fit_score(&candidate);
        let dominated = match &best {
            Some(b) => score > fit_score(b),
            None => true,
        };
        if dominated {
            best = Some(candidate);
        }
    }

    for _ in 0..config.ransac_iterations {
        let candidates = [
            try_fit_plane(samples, &active, &mut rng, config),
            try_fit_sphere(samples, &active, &mut rng, config),
            try_fit_cylinder(samples, &active, density_map, &mut rng, config),
        ];

        for candidate in candidates.into_iter().flatten() {
            let dominated = match &best {
                Some(b) => {
                    let c_score = fit_score(&candidate);
                    let b_score = fit_score(b);
                    c_score > b_score
                }
                None => true,
            };
            if dominated {
                best = Some(candidate);
            }
        }
    }

    best
}

/// Quality-weighted score for primitive ranking.
/// Planes get a strong bonus (simpler parametric form, merge into boxes later).
/// Score = support_count * quality_factor / (1 + mean_error)
fn fit_score(prim: &DetectedPrimitive) -> f64 {
    let support = prim.support.len() as f64;
    let mean_err = if prim.support.is_empty() {
        1.0
    } else {
        prim.fit_error / support
    };
    let complexity_bonus = match &prim.kind {
        PrimitiveKind::Plane { .. } => 1.5,
        PrimitiveKind::Sphere { .. } => 1.0,
        PrimitiveKind::Cylinder { .. } => 0.8,
        PrimitiveKind::Box { .. } => 1.3,
    };
    support * complexity_bonus / (1.0 + mean_err)
}

fn try_fit_plane(
    samples: &[SurfaceSample],
    active: &[usize],
    rng: &mut SimpleRng,
    config: &DecompileConfig,
) -> Option<DetectedPrimitive> {
    if active.len() < 3 {
        return None;
    }

    let i0 = active[rng.next_usize(active.len())];
    let i1 = active[rng.next_usize(active.len())];
    let i2 = active[rng.next_usize(active.len())];
    if i0 == i1 || i1 == i2 || i0 == i2 {
        return None;
    }

    let p0 = samples[i0].position;
    let p1 = samples[i1].position;
    let p2 = samples[i2].position;

    let normal = (p1 - p0).cross(p2 - p0).normalized();
    if normal.length() < 0.5 {
        return None;
    }

    let d = normal.dot(p0);

    let mut support = Vec::new();
    let mut total_error = 0.0;

    for &idx in active {
        let dist = (normal.dot(samples[idx].position) - d).abs();
        let normal_agree = normal.dot(samples[idx].normal).abs();
        if dist < config.distance_threshold && normal_agree > config.normal_threshold {
            total_error += dist;
            support.push(idx);
        }
    }

    if support.len() < 3 {
        return None;
    }

    Some(DetectedPrimitive {
        kind: PrimitiveKind::Plane { point: p0, normal },
        support,
        fit_error: total_error,
    })
}

fn try_fit_sphere(
    samples: &[SurfaceSample],
    active: &[usize],
    rng: &mut SimpleRng,
    config: &DecompileConfig,
) -> Option<DetectedPrimitive> {
    if active.len() < 4 {
        return None;
    }

    // Pick two points; use their normals to estimate center.
    // If normals point radially from a common center, the intersection
    // of the two normal rays gives the center.
    let i0 = active[rng.next_usize(active.len())];
    let i1 = active[rng.next_usize(active.len())];
    if i0 == i1 { return None; }

    let p0 = samples[i0].position;
    let n0 = samples[i0].normal;
    let p1 = samples[i1].position;
    let n1 = samples[i1].normal;

    // Closest point between two rays: p0 + t*n0 and p1 + s*n1
    let center = closest_point_two_rays(p0, n0, p1, n1)?;

    let r0 = (center - p0).length();
    let r1 = (center - p1).length();
    if (r0 - r1).abs() > config.distance_threshold * 10.0 {
        return None;
    }
    let radius = (r0 + r1) * 0.5;
    if radius < 1e-6 {
        return None;
    }

    let mut support = Vec::new();
    let mut total_error = 0.0;

    for &idx in active {
        let dist = ((samples[idx].position - center).length() - radius).abs();
        if dist < config.distance_threshold {
            total_error += dist;
            support.push(idx);
        }
    }

    if support.len() < 4 {
        return None;
    }

    Some(DetectedPrimitive {
        kind: PrimitiveKind::Sphere { center, radius },
        support,
        fit_error: total_error,
    })
}

fn try_fit_cylinder(
    samples: &[SurfaceSample],
    active: &[usize],
    density_map: &DensityMap,
    rng: &mut SimpleRng,
    config: &DecompileConfig,
) -> Option<DetectedPrimitive> {
    if active.len() < 5 {
        return None;
    }

    // Cylinder detection: pick two points with curved surface type,
    // cross their normals to get the axis direction.
    let curved: Vec<usize> = active.iter()
        .filter(|&&idx| {
            density_map.cells.get(&samples[idx].cell_key)
                .map_or(false, |d| d.surface_type == SurfaceType::Curved)
        })
        .copied()
        .collect();

    let source = if curved.len() >= 2 { &curved } else { active };

    let i0 = source[rng.next_usize(source.len())];
    let i1 = source[rng.next_usize(source.len())];
    if i0 == i1 { return None; }

    let n0 = samples[i0].normal;
    let n1 = samples[i1].normal;

    let axis = n0.cross(n1).normalized();
    if axis.length() < 0.3 {
        return None;
    }

    // Project points onto the plane perpendicular to the axis.
    // The cylinder's cross-section in that plane is a circle.
    let p0_proj = project_onto_plane(samples[i0].position, axis);
    let p1_proj = project_onto_plane(samples[i1].position, axis);

    // Use the normals (also projected) to find the center
    let n0_proj = project_onto_plane(n0, axis).normalized();
    let n1_proj = project_onto_plane(n1, axis).normalized();

    let center_2d = closest_point_two_rays(p0_proj, n0_proj, p1_proj, n1_proj)?;

    let r0 = (p0_proj - center_2d).length();
    let r1 = (p1_proj - center_2d).length();
    if (r0 - r1).abs() > config.distance_threshold * 10.0 {
        return None;
    }
    let radius = (r0 + r1) * 0.5;
    if radius < 1e-6 {
        return None;
    }

    // center_2d is in the projected plane; reconstruct 3D point on axis
    let axis_point = center_2d; // The projection zeroed the axis component

    let mut support = Vec::new();
    let mut total_error = 0.0;

    for &idx in active {
        let p = samples[idx].position;
        let p_proj = project_onto_plane(p, axis);
        let dist = ((p_proj - center_2d).length() - radius).abs();
        if dist >= config.distance_threshold { continue; }

        // Normal check: the point's normal projected onto the cross-section plane
        // should be roughly radial from the center (for side surface points)
        let n = samples[idx].normal;
        let n_along_axis = axis * n.dot(axis);
        let n_radial = (n - n_along_axis).normalized();
        let expected_radial = (p_proj - center_2d).normalized();
        let normal_agree = n_radial.dot(expected_radial).abs();

        // Accept if normal is radial (side surface) or axial (cap)
        let is_axial = n.dot(axis).abs() > 0.8;
        if normal_agree > 0.5 || is_axial {
            total_error += dist;
            support.push(idx);
        }
    }

    if support.len() < 5 {
        return None;
    }

    Some(DetectedPrimitive {
        kind: PrimitiveKind::Cylinder { point: axis_point, axis, radius },
        support,
        fit_error: total_error,
    })
}

/// Axis-aligned box detection.
///
/// Global sphere detection using normal convergence.
///
/// For each point, the estimated center is `p - r*n`. If many points
/// produce a consistent center, they form a sphere. Estimates center
/// from a subsample, then scores all active points.
fn try_fit_global_sphere(
    samples: &[SurfaceSample],
    active: &[usize],
    config: &DecompileConfig,
) -> Option<DetectedPrimitive> {
    if active.len() < 10 {
        return None;
    }

    // Estimate a candidate radius from the bounding extent of active points
    let mut min_p = Vec3::new(f64::MAX, f64::MAX, f64::MAX);
    let mut max_p = Vec3::new(f64::MIN, f64::MIN, f64::MIN);
    for &idx in active {
        let p = samples[idx].position;
        min_p.x = min_p.x.min(p.x); min_p.y = min_p.y.min(p.y); min_p.z = min_p.z.min(p.z);
        max_p.x = max_p.x.max(p.x); max_p.y = max_p.y.max(p.y); max_p.z = max_p.z.max(p.z);
    }
    let extent = max_p - min_p;
    let max_dim = extent.x.max(extent.y).max(extent.z);
    let min_dim = extent.x.min(extent.y).min(extent.z);

    // Sphere test: all three dimensions should be similar
    if min_dim < max_dim * 0.75 {
        return None;
    }

    let r_est = max_dim / 2.0;

    // Estimate center: for each point, center = position - normal * radius
    // Use a subsample for speed
    let step = (active.len() / 200).max(1);
    let mut sum_c = Vec3::zero();
    let mut count = 0.0;
    for i in (0..active.len()).step_by(step) {
        let idx = active[i];
        let p = samples[idx].position;
        let n = samples[idx].normal;
        let c = p - n * r_est;
        sum_c = sum_c + c;
        count += 1.0;
    }
    if count < 3.0 { return None; }
    let mut center = sum_c * (1.0 / count);

    // Refine radius from estimated center
    let mut sum_r = 0.0;
    let mut r_count = 0.0;
    for i in (0..active.len()).step_by(step) {
        let idx = active[i];
        let r = (samples[idx].position - center).length();
        sum_r += r;
        r_count += 1.0;
    }
    let mut radius = sum_r / r_count;

    // Second pass: refine center using all points near the sphere
    let thresh = config.distance_threshold * 2.0;
    for _ in 0..3 {
        let mut new_center = Vec3::zero();
        let mut new_count = 0.0;
        let mut new_radius = 0.0;
        for &idx in active {
            let p = samples[idx].position;
            let dist = ((p - center).length() - radius).abs();
            if dist < thresh {
                let n = samples[idx].normal;
                new_center = new_center + (p - n * radius);
                new_radius += (p - center).length();
                new_count += 1.0;
            }
        }
        if new_count < 3.0 { break; }
        center = new_center * (1.0 / new_count);
        radius = new_radius / new_count;
    }

    // Score
    let mut support = Vec::new();
    let mut total_error = 0.0;
    for &idx in active {
        let dist = ((samples[idx].position - center).length() - radius).abs();
        if dist < thresh {
            support.push(idx);
            total_error += dist;
        }
    }

    let min_support = (active.len() as f64 * 0.3).max(10.0) as usize;
    if support.len() < min_support {
        return None;
    }

    Some(DetectedPrimitive {
        kind: PrimitiveKind::Sphere { center, radius },
        support,
        fit_error: total_error,
    })
}

/// Axis-aligned box detection.
///
/// Groups active points by which axis-aligned face they belong to
/// (based on normal direction), then checks if 4+ groups describe
/// a consistent box. Requires points on at least 4 of the 6 faces.
fn try_fit_box(
    samples: &[SurfaceSample],
    active: &[usize],
    config: &DecompileConfig,
) -> Option<DetectedPrimitive> {
    if active.len() < 6 {
        return None;
    }

    let normal_threshold = 0.95;

    // Bucket points by face normal: +X, -X, +Y, -Y, +Z, -Z
    let mut face_points: [Vec<usize>; 6] = Default::default();

    for &idx in active {
        let n = samples[idx].normal;
        let ax = n.x.abs();
        let ay = n.y.abs();
        let az = n.z.abs();
        let max_comp = ax.max(ay).max(az);
        if max_comp < normal_threshold { continue; }

        if ax == max_comp {
            if n.x > 0.0 { face_points[0].push(idx); }
            else { face_points[1].push(idx); }
        } else if ay == max_comp {
            if n.y > 0.0 { face_points[2].push(idx); }
            else { face_points[3].push(idx); }
        } else {
            if n.z > 0.0 { face_points[4].push(idx); }
            else { face_points[5].push(idx); }
        }
    }

    // Need points on at least 4 faces to identify a box
    let populated_faces = face_points.iter().filter(|f| f.len() >= 3).count();
    if populated_faces < 4 {
        return None;
    }

    // Axis-aligned points should be a majority of all active points.
    // For a true box, most surface points have axis-aligned normals.
    // For a cylinder, only cap faces are axis-aligned.
    let aligned_total: usize = face_points.iter().map(|f| f.len()).sum();
    if (aligned_total as f64) < active.len() as f64 * 0.4 {
        return None;
    }

    // Verify each face is actually planar: low variance along the face normal axis
    for (i, points) in face_points.iter().enumerate() {
        if points.len() < 3 { continue; }
        let axis_idx = i / 2;
        let vals: Vec<f64> = points.iter().map(|&idx| {
            let p = samples[idx].position;
            match axis_idx { 0 => p.x, 1 => p.y, _ => p.z }
        }).collect();
        let mean = vals.iter().sum::<f64>() / vals.len() as f64;
        let variance = vals.iter().map(|v| (v - mean).powi(2)).sum::<f64>() / vals.len() as f64;
        let std_dev = variance.sqrt();
        // If variance is high relative to the threshold, this isn't a flat face
        if std_dev > config.distance_threshold * 3.0 {
            return None;
        }
    }

    // Compute face positions (average coordinate along the face normal axis)
    let mut face_pos = [f64::NAN; 6];
    for (i, points) in face_points.iter().enumerate() {
        if points.len() < 3 { continue; }
        let axis_idx = i / 2;
        let sum: f64 = points.iter().map(|&idx| {
            let p = samples[idx].position;
            match axis_idx { 0 => p.x, 1 => p.y, _ => p.z }
        }).sum();
        face_pos[i] = sum / points.len() as f64;
    }

    // Verify consistency: opposing faces should bracket the box
    let mut center = Vec3::zero();
    let mut half = [0.0f64; 3];
    let mut axes_found = 0;

    for axis_idx in 0..3 {
        let pos_face = axis_idx * 2;
        let neg_face = axis_idx * 2 + 1;
        if face_pos[pos_face].is_nan() && face_pos[neg_face].is_nan() {
            continue;
        }

        if !face_pos[pos_face].is_nan() && !face_pos[neg_face].is_nan() {
            let hi = face_pos[pos_face].max(face_pos[neg_face]);
            let lo = face_pos[pos_face].min(face_pos[neg_face]);
            let mid = (hi + lo) / 2.0;
            let h = (hi - lo) / 2.0;
            match axis_idx {
                0 => center.x = mid,
                1 => center.y = mid,
                _ => center.z = mid,
            }
            half[axis_idx] = h;
            axes_found += 1;
        } else {
            // Single face found: estimate from point extent along this axis
            let face_idx = if !face_pos[pos_face].is_nan() { pos_face } else { neg_face };
            let all_box_points: Vec<usize> = face_points.iter().flat_map(|f| f.iter().copied()).collect();
            let (lo, hi) = extent_along_axis(&all_box_points, samples, axis_idx);
            let mid = (hi + lo) / 2.0;
            let h = (hi - lo) / 2.0;
            if h < 1e-6 { continue; }
            match axis_idx {
                0 => center.x = mid,
                1 => center.y = mid,
                _ => center.z = mid,
            }
            half[axis_idx] = h;
            let _ = face_idx;
            axes_found += 1;
        }
    }

    if axes_found < 2 {
        return None;
    }

    // If one axis is missing, estimate from the extent of support points
    if axes_found == 2 {
        for axis_idx in 0..3 {
            if half[axis_idx] > 1e-6 { continue; }
            let all_box_points: Vec<usize> = face_points.iter().flat_map(|f| f.iter().copied()).collect();
            let (lo, hi) = extent_along_axis(&all_box_points, samples, axis_idx);
            let mid = (hi + lo) / 2.0;
            let h = (hi - lo) / 2.0;
            if h < 1e-6 { continue; }
            match axis_idx {
                0 => center.x = mid,
                1 => center.y = mid,
                _ => center.z = mid,
            }
            half[axis_idx] = h;
        }
    }

    if half[0] < 1e-6 || half[1] < 1e-6 || half[2] < 1e-6 {
        return None;
    }

    // Score: use wider threshold for box scoring to capture edge/corner samples
    let mut support = Vec::new();
    let mut total_error = 0.0;
    let thresh = config.distance_threshold * 2.0;

    for &idx in active {
        let p = samples[idx].position;
        let local = Vec3::new(
            (p.x - center.x).abs(),
            (p.y - center.y).abs(),
            (p.z - center.z).abs(),
        );
        // SDF of a box at origin with half_extents
        let q = Vec3::new(
            local.x - half[0],
            local.y - half[1],
            local.z - half[2],
        );
        let outside = Vec3::new(q.x.max(0.0), q.y.max(0.0), q.z.max(0.0));
        let inside_dist = q.x.max(q.y).max(q.z).min(0.0);
        let dist = (outside.length() + inside_dist).abs();

        if dist < thresh {
            total_error += dist;
            support.push(idx);
        }
    }

    let min_support = (active.len() as f64 * 0.1).max(6.0) as usize;
    if support.len() < min_support {
        return None;
    }

    Some(DetectedPrimitive {
        kind: PrimitiveKind::Box {
            center,
            half_extents: half,
            rotation_axis: Vec3::new(1.0, 0.0, 0.0),
            rotation_angle: 0.0,
        },
        support,
        fit_error: total_error,
    })
}

/// Relaxed box detection for consolidation pass.
/// Lower normal threshold and aligned fraction requirement.
fn try_fit_box_relaxed(
    samples: &[SurfaceSample],
    active: &[usize],
    config: &DecompileConfig,
) -> Option<DetectedPrimitive> {
    if active.len() < 6 {
        return None;
    }

    let normal_threshold = 0.85;
    let mut face_points: [Vec<usize>; 6] = Default::default();

    for &idx in active {
        if idx >= samples.len() { continue; }
        let n = samples[idx].normal;
        let ax = n.x.abs();
        let ay = n.y.abs();
        let az = n.z.abs();
        let max_comp = ax.max(ay).max(az);
        if max_comp < normal_threshold { continue; }

        if ax == max_comp {
            if n.x > 0.0 { face_points[0].push(idx); }
            else { face_points[1].push(idx); }
        } else if ay == max_comp {
            if n.y > 0.0 { face_points[2].push(idx); }
            else { face_points[3].push(idx); }
        } else {
            if n.z > 0.0 { face_points[4].push(idx); }
            else { face_points[5].push(idx); }
        }
    }

    let populated_faces = face_points.iter().filter(|f| f.len() >= 3).count();
    if populated_faces < 4 {
        return None;
    }

    // Relaxed: only 25% axis-aligned required
    let aligned_total: usize = face_points.iter().map(|f| f.len()).sum();
    if (aligned_total as f64) < active.len() as f64 * 0.25 {
        return None;
    }

    // Face planarity check
    for (i, points) in face_points.iter().enumerate() {
        if points.len() < 3 { continue; }
        let axis_idx = i / 2;
        let vals: Vec<f64> = points.iter().map(|&idx| {
            let p = samples[idx].position;
            match axis_idx { 0 => p.x, 1 => p.y, _ => p.z }
        }).collect();
        let mean = vals.iter().sum::<f64>() / vals.len() as f64;
        let variance = vals.iter().map(|v| (v - mean).powi(2)).sum::<f64>() / vals.len() as f64;
        if variance.sqrt() > config.distance_threshold * 5.0 {
            return None;
        }
    }

    let mut face_pos = [f64::NAN; 6];
    for (i, points) in face_points.iter().enumerate() {
        if points.len() < 3 { continue; }
        let axis_idx = i / 2;
        let sum: f64 = points.iter().map(|&idx| {
            let p = samples[idx].position;
            match axis_idx { 0 => p.x, 1 => p.y, _ => p.z }
        }).sum();
        face_pos[i] = sum / points.len() as f64;
    }

    let mut center = Vec3::zero();
    let mut half = [0.0f64; 3];
    let mut axes_found = 0;

    for axis_idx in 0..3 {
        let pos_face = axis_idx * 2;
        let neg_face = axis_idx * 2 + 1;
        if face_pos[pos_face].is_nan() && face_pos[neg_face].is_nan() {
            continue;
        }
        if !face_pos[pos_face].is_nan() && !face_pos[neg_face].is_nan() {
            let hi = face_pos[pos_face].max(face_pos[neg_face]);
            let lo = face_pos[pos_face].min(face_pos[neg_face]);
            match axis_idx {
                0 => center.x = (hi + lo) / 2.0,
                1 => center.y = (hi + lo) / 2.0,
                _ => center.z = (hi + lo) / 2.0,
            }
            half[axis_idx] = (hi - lo) / 2.0;
            axes_found += 1;
        } else {
            let all_pts: Vec<usize> = face_points.iter().flat_map(|f| f.iter().copied()).collect();
            let (lo, hi) = extent_along_axis(&all_pts, samples, axis_idx);
            if hi - lo < 1e-6 { continue; }
            match axis_idx {
                0 => center.x = (hi + lo) / 2.0,
                1 => center.y = (hi + lo) / 2.0,
                _ => center.z = (hi + lo) / 2.0,
            }
            half[axis_idx] = (hi - lo) / 2.0;
            axes_found += 1;
        }
    }

    if axes_found < 2 || half[0] < 1e-6 || half[1] < 1e-6 || half[2] < 1e-6 {
        // Estimate missing dimensions from overall extent
        for axis_idx in 0..3 {
            if half[axis_idx] >= 1e-6 { continue; }
            let (lo, hi) = extent_along_axis(active, samples, axis_idx);
            if hi - lo < 1e-6 { return None; }
            match axis_idx {
                0 => center.x = (hi + lo) / 2.0,
                1 => center.y = (hi + lo) / 2.0,
                _ => center.z = (hi + lo) / 2.0,
            }
            half[axis_idx] = (hi - lo) / 2.0;
        }
    }

    if half[0] < 1e-6 || half[1] < 1e-6 || half[2] < 1e-6 {
        return None;
    }

    let mut support = Vec::new();
    let mut total_error = 0.0;
    let thresh = config.distance_threshold * 2.0;

    for &idx in active {
        if idx >= samples.len() { continue; }
        let p = samples[idx].position;
        let local = Vec3::new(
            (p.x - center.x).abs(),
            (p.y - center.y).abs(),
            (p.z - center.z).abs(),
        );
        let q = Vec3::new(local.x - half[0], local.y - half[1], local.z - half[2]);
        let outside = Vec3::new(q.x.max(0.0), q.y.max(0.0), q.z.max(0.0));
        let inside_dist = q.x.max(q.y).max(q.z).min(0.0);
        let dist = (outside.length() + inside_dist).abs();
        if dist < thresh {
            support.push(idx);
            total_error += dist;
        }
    }

    let min_support = (active.len() as f64 * 0.3).max(6.0) as usize;
    if support.len() < min_support {
        return None;
    }

    Some(DetectedPrimitive {
        kind: PrimitiveKind::Box {
            center,
            half_extents: half,
            rotation_axis: Vec3::new(1.0, 0.0, 0.0),
            rotation_angle: 0.0,
        },
        support,
        fit_error: total_error,
    })
}

fn extent_along_axis(points: &[usize], samples: &[SurfaceSample], axis: usize) -> (f64, f64) {
    let mut lo = f64::MAX;
    let mut hi = f64::MIN;
    for &idx in points {
        let p = samples[idx].position;
        let v = match axis { 0 => p.x, 1 => p.y, _ => p.z };
        lo = lo.min(v);
        hi = hi.max(v);
    }
    (lo, hi)
}

fn project_onto_plane(v: Vec3, normal: Vec3) -> Vec3 {
    v - normal * v.dot(normal)
}

fn closest_point_two_rays(p0: Vec3, d0: Vec3, p1: Vec3, d1: Vec3) -> Option<Vec3> {
    let w0 = p0 - p1;
    let a = d0.dot(d0);
    let b = d0.dot(d1);
    let c = d1.dot(d1);
    let d = d0.dot(w0);
    let e = d1.dot(w0);

    let denom = a * c - b * b;
    if denom.abs() < 1e-10 {
        return None;
    }

    let t = (b * e - c * d) / denom;
    let s = (a * e - b * d) / denom;

    let closest_on_0 = p0 + d0 * t;
    let closest_on_1 = p1 + d1 * s;

    Some(Vec3 {
        x: (closest_on_0.x + closest_on_1.x) * 0.5,
        y: (closest_on_0.y + closest_on_1.y) * 0.5,
        z: (closest_on_0.z + closest_on_1.z) * 0.5,
    })
}

/// Xorshift64 RNG for RANSAC sampling.
struct SimpleRng {
    state: u64,
}

impl SimpleRng {
    fn new(seed: u64) -> Self {
        Self { state: seed.wrapping_add(1) }
    }

    fn next_u64(&mut self) -> u64 {
        self.state ^= self.state << 13;
        self.state ^= self.state >> 7;
        self.state ^= self.state << 17;
        self.state
    }

    fn next_usize(&mut self, max: usize) -> usize {
        (self.next_u64() % max as u64) as usize
    }
}