use crate::fit::{DetectedPrimitive, PrimitiveKind};
use crate::mesh::Vec3;
use crate::sparse_grid::SurfaceSample;
use cord_sdf::SdfNode;

/// Build an SDF tree from detected primitives.
///
/// Runs box-merging on detected planes before tree construction.
/// Each entry in `subtractive` flags whether the corresponding
/// primitive represents removed material (hole/cutout). Additive
/// primitives are unioned; subtractive ones are differenced from
/// the base.
pub fn build_sdf_tree(
    primitives: &[DetectedPrimitive],
    samples: &[SurfaceSample],
    subtractive: &[bool],
) -> SdfNode {
    let (merged, merged_sub) = merge_planes_into_boxes(primitives, subtractive, samples);

    if merged.is_empty() {
        return SdfNode::Sphere { radius: 1.0 };
    }

    if merged.len() == 1 {
        return primitive_to_sdf(&merged[0], samples);
    }

    let mut additive = Vec::new();
    let mut subtractive_nodes = Vec::new();

    for (i, prim) in merged.iter().enumerate() {
        let is_sub = merged_sub.get(i).copied().unwrap_or(false);
        if is_sub {
            subtractive_nodes.push(primitive_to_sdf(prim, samples));
        } else {
            additive.push(primitive_to_sdf(prim, samples));
        }
    }

    let base = if additive.len() == 1 {
        additive.remove(0)
    } else if additive.is_empty() {
        if subtractive_nodes.is_empty() {
            return SdfNode::Sphere { radius: 1.0 };
        }
        subtractive_nodes.remove(0)
    } else {
        SdfNode::Union(additive)
    };

    if subtractive_nodes.is_empty() {
        base
    } else {
        SdfNode::Difference {
            base: Box::new(base),
            subtract: subtractive_nodes,
        }
    }
}

/// Merge opposing parallel plane pairs into box primitives.
///
/// Scans for pairs of planes whose normals are antiparallel (dot < -0.95).
/// If three mutually orthogonal pairs are found, they form a box.
/// Remaining pairs that can't form a full box are emitted as-is.
fn merge_planes_into_boxes(
    primitives: &[DetectedPrimitive],
    subtractive: &[bool],
    _samples: &[SurfaceSample],
) -> (Vec<DetectedPrimitive>, Vec<bool>) {
    let mut plane_indices: Vec<usize> = Vec::new();
    let mut non_plane_indices: Vec<usize> = Vec::new();

    for (i, prim) in primitives.iter().enumerate() {
        match &prim.kind {
            PrimitiveKind::Plane { .. } => plane_indices.push(i),
            _ => non_plane_indices.push(i),
        }
    }

    if plane_indices.len() < 2 {
        return (primitives.to_vec(), subtractive.to_vec());
    }

    // Find opposing plane pairs
    let mut used = vec![false; primitives.len()];
    let mut pairs: Vec<(usize, usize, Vec3, f64)> = Vec::new(); // (i, j, axis, half_thickness)

    for a in 0..plane_indices.len() {
        if used[plane_indices[a]] { continue; }
        for b in (a + 1)..plane_indices.len() {
            if used[plane_indices[b]] { continue; }
            let ia = plane_indices[a];
            let ib = plane_indices[b];
            if let (
                PrimitiveKind::Plane { point: p1, normal: n1 },
                PrimitiveKind::Plane { point: p2, normal: n2 },
            ) = (&primitives[ia].kind, &primitives[ib].kind) {
                let dot = n1.dot(*n2);
                if dot < -0.95 {
                    // Opposing parallel planes
                    let axis = n1.normalized();
                    let d1 = axis.dot(*p1);
                    let d2 = axis.dot(*p2);
                    let half = (d1 - d2).abs() / 2.0;
                    if half > 1e-6 {
                        pairs.push((ia, ib, axis, half));
                        used[ia] = true;
                        used[ib] = true;
                        break;
                    }
                }
            }
        }
    }

    // Try to form boxes from 3 mutually orthogonal pairs
    let mut box_groups: Vec<Vec<usize>> = Vec::new(); // groups of pair indices
    let mut pair_used = vec![false; pairs.len()];

    for a in 0..pairs.len() {
        if pair_used[a] { continue; }
        for b in (a + 1)..pairs.len() {
            if pair_used[b] { continue; }
            let dot_ab = pairs[a].2.dot(pairs[b].2).abs();
            if dot_ab > 0.1 { continue; } // not orthogonal
            for c in (b + 1)..pairs.len() {
                if pair_used[c] { continue; }
                let dot_ac = pairs[a].2.dot(pairs[c].2).abs();
                let dot_bc = pairs[b].2.dot(pairs[c].2).abs();
                if dot_ac < 0.1 && dot_bc < 0.1 {
                    box_groups.push(vec![a, b, c]);
                    pair_used[a] = true;
                    pair_used[b] = true;
                    pair_used[c] = true;
                    break;
                }
            }
            if pair_used[a] { break; }
        }
    }

    let mut result_prims: Vec<DetectedPrimitive> = Vec::new();
    let mut result_sub: Vec<bool> = Vec::new();

    // Emit detected boxes
    for group in &box_groups {
        let mut half_extents = [0.0f64; 3];
        let mut center = Vec3::zero();
        let mut axes = [Vec3::zero(); 3];
        let mut total_support = Vec::new();
        let mut total_error = 0.0;
        let mut sub_votes = 0usize;
        let mut add_votes = 0usize;

        for (slot, &pair_idx) in group.iter().enumerate() {
            let (ia, ib, axis, half) = &pairs[pair_idx];
            axes[slot] = *axis;
            half_extents[slot] = *half;

            if let (
                PrimitiveKind::Plane { point: p1, .. },
                PrimitiveKind::Plane { point: p2, .. },
            ) = (&primitives[*ia].kind, &primitives[*ib].kind) {
                let d1 = axis.dot(*p1);
                let d2 = axis.dot(*p2);
                let mid = (d1 + d2) / 2.0;
                center = center + *axis * mid;
            }

            total_support.extend_from_slice(&primitives[*ia].support);
            total_support.extend_from_slice(&primitives[*ib].support);
            total_error += primitives[*ia].fit_error + primitives[*ib].fit_error;

            let sa = subtractive.get(*ia).copied().unwrap_or(false);
            let sb = subtractive.get(*ib).copied().unwrap_or(false);
            if sa { sub_votes += 1; } else { add_votes += 1; }
            if sb { sub_votes += 1; } else { add_votes += 1; }
        }

        // Reorder half_extents to align with XYZ via the rotation
        // For now, emit as a box at the computed center with a rotation
        let (rot_axis, rot_angle, ordered_half) = align_box_axes(axes, half_extents);

        total_support.sort_unstable();
        total_support.dedup();

        result_prims.push(DetectedPrimitive {
            kind: PrimitiveKind::Box {
                center,
                half_extents: ordered_half,
                rotation_axis: rot_axis,
                rotation_angle: rot_angle,
            },
            support: total_support,
            fit_error: total_error,
        });
        result_sub.push(sub_votes > add_votes);
    }

    // Emit remaining unpaired pairs as planes
    for (i, pair) in pairs.iter().enumerate() {
        if pair_used[i] { continue; }
        let (ia, ib, _, _) = pair;
        // Re-mark as unused so they get emitted below
        used[*ia] = false;
        used[*ib] = false;
    }

    // Emit non-plane primitives and unused planes
    for &i in &non_plane_indices {
        result_prims.push(primitives[i].clone());
        result_sub.push(subtractive.get(i).copied().unwrap_or(false));
    }
    for &i in &plane_indices {
        if !used[i] {
            result_prims.push(primitives[i].clone());
            result_sub.push(subtractive.get(i).copied().unwrap_or(false));
        }
    }

    (result_prims, result_sub)
}

