cgar 0.2.0

// SPDX-License-Identifier: MIT
//
// Copyright (c) 2025 Alexandre Severino
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

use std::{array::from_fn, ops::Sub};

use ahash::{AHashMap, AHashSet};
use smallvec::*;

use crate::{
    geometry::{
        Aabb, AabbTree,
        point::{Point, PointOps},
        spatial_element::SpatialElement,
        vector::VectorOps,
    },
    impl_mesh, kernel,
    mesh::{
        basic_types::*, face::Face, half_edge::HalfEdge, intersection_segment::IntersectionSegment,
        topology::FindFaceResult, vertex::Vertex,
    },
    numeric::scalar::Scalar,
};

impl_mesh! {
    pub fn new() -> Self {
        let s = Self {
            vertices: Vec::new(),
            half_edges: Vec::new(),
            faces: Vec::new(),
            edge_map: AHashMap::new(),
            vertex_spatial_hash: AHashMap::new(),
            face_split_map: AHashMap::new(),
            half_edge_split_map: AHashMap::new(),
            cell: 0.0,
            hash_inv: 0.0,
        };

        s.with_default_hash()
    }

    pub fn build_boundary_map(
        &self,
        intersection_segments: &[IntersectionSegment<T, N>],
    ) -> AHashSet<(usize, usize)> {
        fn ordered(a: usize, b: usize) -> (usize, usize) {
            if a < b { (a, b) } else { (b, a) }
        }

        let mut boundary_edges = AHashSet::new();

        for (seg_idx, seg) in intersection_segments.iter().enumerate() {
            if seg[0].resulting_vertex == seg[1].resulting_vertex {
                continue; // degenerate segment
            }
            let he = self
                .edge_map
                .get(&(
                    seg[0].resulting_vertex.unwrap(),
                    seg[1].resulting_vertex.unwrap(),
                ))
                .expect(&format!(
                    "Edge map must contain the segment vertices pair. Segment {}\n{:?}",
                    seg_idx, seg
                ));

            let face0 = self.half_edges[*he]
                .face
                .expect("Half-edge must have a face");
            let twin = self.find_valid_half_edge(self.half_edges[*he].twin, &seg.segment.a);
            let face1 = self.half_edges[twin]
                .face
                .expect("Half-edge must have a face");

            boundary_edges.insert(ordered(face0, face1));
        }

        boundary_edges
    }

    /// - Border half-edges have `face == None` and a valid interior twin `t = twin(b)`.
    /// - For each border `b = u->v`, we set:
    ///       b.next = the next border spoke at vertex v (found by rotating through interior faces)
    ///       and derive b_prev by setting `half_edges[b.next].prev = b`.
    pub fn build_boundary_loops(&mut self) {
        let m = self.half_edges.len();

        // 0) Collect all live border half-edges once
        let mut borders: Vec<usize> = Vec::new();
        borders.reserve(m);
        for i in 0..m {
            let e = &self.half_edges[i];
            if !e.removed && e.face.is_none() {
                borders.push(i);
            }
        }

        // 1) Compute next[b] by rotating around the head of `b` until we hit the next border spoke.
        //    We only traverse interior half-edges' prev/next; we do not read border prev/next here.
        let mut next_of = vec![usize::MAX; m];

        for &b in &borders {
            let t0 = self.half_edges[b].twin; // interior v->u (origin = head(b))
            if t0 >= m || self.half_edges[t0].removed || !self.face_ok(t0) {
                // Degenerate: keep a safe self-loop; will still derive prev from next below
                next_of[b] = b;
                continue;
            }

            // Walk around the head vertex via interior spokes until we encounter a border spoke.
            let mut t = t0;
            let mut steps = 0usize;
            let b_next = loop {
                // CCW around the origin (head of b): twin(prev(t))
                let prev_t = self.half_edges[t].prev;
                let cand   = self.half_edges[prev_t].twin; // this is a spoke leaving the same vertex

                if cand >= m || self.half_edges[cand].removed {
                    // Paranoia: bail to self-loop
                    break b;
                }
                if self.half_edges[cand].face.is_none() {
                    // Found the next BORDER spoke around the head vertex
                    break cand;
                }

                // Keep rotating around the same vertex via the interior side of `cand`
                t = self.half_edges[cand].twin;
                if t >= m || self.half_edges[t].removed || !self.face_ok(t) {
                    // If we lose a valid interior, bail to self-loop
                    break b;
                }

                steps += 1;
                if steps > m {
                    // Safety bound (should be <= valence at the vertex)
                    break b;
                }
            };

            next_of[b] = b_next;
        }

        // 2) Write next and derive prev from next to guarantee reciprocity
        for &b in &borders {
            let nb = next_of[b];
            if nb != usize::MAX {
                self.half_edges[b].next = nb;
            }
        }
        for &b in &borders {
            let nb = self.half_edges[b].next;
            if nb < m {
                self.half_edges[nb].prev = b;
            }
        }

        // 3) Optional sanity checks (enabled in debug builds)
        #[cfg(debug_assertions)]
        {
            for &b in &borders {
                let he = &self.half_edges[b];
                let n = he.next;
                let p = he.prev;
                assert!(n < m && p < m, "boundary next/prev out of range at {}", b);
                assert!(self.half_edges[n].face.is_none(), "b.next must be border at {}", b);
                assert!(self.half_edges[p].face.is_none(), "b.prev must be border at {}", b);
                assert_eq!(self.half_edges[n].prev, b, "boundary next->prev mismatch at {}", b);
                assert_eq!(self.half_edges[p].next, b, "boundary prev->next mismatch at {}", b);
            }
        }
    }

    pub fn build_face_adjacency_graph(&self) -> AHashMap<usize, Vec<usize>> {
        let mut adjacency_graph = AHashMap::new();

        for face_idx in 0..self.faces.len() {
            let mut adjacent_faces = Vec::new();

            // Use iterative approach with safety limits
            if let Some(half_edges) = self.get_face_half_edges_iterative(face_idx) {
                for he_idx in half_edges {
                    if he_idx < self.half_edges.len() {
                        let twin_idx = self.half_edges[he_idx].twin;

                        if twin_idx != usize::MAX && twin_idx < self.half_edges.len() {
                            if let Some(twin_face) = self.half_edges[twin_idx].face {
                                if twin_face != face_idx
                                    && twin_face < self.faces.len()
                                    && self.faces[twin_face].half_edge != usize::MAX
                                {
                                    adjacent_faces.push(twin_face);
                                }
                            }
                        }
                    }
                }
            }

            adjacency_graph.insert(face_idx, adjacent_faces);
        }

        adjacency_graph
    }

    pub fn remove_invalidated_faces(&mut self) {
        // Filter out invalidated faces
        let mut new_faces = Vec::new();
        let mut face_mapping = vec![None; self.faces.len()];

        for (old_idx, face) in self.faces.iter().enumerate() {
            if !face.removed {
                let new_idx = new_faces.len();
                face_mapping[old_idx] = Some(new_idx);
                new_faces.push(face.clone());
            }
        }

        // Update half-edge face references
        for he in &mut self.half_edges {
            if he.removed {
                continue; // Skip removed half-edges
            }
            if let Some(ref mut face_ref) = he.face {
                if let Some(new_face_idx) = face_mapping[*face_ref] {
                    *face_ref = new_face_idx;
                } else {
                    he.face = None; // Face was invalidated
                }
            }
        }

        // Replace faces
        self.faces = new_faces;
    }

    /// Remove duplicate vertices and update all references
    /// Returns the number of vertices removed
    pub fn remove_duplicate_vertices(&mut self) -> usize {
        if self.vertices.is_empty() {
            return 0;
        }

        let initial_count = self.vertices.len();

        // Build spatial hash for efficient duplicate detection
        let mut spatial_groups: AHashMap<u128, Bucket> = AHashMap::new();

        for (vertex_idx, vertex) in self.vertices.iter().enumerate() {
            let hash_key = self.position_to_packed_key(&vertex.position);
            spatial_groups.entry(hash_key).or_default().push(vertex_idx);
        }

        // Find duplicates within each spatial group
        let mut vertex_mapping = (0..self.vertices.len()).collect::<Vec<_>>();
        let mut duplicates = AHashSet::new();

        for group in spatial_groups.values() {
            if group.len() < 2 {
                continue;
            }

            // Check all pairs within the group
            for i in 0..group.len() {
                if duplicates.contains(&group[i]) {
                    continue;
                }

                for j in (i + 1)..group.len() {
                    if duplicates.contains(&group[j]) {
                        continue;
                    }

                    let pos_i = &self.vertices[group[i]].position;
                    let pos_j = &self.vertices[group[j]].position;

                    if kernel::are_equal(pos_i, pos_j) {
                        // Mark j as duplicate of i
                        vertex_mapping[group[j]] = group[i];
                        duplicates.insert(group[j]);
                    }
                }
            }
        }

        if duplicates.is_empty() {
            return 0;
        }

        // Create compacted vertex list and final mapping
        let mut new_vertices = Vec::new();
        let mut old_to_new = vec![usize::MAX; self.vertices.len()];

        for (old_idx, vertex) in self.vertices.iter().enumerate() {
            if !duplicates.contains(&old_idx) {
                let new_idx = new_vertices.len();
                old_to_new[old_idx] = new_idx;
                new_vertices.push(vertex.clone());
            }
        }

        // Update mapping for duplicates to point to their canonical vertex's new index
        for &duplicate_idx in &duplicates {
            let canonical_idx = vertex_mapping[duplicate_idx];
            old_to_new[duplicate_idx] = old_to_new[canonical_idx];
        }

        // Update all half-edge vertex references
        for half_edge in &mut self.half_edges {
            if half_edge.removed {
                continue;
            }

            let old_vertex = half_edge.vertex;
            if old_vertex < old_to_new.len() {
                if let Some(&new_vertex) = old_to_new.get(old_vertex) {
                    if new_vertex != usize::MAX {
                        half_edge.vertex = new_vertex;
                    }
                }
            }
        }

        // Update vertex half-edge references
        for vertex in &mut new_vertices {
            if let Some(he_idx) = vertex.half_edge {
                // Verify the half-edge still exists and is valid
                if he_idx >= self.half_edges.len() || self.half_edges[he_idx].removed {
                    vertex.half_edge = None;
                }
            }
        }

        // Rebuild edge map with new vertex indices
        let mut new_edge_map = AHashMap::new();
        for (&(old_v1, old_v2), &he_idx) in &self.edge_map {
            if old_v1 < old_to_new.len() && old_v2 < old_to_new.len() {
                let new_v1 = old_to_new[old_v1];
                let new_v2 = old_to_new[old_v2];

                if new_v1 != usize::MAX && new_v2 != usize::MAX && new_v1 != new_v2 {
                    // Only keep edges that don't become self-loops
                    new_edge_map.insert((new_v1, new_v2), he_idx);
                }
            }
        }

        // Rebuild spatial hash with new vertex indices
        let mut new_spatial_hash: AHashMap<u128, Bucket> = AHashMap::new();
        for (vertex_idx, vertex) in new_vertices.iter().enumerate() {
            let hash_key = self.position_to_packed_key(&vertex.position);
            new_spatial_hash
                .entry(hash_key)
                .or_default()
                .push(vertex_idx);
        }

        // Update mesh data structures
        self.vertices = new_vertices;
        self.edge_map = new_edge_map;
        self.vertex_spatial_hash = new_spatial_hash;

        // Remove any degenerate faces that may have been created
        // self.remove_degenerate_faces();

        initial_count - self.vertices.len()
    }

    pub fn remove_unused_vertices(&mut self) {
        // 1. Find used vertices
        let mut used = vec![false; self.vertices.len()];
        for face_idx in 0..self.faces.len() {
            // Skip removed faces
            if self.faces[face_idx].removed {
                continue;
            }
            for &v_idx in &self.face_vertices(face_idx) {
                used[v_idx] = true;
            }
        }

        // 2. Build mapping from old to new indices
        let mut old_to_new = vec![None; self.vertices.len()];
        let mut new_vertices = Vec::new();
        for (old_idx, vertex) in self.vertices.iter().enumerate() {
            if used[old_idx] {
                let new_idx = new_vertices.len();
                new_vertices.push(vertex);
                old_to_new[old_idx] = Some(new_idx);
            }
        }

        // 3. Remap faces (skip invalidated ones)
        let mut new_mesh = Mesh::new();
        for v in &new_vertices {
            new_mesh.add_vertex(v.position.clone());
        }
        for face_idx in 0..self.faces.len() {
            // Skip removed faces
            if self.faces[face_idx].removed {
                continue;
            }
            let vs = self.face_vertices(face_idx);
            let mapped: Vec<usize> = vs.iter().map(|&vi| old_to_new[vi].unwrap()).collect();
            if mapped[0] != mapped[1] && mapped[1] != mapped[2] && mapped[2] != mapped[0] {
                new_mesh.add_triangle(mapped[0], mapped[1], mapped[2]);
            }
        }
        *self = new_mesh;
    }

    pub fn build_face_tree(&self) -> AabbTree<T, N, Point<T, N>, usize>
    where
        for<'a> &'a T: Sub<&'a T, Output = T>,
    {
        let face_aabbs = self.internal_build_face_tree();
        AabbTree::build(face_aabbs)
    }

    pub fn build_face_tree_with_lookup(&self) -> (AabbTree<T, N, Point<T, N>, usize>, Vec<Aabb<T, N, Point<T, N>>>)
    where
        for<'a> &'a T: Sub<&'a T, Output = T>,
    {
        let face_aabbs = self.internal_build_face_tree();
        AabbTree::build_with_lookup(face_aabbs)
    }

    fn internal_build_face_tree(&self) -> Vec<(Aabb<T, N, Point<T, N>>, usize)> {
        let mut face_aabbs = Vec::with_capacity(self.faces.len());

        for (face_idx, face) in self.faces.iter().enumerate() {
            let he0 = face.half_edge;

            let he1 = self.half_edges[he0].next;
            let he2 = self.half_edges[he1].next;

            let v0_idx = self.half_edges[he0].vertex;
            let v1_idx = self.half_edges[he1].vertex;
            let v2_idx = self.half_edges[he2].vertex;

            let p0 = &self.vertices[v0_idx].position;
            let p1 = &self.vertices[v1_idx].position;
            let p2 = &self.vertices[v2_idx].position;

            let aabb = if let Some(fast_aabb) = compute_triangle_aabb_fast(p0, p1, p2) {
                fast_aabb
            } else {
                compute_triangle_aabb_exact(p0, p1, p2)
            };

            face_aabbs.push((aabb, face_idx));
        }

        face_aabbs
    }

    /// Compute the AABB of face `f`.
    pub fn face_aabb(&self, f: usize) -> Aabb<T, N, Point<T, N>> {
        let face = &self.faces[f];
        if face.removed || face.half_edge == usize::MAX || self.half_edges[face.half_edge].removed {
            // Return degenerate AABB for invalid faces
            let origin = Point::<T, N>::from_vals(from_fn(|_| T::zero()));
            return Aabb::from_points(&origin, &origin);
        }

        let hes = self.face_half_edges(f);

        // Safety checks
        if hes[1] >= self.half_edges.len() || hes[2] >= self.half_edges.len() {
            let origin = Point::<T, N>::from_vals(from_fn(|_| T::zero()));
            return Aabb::from_points(&origin, &origin);
        }

        let v0_idx = self.half_edges[hes[0]].vertex;
        let v1_idx = self.half_edges[hes[1]].vertex;
        let v2_idx = self.half_edges[hes[2]].vertex;

        // Safety checks for vertex indices
        if v0_idx >= self.vertices.len()
            || v1_idx >= self.vertices.len()
            || v2_idx >= self.vertices.len()
        {
            let origin = Point::<T, N>::from_vals(from_fn(|_| T::zero()));
            return Aabb::from_points(&origin, &origin);
        }

        let p0 = &self.vertices[v0_idx].position;
        let p1 = &self.vertices[v1_idx].position;
        let p2 = &self.vertices[v2_idx].position;

        // Try fast approximate AABB first
        if let Some(aabb) = compute_triangle_aabb_fast(p0, p1, p2) {
            aabb
        } else {
            // Fallback to exact computation only if approximate fails
            compute_triangle_aabb_exact(p0, p1, p2)
        }
    }

    pub fn get_or_insert_vertex(&mut self, pos: &Point<T, N>) -> (usize, bool) {
        let center_key = self.position_to_packed_key(pos);

        // center cell first
        if let Some(bucket) = self.vertex_spatial_hash.get(&center_key) {
            for &vi in bucket {
                if kernel::are_approximately_equal(&self.vertices[vi].position, pos) {
                    return (vi, true);
                }
            }
        }

        // 26 neighbors
        const OFFS: [(i64,i64,i64); 26] = {
            const fn r#gen() -> [(i64,i64,i64); 26] {
                let mut arr = [(0,0,0); 26];
                let mut i = 0;
                let mut dx = -1;
                while dx <= 1 {
                    let mut dy = -1;
                    while dy <= 1 {
                        let mut dz = -1;
                        while dz <= 1 {
                            if !(dx == 0 && dy == 0 && dz == 0) {
                                arr[i] = (dx,dy,dz);
                                i += 1;
                            }
                            dz += 1;
                        }
                        dy += 1;
                    }
                    dx += 1;
                }
                arr
            }
            r#gen()
        };

        let (kx, ky, kz) = self.position_to_hash_key(pos);
        for (dx,dy,dz) in OFFS {
            let key = Self::pack_key3(kx + dx, ky + dy, kz + dz);
            if let Some(bucket) = self.vertex_spatial_hash.get(&key) {
                for &vi in bucket {
                    if kernel::are_equal(&self.vertices[vi].position, pos) {
                        return (vi, true);
                    }
                }
            }
        }

        // insert
        let idx = self.vertices.len();
        self.vertices.push(Vertex::new(pos.clone()));
        self.vertex_spatial_hash.entry(center_key).or_default().push(idx);
        (idx, false)
    }

    pub fn add_vertex(&mut self, position: Point<T, N>) -> usize {
        let idx = self.vertices.len();
        self.vertices.push(Vertex::new(position));

        let key = self.position_to_packed_key(&self.vertices[idx].position);
        self.vertex_spatial_hash.entry(key).or_default().push(idx);

        idx
    }

    /// Flip an interior edge given one of its half‐edges `he`.
    /// Returns Err if `he` is on the boundary (i.e. twin or face is None).
    pub fn flip_edge(&mut self, he_a: usize) -> Result<(), &'static str> {
        // Basic validity
        if he_a >= self.half_edges.len() {
            return Err("flip_edge: half-edge out of range");
        }
        if self.half_edges[he_a].removed {
            return Err("flip_edge: removed");
        }
        let he_b = self.half_edges[he_a].twin;
        if he_b == usize::MAX || he_b >= self.half_edges.len() || self.half_edges[he_b].removed {
            return Err("flip_edge: invalid twin");
        }
        let f0 = match self.half_edges[he_a].face {
            Some(f) => f,
            None => return Err("flip_edge: boundary"),
        };
        let f1 = match self.half_edges[he_b].face {
            Some(f) => f,
            None => return Err("flip_edge: boundary"),
        };

        // Gather local configuration
        let he_ab  = he_a;                 // a->b (to be flipped)
        let he_bc  = self.half_edges[he_ab].next;
        let he_ca  = self.half_edges[he_bc].next;

        let he_ba  = he_b;                 // b->a (twin side)
        let he_ad  = self.half_edges[he_ba].next;
        let he_db  = self.half_edges[he_ad].next;

        // Safety
        for &he in &[he_bc, he_ca, he_ad, he_db] {
            if he >= self.half_edges.len() || self.half_edges[he].removed {
                return Err("flip_edge: corrupted cycle");
            }
        }

        // Vertices (dest/head of each half-edge)
        let b = self.half_edges[he_ab].vertex;
        let c = self.half_edges[he_bc].vertex;
        let a = self.half_edges[he_ca].vertex;
        let d = self.half_edges[he_ad].vertex;
        debug_assert_eq!(a, self.half_edges[he_ba].vertex); // he_ba dest a
        debug_assert_eq!(b, self.half_edges[he_db].vertex);

        if c == d {
            return Err("flip_edge: degenerate (c==d)");
        }

        // Prevent creating duplicate edge (c,d) if already exists
        if let Some(&eidx) = self.edge_map.get(&(c, d)) {
            if eidx != he_ab && eidx != he_ba {
                return Err("flip_edge: duplicate edge (c,d)");
            }
        }
        if let Some(&eidx) = self.edge_map.get(&(d, c)) {
            if eidx != he_ab && eidx != he_ba {
                return Err("flip_edge: duplicate edge (d,c)");
            }
        }

        // Degeneracy / area check (only for 3D)
        #[allow(unused)]
        if N == 3 {
            // Access positions
            let pa = &self.vertices[a].position;
            let pb = &self.vertices[b].position;
            let pc = &self.vertices[c].position;
            let pd = &self.vertices[d].position;

            // (d - a) x (c - a)
            let da0 = &pd[0] - &pa[0]; let da1 = &pd[1] - &pa[1]; let da2 = &pd[2] - &pa[2];
            let ca0 = &pc[0] - &pa[0]; let ca1 = &pc[1] - &pa[1]; let ca2 = &pc[2] - &pa[2];
            let n1x = &da1 * &ca2 - &da2 * &ca1;
            let n1y = &da2 * &ca0 - &da0 * &ca2;
            let n1z = &da0 * &ca1 - &da1 * &ca0;

            // (c - b) x (d - b)
            let cb0 = &pc[0] - &pb[0]; let cb1 = &pc[1] - &pb[1]; let cb2 = &pc[2] - &pb[2];
            let db0 = &pd[0] - &pb[0]; let db1 = &pd[1] - &pb[1]; let db2 = &pd[2] - &pb[2];
            let n2x = &cb1 * &db2 - &cb2 * &db1;
            let n2y = &cb2 * &db0 - &cb0 * &db2;
            let n2z = &cb0 * &db1 - &cb1 * &db0;

            let zero = n1x.clone() - n1x.clone(); // cheap zero
            let tri1_degenerate = n1x == zero && n1y == zero && n1z == zero;
            let tri2_degenerate = n2x == zero && n2y == zero && n2z == zero;
            if tri1_degenerate || tri2_degenerate {
                return Err("flip_edge: degenerate area");
            }
        }

        // Reassign central edge vertices to new diagonal c<->d
        self.half_edges[he_ab].vertex = d; // c->d after rewiring (origin will become c)
        self.half_edges[he_ba].vertex = c; // d->c

        // Face reassignment:
        // New Face f0 (a,d,c): he_ad (a->d), he_ba (d->c), he_ca (c->a)
        // New Face f1 (b,c,d): he_bc (b->c), he_ab (c->d), he_db (d->b)
        self.half_edges[he_ad].face = Some(f0);
        self.half_edges[he_ba].face = Some(f0);
        self.half_edges[he_ca].face = Some(f0);

        self.half_edges[he_bc].face = Some(f1);
        self.half_edges[he_ab].face = Some(f1);
        self.half_edges[he_db].face = Some(f1);

        // Rewire cycles for f0
        self.half_edges[he_ad].next = he_ba;
        self.half_edges[he_ba].next = he_ca;
        self.half_edges[he_ca].next = he_ad;

        self.half_edges[he_ba].prev = he_ad;
        self.half_edges[he_ca].prev = he_ba;
        self.half_edges[he_ad].prev = he_ca;

        // Rewire cycles for f1
        self.half_edges[he_bc].next = he_ab;
        self.half_edges[he_ab].next = he_db;
        self.half_edges[he_db].next = he_bc;

        self.half_edges[he_ab].prev = he_bc;
        self.half_edges[he_db].prev = he_ab;
        self.half_edges[he_bc].prev = he_db;

        // Update representative half-edges for faces
        self.faces[f0].half_edge = he_ad;
        self.faces[f1].half_edge = he_bc;

        // Update edge map
        self.edge_map.remove(&(a, b));
        self.edge_map.remove(&(b, a));
        self.edge_map.insert((c, d), he_ab);
        self.edge_map.insert((d, c), he_ba);

        // Update vertex half_edge pointers if they pointed to obsolete direction
        if self.vertices[a].half_edge == Some(he_ab) { self.vertices[a].half_edge = Some(he_ad); }
        if self.vertices[b].half_edge == Some(he_ba) { self.vertices[b].half_edge = Some(he_bc); }
        // For completeness ensure c,d have some outgoing
        if self.vertices[c].half_edge.is_none() { self.vertices[c].half_edge = Some(he_ca); }
        if self.vertices[d].half_edge.is_none() { self.vertices[d].half_edge = Some(he_db); }

        Ok(())
    }

    pub fn split_edge(
        &mut self,
        aabb_tree: &mut AabbTree<T, N, Point<T, N>, usize>,
        he: usize,
        pos: &Point<T, N>,
        update_tree: bool,
    ) -> Result<SplitResult, &'static str>
    {
        let he_ca = self.find_valid_half_edge(he, pos);
        let he_ab = self.half_edges[he_ca].next;
        let he_bc = self.half_edges[he_ab].next;
        let he_ac = self.half_edges[he_ca].twin;

        let a = self.half_edges[he_ca].vertex;
        let b = self.half_edges[he_ab].vertex;
        let c = self.half_edges[he_bc].vertex;

        let (w, reuse) = self.get_or_insert_vertex(pos);
        if reuse {
            return Ok(SplitResult {
                kind: SplitResultKind::NoSplit,
                vertex: w,
                new_faces: [usize::MAX; 4]
            });
        }

        let original_face_1 = self.half_edges[he_ca].face.unwrap();
        let original_face_2 = self.half_edges[he_ac].face;

        if original_face_2.is_none() {
            // Open borders scenario
            let ex_he_ba = self.half_edges[he_ab].twin; // b->a
            let ex_he_cb = self.half_edges[he_bc].twin; // c->b

            // Mark old faces and half-edges removed
            self.faces[original_face_1].removed = true;
            self.half_edges[he_ca].removed = true;
            self.half_edges[he_ac].removed = true;

            // Remove old edge (a,c) / (c,a) from edge_map
            self.edge_map.remove(&(a, c));
            self.edge_map.remove(&(c, a));

            // Base indices
            let base_face_idx = self.faces.len(); // two new real faces
            let base_he_idx = self.half_edges.len(); // we will add 8 new half-edges:
            // interior: c->w (0), w->a (1), b->w (2), w->b (3)
            // border:   w->c (4), a->w (5), (two border twins of above)
            //           plus their self-loop next/prev initialization
            //           Actually we need exactly 6 new half-edges:
            //             interior: c->w, w->a, b->w, w->b (4)
            //             border:   w->c, a->w (2)  => total 6
            // (We do NOT add border twins for b-w; that is internal)

            // Create interior half-edges first (face assignment deferred)
            let he_cw = base_he_idx + 0; // c -> w (face 2 later)
            let he_wa = base_he_idx + 1; // w -> a (face 1 later)
            let he_bw = base_he_idx + 2; // b -> w (face 1)
            let he_wb = base_he_idx + 3; // w -> b (face 2)
            let bhe_wc = base_he_idx + 4; // w -> c (border null face)
            let bhe_aw = base_he_idx + 5; // a -> w (border null face)

            // Helper to push a blank half-edge
            let mut push_he = |to: usize| {
                let mut he = HalfEdge::new(to);
                he.face = None;
                self.half_edges.push(he);
            };

            // Interior edges
            push_he(w); // c->w
            push_he(a); // w->a
            push_he(w); // b->w
            push_he(b); // w->b
            // Border twins
            push_he(c); // w->c
            push_he(w); // a->w

            // Set origins implicitly via prev pointers later; ensure vertex field is 'to' (already)
            // Assign twins
            self.half_edges[he_cw].twin = bhe_wc;
            self.half_edges[bhe_wc].twin = he_cw;
            self.half_edges[he_wa].twin = bhe_aw;
            self.half_edges[bhe_aw].twin = he_wa;
            self.half_edges[he_bw].twin = he_wb;
            self.half_edges[he_wb].twin = he_bw;

            // Build two new real faces:
            // Face F1: (a,b,w)   cycle: he_ab (a->b), he_bw (b->w), he_wa (w->a)
            // Face F2: (w,b,c)   cycle: he_wb (w->b), he_bc (b->c), he_cw (c->w)

            // Insert faces
            self.faces.push(Face::new(he_ab)); // face index = base_face_idx (F1)
            self.faces.push(Face::new(he_wb)); // face index = base_face_idx+1 (F2)

            // Wire F1 half-edges
            self.half_edges[he_ab].face = Some(base_face_idx);
            self.half_edges[he_bw].face = Some(base_face_idx);
            self.half_edges[he_wa].face = Some(base_face_idx);

            self.half_edges[he_ab].next = he_bw;
            self.half_edges[he_ab].prev = he_wa;
            self.half_edges[he_bw].next = he_wa;
            self.half_edges[he_bw].prev = he_ab;
            self.half_edges[he_wa].next = he_ab;
            self.half_edges[he_wa].prev = he_bw;

            // Wire F2 half-edges
            self.half_edges[he_wb].face = Some(base_face_idx + 1);
            self.half_edges[he_bc].face = Some(base_face_idx + 1);
            self.half_edges[he_cw].face = Some(base_face_idx + 1);

            self.half_edges[he_wb].next = he_bc;
            self.half_edges[he_wb].prev = he_cw;
            self.half_edges[he_bc].next = he_cw;
            self.half_edges[he_bc].prev = he_wb;
            self.half_edges[he_cw].next = he_wb;
            self.half_edges[he_cw].prev = he_bc;

            // Re-hook external twins (keep adjacency) on edges a-b, b-c
            // (he_ab twin already points to ex_he_ba, he_bc twin to ex_he_cb)
            // Ensure those external twins still point back
            if ex_he_ba != usize::MAX {
                self.half_edges[ex_he_ba].twin = he_ab;
            }
            if ex_he_cb != usize::MAX {
                self.half_edges[ex_he_cb].twin = he_bc;
            }

            // Create two new null faces for border half-edges bhe_wc (w->c) and bhe_aw (a->w)
            self.half_edges[bhe_wc].face = None;
            // self-loop
            self.half_edges[bhe_wc].next = bhe_wc;
            self.half_edges[bhe_wc].prev = bhe_wc;

            self.half_edges[bhe_aw].face = None;
            self.half_edges[bhe_aw].next = bhe_aw;
            self.half_edges[bhe_aw].prev = bhe_aw;

            // Edge map updates
            self.edge_map.insert((c, w), he_cw);
            self.edge_map.insert((w, a), he_wa);
            self.edge_map.insert((b, w), he_bw);
            self.edge_map.insert((w, b), he_wb);
            self.edge_map.insert((w, c), bhe_wc);
            self.edge_map.insert((a, w), bhe_aw);

            // Update representative half-edges at vertices
            self.vertices[w].half_edge.get_or_insert(he_wb);
            self.vertices[a].half_edge.get_or_insert(he_ab);
            self.vertices[b].half_edge.get_or_insert(he_bc);
            self.vertices[c].half_edge.get_or_insert(he_cw);

            // Record half-edge split lineage
            self.half_edge_split_map.insert(he_ca, (he_cw, he_wa));
            self.half_edge_split_map.insert(he_ac, (bhe_aw, bhe_wc));

            if update_tree {
                // AABB tree updates (invalidate old real face; null face not in tree originally)
                aabb_tree.invalidate(&original_face_1);
                // Insert the two new real faces
                let f1_aabb = self.face_aabb(base_face_idx);
                let f2_aabb = self.face_aabb(base_face_idx + 1);
                aabb_tree.insert(f1_aabb, base_face_idx);
                aabb_tree.insert(f2_aabb, base_face_idx + 1);
            }

            let split_result = SplitResult {
                kind: SplitResultKind::SplitEdge,
                vertex: w,
                new_faces: [base_face_idx, base_face_idx + 1, usize::MAX, usize::MAX],
            };

            return Ok(split_result);
        }

        let original_face_2 = original_face_2.unwrap();

        let he_cd = self.half_edges[he_ac].next;
        let he_da = self.half_edges[he_cd].next;

        let ex_he_ba = self.half_edges[he_ab].twin;
        let ex_he_cb = self.half_edges[he_bc].twin;
        let ex_he_dc = self.half_edges[he_cd].twin;
        let ex_he_ad = self.half_edges[he_da].twin;

        let d = self.half_edges[he_cd].vertex;

        // 1. Build the new faces (4 in total) and create their half-edges (no twins for now)
        let evs = [
            (w, b), // face wbc
            (c, w),
            (w, a), // face wab
            (b, w),
            (w, c), // face wcd
            (d, w),
            (w, d), // face wda
            (a, w),
        ];

        let existing_evs = [he_bc, he_ab, he_cd, he_da];

        let base_face_idx = self.faces.len();

        let base_half_edge_idx = self.half_edges.len();

        // let he_wb = base_half_edge_idx;
        let he_cw = base_half_edge_idx + 1;
        let he_wa = base_half_edge_idx + 2;
        // let he_bw = base_half_edge_idx + 3;
        let he_wc = base_half_edge_idx + 4;
        // let he_dw = base_half_edge_idx + 5;
        // let he_wd = base_half_edge_idx + 6;
        let he_aw = base_half_edge_idx + 7;

        let mut i = 0;
        for (from, to) in evs {
            let mut he = HalfEdge::new(to);
            he.face = Some(base_face_idx + i / 2);
            he.vertex = to;
            self.half_edges.push(he);
            self.edge_map.insert((from, to), base_half_edge_idx + i);
            i += 1;
        }

        for i in 0..4 {
            let base_he_idx = base_half_edge_idx + i * 2;
            let edge_indices = [base_he_idx, existing_evs[i], base_he_idx + 1];
            self.half_edges[edge_indices[0]].next = edge_indices[1];
            self.half_edges[edge_indices[0]].prev = edge_indices[2];
            self.half_edges[edge_indices[1]].next = edge_indices[2];
            self.half_edges[edge_indices[1]].prev = edge_indices[0];
            self.half_edges[edge_indices[2]].next = edge_indices[0];
            self.half_edges[edge_indices[2]].prev = edge_indices[1];

            self.faces.push(Face::new(edge_indices[0]));
            self.faces[base_face_idx + i].half_edge = edge_indices[0];
        }

        let twins = [
            (0, 3), // w -> b | b -> w
            (1, 4), // c -> w | w -> c
            (2, 7), // w -> a | a -> w
            (5, 6), // d -> w | w -> d
        ];

        for i in 0..4 {
            self.half_edges[base_half_edge_idx + twins[i].0].twin = base_half_edge_idx + twins[i].1;
            self.half_edges[base_half_edge_idx + twins[i].1].twin = base_half_edge_idx + twins[i].0;
        }

        // Update vertices half-edges
        self.vertices[w].half_edge = Some(base_half_edge_idx); // w -> b
        self.vertices[a].half_edge = Some(he_ab); // a -> b
        self.vertices[b].half_edge = Some(he_bc); // b -> c
        self.vertices[c].half_edge = Some(he_cd); // c -> d
        self.vertices[d].half_edge = Some(he_da); // d -> a

        // Remove old edges from edge map
        self.edge_map.remove(&(c, a));
        self.edge_map.remove(&(a, c));

        // External twins (unchanged neighboring faces)
        self.half_edges[ex_he_ba].twin = he_ab; // external b->a | a->b
        self.half_edges[he_ab].twin = ex_he_ba;

        self.half_edges[ex_he_cb].twin = he_bc; // external c->b | b->c
        self.half_edges[he_bc].twin = ex_he_cb;

        self.half_edges[ex_he_dc].twin = he_cd; // external d->c | c->d
        self.half_edges[he_cd].twin = ex_he_dc;

        self.half_edges[ex_he_ad].twin = he_da; // external a->d | d->a
        self.half_edges[he_da].twin = ex_he_ad;

        let triangle_wbc = FaceInfo {
            face_idx: base_face_idx,
            vertices: Triangle(w, b, c),
        };
        let triangle_wab = FaceInfo {
            face_idx: base_face_idx + 1,
            vertices: Triangle(w, a, b),
        };
        let triangle_wcd = FaceInfo {
            face_idx: base_face_idx + 2,
            vertices: Triangle(w, c, d),
        };
        let triangle_wda = FaceInfo {
            face_idx: base_face_idx + 3,
            vertices: Triangle(w, d, a),
        };

        // Mark old faces as removed
        self.faces[original_face_1].removed = true;
        self.faces[original_face_2].removed = true;

        // Mark old half-edges as removed
        self.half_edges[he_ca].removed = true;
        self.half_edges[he_ac].removed = true;

        self.half_edges[he_bc].face = Some(base_face_idx);
        self.half_edges[he_ab].face = Some(base_face_idx + 1);
        self.half_edges[he_cd].face = Some(base_face_idx + 2);
        self.half_edges[he_da].face = Some(base_face_idx + 3);

        // Connect the new half-edges to their respective vertices
        self.half_edges[he_bc].vertex = c; // b -> c
        self.half_edges[he_ab].vertex = b; // a -> b
        self.half_edges[he_cd].vertex = d; // c -> d
        self.half_edges[he_da].vertex = a; // d -> a

        self.half_edge_split_map.insert(he_ca, (he_cw, he_wa));
        self.half_edge_split_map.insert(he_ac, (he_aw, he_wc));

        let face_split = FaceSplitMap {
            face: original_face_1,
            new_faces: smallvec![triangle_wbc, triangle_wab],
        };
        self.face_split_map.insert(original_face_1, face_split);

        let face_split = FaceSplitMap {
            face: original_face_2,
            new_faces: smallvec![triangle_wcd, triangle_wda],
        };
        self.face_split_map.insert(original_face_2, face_split);

        let split_result = SplitResult {
            kind: SplitResultKind::SplitEdge,
            vertex: w,
            new_faces: [
                base_face_idx,
                base_face_idx + 1,
                base_face_idx + 2,
                base_face_idx + 3,
            ],
        };

        if update_tree {
            for &new_face_idx in &split_result.new_faces {
                let face_aabb = self.face_aabb(new_face_idx);
                aabb_tree.insert(face_aabb, new_face_idx);
            }

            aabb_tree.invalidate(&original_face_1);
            aabb_tree.invalidate(&original_face_2);
        }

        Ok(split_result)
    }

    pub fn split_face(
        &mut self,
        aabb_tree: &mut AabbTree<T, N, Point<T, N>, usize>,
        mut face: usize,
        p: &Point<T, N>,
        update_tree: bool,
    ) -> Result<SplitResult, &'static str>
    {
        match self.find_valid_face(face, p) {
            FindFaceResult::OnEdge { he, .. } => {
                return self.split_edge(aabb_tree, he, &p, update_tree);
            },
            FindFaceResult::OnVertex { v, .. } => {
                return Ok(SplitResult {
                    kind: SplitResultKind::NoSplit,
                    vertex: v,
                    new_faces: [usize::MAX; 4],
                });
            },
            FindFaceResult::Inside { f, .. } => {
                face = f;
            },
        }

        if self.faces[face].removed {
            panic!("Cannot find or insert vertex on a removed face");
        }

        let he_ab = self.faces[face].half_edge;
        let he_bc = self.half_edges[he_ab].next;
        let he_ca = self.half_edges[he_bc].next;

        let w = self.add_vertex(p.clone());

        let a = self.half_edges[he_ca].vertex;
        let b = self.half_edges[he_ab].vertex;
        let c = self.half_edges[he_bc].vertex;

        let subface_1_verts = [a, w, c];
        let subface_2_verts = [c, w, b];
        let subface_3_verts = [b, w, a];

        let subface_1_idx = self.faces.len();
        let subface_2_idx = self.faces.len() + 1;
        let subface_3_idx = self.faces.len() + 2;

        // Create subface 1
        let base_he_idx_1 = self.half_edges.len();
        let edge_vertices_1 = [
            (subface_1_verts[0], subface_1_verts[1]), // a -> w
            (subface_1_verts[1], subface_1_verts[2]), // w -> c
                                                      // c -> a
        ];

        // Create half-edges for face 1
        for (_i, &(_from, to)) in edge_vertices_1.iter().enumerate() {
            let mut he = HalfEdge::new(to);
            he.face = Some(subface_1_idx);
            self.half_edges.push(he);
        }

        // Link face 1 half-edges
        let edge_indices_1 = [base_he_idx_1, base_he_idx_1 + 1, he_ca];
        self.half_edges[edge_indices_1[0]].next = edge_indices_1[1];
        self.half_edges[edge_indices_1[0]].prev = edge_indices_1[2];
        self.half_edges[edge_indices_1[1]].next = edge_indices_1[2];
        self.half_edges[edge_indices_1[1]].prev = edge_indices_1[0];
        self.half_edges[edge_indices_1[2]].next = edge_indices_1[0];
        self.half_edges[edge_indices_1[2]].prev = edge_indices_1[1];

        // Create face 2
        let base_he_idx_2 = self.half_edges.len();
        let edge_vertices_2 = [
            (subface_2_verts[0], subface_2_verts[1]), // c -> w
            (subface_2_verts[1], subface_2_verts[2]), // w -> b
                                                      // b -> c
        ];

        // Create half-edges for face 2
        for (_i, &(_from, to)) in edge_vertices_2.iter().enumerate() {
            let mut he = HalfEdge::new(to);
            he.face = Some(subface_2_idx);
            self.half_edges.push(he);
        }

        // Link face 2 half-edges
        let edge_indices_2 = [base_he_idx_2, base_he_idx_2 + 1, he_bc];
        self.half_edges[edge_indices_2[0]].next = edge_indices_2[1];
        self.half_edges[edge_indices_2[0]].prev = edge_indices_2[2];
        self.half_edges[edge_indices_2[1]].next = edge_indices_2[2];
        self.half_edges[edge_indices_2[1]].prev = edge_indices_2[0];
        self.half_edges[edge_indices_2[2]].next = edge_indices_2[0];
        self.half_edges[edge_indices_2[2]].prev = edge_indices_2[1];

        // Create face 3
        let base_he_idx_3 = self.half_edges.len();
        let edge_vertices_3 = [
            (subface_3_verts[0], subface_3_verts[1]), // b -> w
            (subface_3_verts[1], subface_3_verts[2]), // w -> a
                                                      // a -> b
        ];

        // Create half-edges for face 3
        for (_i, &(_from, to)) in edge_vertices_3.iter().enumerate() {
            let mut he = HalfEdge::new(to);
            he.face = Some(subface_3_idx);
            self.half_edges.push(he);
        }

        // Link face 3 half-edges
        let edge_indices_3 = [base_he_idx_3, base_he_idx_3 + 1, he_ab];
        self.half_edges[edge_indices_3[0]].next = edge_indices_3[1];
        self.half_edges[edge_indices_3[0]].prev = edge_indices_3[2];
        self.half_edges[edge_indices_3[1]].next = edge_indices_3[2];
        self.half_edges[edge_indices_3[1]].prev = edge_indices_3[0];
        self.half_edges[edge_indices_3[2]].next = edge_indices_3[0];
        self.half_edges[edge_indices_3[2]].prev = edge_indices_3[1];

        self.faces.push(Face::new(edge_indices_1[0]));
        self.faces.push(Face::new(edge_indices_2[0]));
        self.faces.push(Face::new(edge_indices_3[0]));

        self.faces[face].removed = true;

        // self.half_edges[base_he_idx_1].face = Some(subface_1_idx);
        // self.half_edges[base_he_idx_1 + 1].face = Some(subface_1_idx);
        self.half_edges[he_ca].face = Some(subface_1_idx);

        // self.half_edges[base_he_idx_2].face = Some(subface_2_idx);
        // self.half_edges[base_he_idx_2 + 1].face = Some(subface_2_idx);
        self.half_edges[he_bc].face = Some(subface_2_idx);

        // self.half_edges[base_he_idx_3].face = Some(subface_3_idx);
        // self.half_edges[base_he_idx_3 + 1].face = Some(subface_3_idx);
        self.half_edges[he_ab].face = Some(subface_3_idx);

        self.vertices[w].half_edge = Some(base_he_idx_2 + 1); // w -> b
        self.vertices[a].half_edge = Some(he_ab);
        self.vertices[b].half_edge = Some(he_bc);
        self.vertices[c].half_edge = Some(he_ca);

        self.half_edges[base_he_idx_1].vertex = w; // a -> w
        self.half_edges[base_he_idx_1 + 1].vertex = c; // w -> c

        self.half_edges[base_he_idx_2].vertex = w; // c -> w
        self.half_edges[base_he_idx_2 + 1].vertex = b; // w -> b

        self.half_edges[base_he_idx_3].vertex = w; // b -> w
        self.half_edges[base_he_idx_3 + 1].vertex = a; // w -> a

        self.faces[subface_1_idx].half_edge = base_he_idx_1;
        self.faces[subface_2_idx].half_edge = base_he_idx_2;
        self.faces[subface_3_idx].half_edge = base_he_idx_3;

        // internal twins
        // w -> a and a -> w, respectively
        self.half_edges[base_he_idx_3 + 1].twin = base_he_idx_1;
        self.half_edges[base_he_idx_1].twin = base_he_idx_3 + 1;

        // w -> c and c -> w, respectively
        self.half_edges[base_he_idx_1 + 1].twin = base_he_idx_2;
        self.half_edges[base_he_idx_2].twin = base_he_idx_1 + 1;

        // w -> b and b -> w, respectively
        self.half_edges[base_he_idx_2 + 1].twin = base_he_idx_3;
        self.half_edges[base_he_idx_3].twin = base_he_idx_2 + 1;

        self.edge_map.insert((a, w), base_he_idx_1);
        self.edge_map.insert((w, c), base_he_idx_1 + 1);

        self.edge_map.insert((c, w), base_he_idx_2);
        self.edge_map.insert((w, b), base_he_idx_2 + 1);

        self.edge_map.insert((b, w), base_he_idx_3);
        self.edge_map.insert((w, a), base_he_idx_3 + 1);

        let triangle_awc = FaceInfo {
            face_idx: subface_1_idx,
            vertices: Triangle(a, w, c),
        };
        let triangle_cwb = FaceInfo {
            face_idx: subface_2_idx,
            vertices: Triangle(c, w, b),
        };
        let triangle_bwa = FaceInfo {
            face_idx: subface_3_idx,
            vertices: Triangle(b, w, a),
        };

        let face_split = FaceSplitMap {
            face: face,
            new_faces: smallvec![triangle_awc, triangle_cwb, triangle_bwa],
        };
        self.face_split_map.insert(face, face_split);

        let split_result = SplitResult {
            kind: SplitResultKind::SplitFace,
            vertex: w,
            new_faces: [subface_1_idx, subface_2_idx, subface_3_idx, usize::MAX],
        };

        if update_tree {
            for i in 0..3 {
                let face_aabb = self.face_aabb(split_result.new_faces[i]);
                aabb_tree.insert(face_aabb, split_result.new_faces[i]);
            }

            aabb_tree.invalidate(&face);
        }


        Ok(split_result)
    }

    /// Tangential (Taubin/Desbrun style) smoothing.
    /// - `sweeps`: number of Jacobi sweeps
    /// - `step`:   blend factor in (0,1], e.g. 0.2
    /// Returns true if any vertex moved.
    pub fn smooth_tangential(&mut self, v: usize, alpha: T, max_edge_length_sq: &T) -> bool
    where
        T: Scalar + PartialOrd + Clone,
        for<'a> &'a T:
            core::ops::Add<&'a T, Output = T> +
            core::ops::Sub<&'a T, Output = T> +
            core::ops::Mul<&'a T, Output = T> +
            core::ops::Div<&'a T, Output = T>,
    {
        if N != 3 || v >= self.vertices.len() || self.vertices[v].removed {
            return false;
        }

        // Single ring traversal - cache boundary check and neighbor data
        let ring_hes = self.vertex_ring_ccw(v);
        if ring_hes.neighbors_ccw.is_empty() {
            return false;
        }

        // Combined boundary check and neighbor collection
        let mut is_boundary = false;
        let mut valid_neighbors = Vec::with_capacity(ring_hes.halfedges_ccw.len());

        for &he in &ring_hes.halfedges_ccw {
            if he >= self.half_edges.len() || self.half_edges[he].removed {
                continue;
            }

            if self.half_edges[he].face.is_none() {
                is_boundary = true;
            }

            let b_idx = self.half_edges[he].vertex;
            if b_idx < self.vertices.len() && !self.vertices[b_idx].removed {
                valid_neighbors.push((he, b_idx));
            }
        }

        if is_boundary || valid_neighbors.len() < 2 {
            return false;
        }

        // Current position (avoid clones in loop)
        let a = &self.vertices[v].position;
        let a0 = &a[0];
        let a1 = &a[1];
        let a2 = &a[2];

        // Accumulate using references to avoid clones
        let mut n0 = T::zero();
        let mut n1 = T::zero();
        let mut n2 = T::zero();
        let mut d0 = T::zero();
        let mut d1 = T::zero();
        let mut d2 = T::zero();

        for &(he, b_idx) in &valid_neighbors {
            let b = &self.vertices[b_idx].position;

            // Laplacian term (b - a)
            d0 = &d0 + &(&b[0] - a0);
            d1 = &d1 + &(&b[1] - a1);
            d2 = &d2 + &(&b[2] - a2);

            // Face normal if valid face
            if self.half_edges[he].face.is_some() {
                let he_next = self.half_edges[he].next;
                if he_next < self.half_edges.len() && !self.half_edges[he_next].removed {
                    let c_idx = self.half_edges[he_next].vertex;
                    if c_idx < self.vertices.len() && !self.vertices[c_idx].removed {
                        let c = &self.vertices[c_idx].position;

                        // u = b - a, v = c - a, cross product u x v
                        let ux = &b[0] - a0;
                        let uy = &b[1] - a1;
                        let uz = &b[2] - a2;
                        let vx = &c[0] - a0;
                        let vy = &c[1] - a1;
                        let vz = &c[2] - a2;

                        n0 = &n0 + &(&(&uy * &vz) - &(&uz * &vy));
                        n1 = &n1 + &(&(&uz * &vx) - &(&ux * &vz));
                        n2 = &n2 + &(&(&ux * &vy) - &(&uy * &vx));
                    }
                }
            }
        }

        // Normalize and project to tangent plane
        let inv_val = &T::one() / &T::from(valid_neighbors.len() as f64);
        d0 = &d0 * &inv_val;
        d1 = &d1 * &inv_val;
        d2 = &d2 * &inv_val;

        let nn = &n0 * &n0 + &n1 * &n1 + &n2 * &n2;
        let mut t0 = d0;
        let mut t1 = d1;
        let mut t2 = d2;

        if nn > T::zero() {
            let dn = &t0 * &n0 + &t1 * &n1 + &t2 * &n2;
            let k = &dn / &nn;
            t0 = &t0 - &(&n0 * &k);
            t1 = &t1 - &(&n1 * &k);
            t2 = &t2 - &(&n2 * &k);
        }

        // New position
        let axp = a0 + &(&alpha * &t0);
        let ayp = a1 + &(&alpha * &t1);
        let azp = a2 + &(&alpha * &t2);

        // Check if new position would create edges outside acceptable range
        let new_pos = Point::<T, N>::from_vals(from_fn(|i| match i {
            0 => axp.clone(),
            1 => ayp.clone(),
            2 => azp.clone(),
            _ => T::zero(),
        }));

        for &(_he, b_idx) in &valid_neighbors {
            let b = &self.vertices[b_idx].position;
            let edge_vec = &new_pos - &b;
            let len_sq = edge_vec.as_vector().norm2();

            // Use a slightly relaxed threshold (e.g., 1.5x instead of exact max)
            // to avoid preventing all smoothing

            if &len_sq > max_edge_length_sq {
                return false;  // Reject move
            }
        }

        // Fast rejection using ball approximation for degeneracy check
        // ...existing degeneracy check...

        // Commit new position
        self.vertices[v].position[0] = axp;
        self.vertices[v].position[1] = ayp;
        self.vertices[v].position[2] = azp;

        true
    }
}