/// Compute rotation to align detected box axes with XYZ.
/// Returns (axis, angle_radians, reordered_half_extents).
fn align_box_axes(
    axes: [Vec3; 3],
    half_extents: [f64; 3],
) -> (Vec3, f64, [f64; 3]) {
    let canonical = [
        Vec3::new(1.0, 0.0, 0.0),
        Vec3::new(0.0, 1.0, 0.0),
        Vec3::new(0.0, 0.0, 1.0),
    ];

    let mut assignment = [0usize; 3];
    let mut src_assigned = [false; 3];
    let mut dst_assigned = [false; 3];

    for _ in 0..3 {
        let mut best_dot = 0.0f64;
        let mut best_src = 0;
        let mut best_dst = 0;
        for src in 0..3 {
            if src_assigned[src] { continue; }
            for dst in 0..3 {
                if dst_assigned[dst] { continue; }
                let d = axes[src].dot(canonical[dst]).abs();
                if d > best_dot {
                    best_dot = d;
                    best_src = src;
                    best_dst = dst;
                }
            }
        }
        assignment[best_src] = best_dst;
        src_assigned[best_src] = true;
        dst_assigned[best_dst] = true;
    }

    let mut ordered = [0.0; 3];
    for i in 0..3 {
        ordered[assignment[i]] = half_extents[i];
    }

    // Check if axes are already aligned (common case for axis-aligned boxes)
    let mut sum_dot = 0.0;
    for i in 0..3 {
        sum_dot += axes[i].dot(canonical[assignment[i]]).abs();
    }

    if sum_dot > 2.9 {
        // Nearly axis-aligned, no rotation needed
        return (Vec3::new(1.0, 0.0, 0.0), 0.0, ordered);
    }

    // General case: compute rotation from detected frame to canonical
    // Use the first axis mismatch to derive an axis-angle rotation
    let from = axes[0].normalized();
    let to = canonical[assignment[0]];
    let cross = from.cross(to);
    let dot = from.dot(to);
    let angle = dot.acos();
    let axis = if cross.length() > 1e-6 { cross.normalized() } else { Vec3::new(1.0, 0.0, 0.0) };

    (axis, angle, ordered)
}

/// Ideal outward-pointing normal of a primitive at a given point.
pub(crate) fn ideal_outward_normal(kind: &PrimitiveKind, point: Vec3) -> Vec3 {
    match kind {
        PrimitiveKind::Plane { normal, .. } => *normal,

        PrimitiveKind::Sphere { center, .. } => {
            (point - *center).normalized()
        }

        PrimitiveKind::Cylinder { point: axis_point, axis, .. } => {
            let v = point - *axis_point;
            let along = *axis * v.dot(*axis);
            (v - along).normalized()
        }

        PrimitiveKind::Box { center, .. } => {
            (point - *center).normalized()
        }
    }
}

fn primitive_to_sdf(prim: &DetectedPrimitive, samples: &[SurfaceSample]) -> SdfNode {
    match &prim.kind {
        PrimitiveKind::Plane { point, normal } => {
            // Plane approximated as large thin box along the normal
            let thickness = 0.1;
            let extent = 1000.0;

            let (axis, angle) = rotation_to_align_z(*normal);
            let node = SdfNode::Box {
                half_extents: [extent, extent, thickness],
            };
            let rotated = if angle.abs() > 1e-6 {
                SdfNode::Rotate {
                    axis: [axis.x, axis.y, axis.z],
                    angle_deg: angle.to_degrees(),
                    child: Box::new(node),
                }
            } else {
                node
            };

            SdfNode::Translate {
                offset: [point.x, point.y, point.z],
                child: Box::new(rotated),
            }
        }

        PrimitiveKind::Sphere { center, radius } => {
            let node = SdfNode::Sphere { radius: *radius };
            if center.x.abs() < 1e-6 && center.y.abs() < 1e-6 && center.z.abs() < 1e-6 {
                node
            } else {
                SdfNode::Translate {
                    offset: [center.x, center.y, center.z],
                    child: Box::new(node),
                }
            }
        }

        PrimitiveKind::Cylinder { point, axis, radius } => {
            let height = cylinder_height_from_support(prim, *axis, samples);

            let (rot_axis, angle) = rotation_to_align_z(*axis);
            let node = SdfNode::Cylinder { radius: *radius, height };
            let rotated = if angle.abs() > 1e-6 {
                SdfNode::Rotate {
                    axis: [rot_axis.x, rot_axis.y, rot_axis.z],
                    angle_deg: angle.to_degrees(),
                    child: Box::new(node),
                }
            } else {
                node
            };

            SdfNode::Translate {
                offset: [point.x, point.y, point.z],
                child: Box::new(rotated),
            }
        }

        PrimitiveKind::Box { center, half_extents, rotation_axis, rotation_angle } => {
            let node = SdfNode::Box { half_extents: *half_extents };
            let rotated = if rotation_angle.abs() > 1e-6 {
                SdfNode::Rotate {
                    axis: [rotation_axis.x, rotation_axis.y, rotation_axis.z],
                    angle_deg: rotation_angle.to_degrees(),
                    child: Box::new(node),
                }
            } else {
                node
            };

            if center.x.abs() < 1e-6 && center.y.abs() < 1e-6 && center.z.abs() < 1e-6 {
                rotated
            } else {
                SdfNode::Translate {
                    offset: [center.x, center.y, center.z],
                    child: Box::new(rotated),
                }
            }
        }
    }
}

/// Estimate cylinder height from support points projected onto the axis.
fn cylinder_height_from_support(prim: &DetectedPrimitive, axis: Vec3, samples: &[SurfaceSample]) -> f64 {
    if prim.support.is_empty() {
        return 10.0;
    }
    let mut min_t = f64::MAX;
    let mut max_t = f64::MIN;
    for &idx in &prim.support {
        if idx >= samples.len() { continue; }
        let t = samples[idx].position.dot(axis);
        min_t = min_t.min(t);
        max_t = max_t.max(t);
    }
    if max_t > min_t { max_t - min_t } else { 10.0 }
}

/// Compute axis-angle rotation that aligns +Z to the given direction.
fn rotation_to_align_z(target: Vec3) -> (Vec3, f64) {
    let z = Vec3::new(0.0, 0.0, 1.0);
    let dot = z.dot(target);

    if dot > 0.9999 {
        return (Vec3::new(1.0, 0.0, 0.0), 0.0);
    }
    if dot < -0.9999 {
        return (Vec3::new(1.0, 0.0, 0.0), std::f64::consts::PI);
    }

    let axis = z.cross(target).normalized();
    let angle = dot.acos();
    (axis, angle)
}