#[inline(always)]
fn min3_ref<'a, T: Scalar>(a: &'a T, b: &'a T, c: &'a T) -> &'a T
where
    for<'x> &'x T: Sub<&'x T, Output = T>,
{
    // pick min by sign of differences (LazyExact uses Ball fast-path here)
    let ab = if (a - b).is_negative() { a } else { b };
    if (c - ab).is_negative() { c } else { ab }
}

#[inline(always)]
fn max3_ref<'a, T: Scalar>(a: &'a T, b: &'a T, c: &'a T) -> &'a T
where
    for<'x> &'x T: Sub<&'x T, Output = T>,
{
    let ab = if (a - b).is_positive() { a } else { b };
    if (c - ab).is_positive() { c } else { ab }
}

/// Fast approximate triangle AABB using double intervals
pub fn compute_triangle_aabb_fast<T: Scalar, const N: usize>(
    p0: &Point<T, N>,
    p1: &Point<T, N>,
    p2: &Point<T, N>,
) -> Option<Aabb<T, N, Point<T, N>>>
where
    T: From<f64>,
{
    // Single allocation for all coordinates
    let mut coords = [[0.0; N]; 3];

    // Batch extract all coordinates
    for i in 0..N {
        coords[0][i] = p0[i].as_f64_fast()?;
        coords[1][i] = p1[i].as_f64_fast()?;
        coords[2][i] = p2[i].as_f64_fast()?;
    }

    // Vectorized min/max computation
    let min_coords =
        std::array::from_fn(|i| T::from(coords[0][i].min(coords[1][i]).min(coords[2][i])));

    let max_coords =
        std::array::from_fn(|i| T::from(coords[0][i].max(coords[1][i]).max(coords[2][i])));

    Some(Aabb::new(
        Point::<T, N>::from_vals(min_coords),
        Point::<T, N>::from_vals(max_coords),
    ))
}

/// Exact triangle AABB computation (fallback)
pub fn compute_triangle_aabb_exact<T: Scalar, const N: usize>(
    p0: &Point<T, N>,
    p1: &Point<T, N>,
    p2: &Point<T, N>,
) -> Aabb<T, N, Point<T, N>>
where
    for<'a> &'a T: Sub<&'a T, Output = T>,
{
    let mins = std::array::from_fn(|i| {
        let m = min3_ref(&p0[i], &p1[i], &p2[i]);
        m.clone()
    });
    let maxs = std::array::from_fn(|i| {
        let m = max3_ref(&p0[i], &p1[i], &p2[i]);
        m.clone()
    });

    Aabb::from_points(
        &Point::<T, N>::from_vals(mins),
        &Point::<T, N>::from_vals(maxs),
    )
}