remesh 0.0.5

// SPDX-License-Identifier: MIT OR Apache-2.0
// Copyright (c) 2025 lacklustr@protonmail.com https://github.com/eadf

use super::super::IsotropicRemeshAlgo;
use crate::isotropic_remesh::collapse_edges::{CollapseCandidate, CollapseData};

use crate::common::VertexIndex;
use crate::common::macros::{integrity_assert, integrity_assert_eq, integrity_println};
use crate::corner_table::{TriangleIndex, VertexFan};
#[allow(unused_imports)]
use rayon::prelude::*;
use std::fmt::Debug;
use vector_traits::num_traits::AsPrimitive;
use vector_traits::prelude::SimdUpgradable;

impl<S, V, const ENABLE_UNSAFE: bool> IsotropicRemeshAlgo<S, V, ENABLE_UNSAFE>
where
    S: crate::common::sealed::ScalarType,
    f64: AsPrimitive<S>,
    V: Debug + Copy + From<[S; 3]> + Into<[S; 3]> + Sync + 'static,
{
    pub(crate) fn collapse_edges_qem(&mut self, collapse_threshold_sq: S) -> bool {
        let original_num_triangles = self.corner_table.active_triangles();
        self.dirty_vertices.prepare(self.vertices.len());

        integrity_println!(
            "####### collapse_edges_qem threshold:{}",
            collapse_threshold_sq.sqrt()
        );
        integrity_println!("####    collapse_edges_qem: building candidate list");

        //#[cfg(feature = "integrity_check")]
        //self.dump_status("pre collapse_short_edges()");

        // 1. Identify all short edges
        let mut edges_to_collapse = self.identify_collapse_candidates_qem(collapse_threshold_sq);

        // 2. Sort edges by quality improvement for better collapse order
        edges_to_collapse.sort_unstable_by(|a, b| {
            (b.1, b.2)
                .partial_cmp(&(a.1, a.2))
                .unwrap_or(std::cmp::Ordering::Equal)
        });

        //integrity_println!("edges_to_collapse: {:?}", edges_to_collapse);
        while let Some((candidate, _cost, ..)) = edges_to_collapse.pop() {
            /*if true {
                let vic = self.corner_table.get_vertex_of_corner(candidate.c0p);
                let vicn = self
                    .corner_table
                    .get_vertex_of_corner(self.corner_table.next(candidate.c0p));

                if (vic.0 == 1 || vicn.0 == 1) && (vic.0 == 4 || vicn.0 == 4) {
                    integrity_println!(
                        "#### hello debugger! evaluate candidate corner:{:?} twin:{:?}",
                        self.dbg_corner(candidate.c0p),
                        self.dbg_corner(self.corner_table.twin(candidate.c0p))
                    );
                }
            }*/

            if self.corner_table.is_corner_deleted(candidate.c0p) {
                integrity_println!(
                    "Skipping : {candidate:?} as corner {:?} is already deleted",
                    candidate.c0p
                );
                continue;
            }

            let c0p_fan = self.corner_table.ccw_vertex_fan(candidate.c0p);
            let twin_fan = self
                .corner_table
                .ccw_vertex_fan(self.corner_table.twin(candidate.c0p));

            let Some(candidate) =
                self.evaluate_collapse_candidate_qem::<false>(candidate, &c0p_fan, &twin_fan)
            else {
                continue;
            };

            // TODO: move this vertex dirty evaluation to collapse_edge evaluation
            let vc = self.corner_table.vertex(candidate.c0p);
            let vct = self.corner_table.vertex(twin_fan[0]);

            integrity_println!(
                "collapsing: {candidate:?} c0p:{:?} cost:{_cost:?} v0:{vc:?} vR:{vct:?}",
                self.dbg_corner(candidate.c0p)
            );
            self.collapse_edge_candidate(candidate);
            self.dirty_vertices.mark_dirty(vc);
            self.dirty_vertices.mark_dirty(vct);

            #[cfg(feature = "integrity_check")]
            self.check_mesh_integrity(
                format!("after manifold edge collapse {:?}-{:?}", vc, vct).as_str(),
            )
            .unwrap();
        }

        original_num_triangles != self.corner_table.active_triangles()
    }

    /// creates a list of (CornerId,S) where S is the length squared between V(c)->V(c.next)
    fn identify_collapse_candidates_qem(
        &self,
        threshold_sq: S,
    ) -> Vec<(CollapseCandidate<S>, f64, u64)> {
        //let vertex_quadrics_nalgebra = self.build_quadratics_faer();
        let vertex_quadrics_faer = self.build_qem_quadratics_faer();

        //println!("quadrics:n: {:?}", vertex_quadrics_nalgebra);
        //println!("quadrics:f: {:?}", vertex_quadrics_faer);

        // Step 1: Parallel edge detection with duplicate elimination
        let triangle_count = self.corner_table.total_triangle_count();

        let candidate_edges: Vec<_> = (0..triangle_count)
            .into_par_iter()
            .map(TriangleIndex)
            .filter(|&t| !self.corner_table.is_triangle_deleted(t))
            .flat_map(|triangle_idx| {
                let mut local_edges = Vec::new();

                for c0p in triangle_idx.corners() {
                    let c0 = self.corner_table.next(c0p);

                    let vic0p = self.corner_table.vertex(c0p);
                    let vic0 = self.corner_table.vertex(c0);

                    // Only process edge in one direction to avoid duplicates
                    if vic0p.0 < vic0.0 {
                        // Canonical ordering
                        let l_sq = self.edge_length_sq(vic0p, vic0);

                        if l_sq < threshold_sq {
                            if let Some((collapse, cost, edge_hash)) = self
                                .calculate_edge_collapse_qem_cost_faer(c0p, &vertex_quadrics_faer)
                            {
                                integrity_println!(
                                    "Added collapse candidate corner {:?} new_pos:{:?}, {l_sq} vs {threshold_sq}", self.dbg_corner(collapse.c0p), collapse.new_pos,
                                );
                                local_edges.push((collapse, cost, edge_hash));
                            }
                        }
                    }
                }
                local_edges
            })
            .collect();

        candidate_edges
    }

    /// Checks that the `candidate` does not cause overlapping or zero area triangles
    /// and other non-manifold mesh anomalies.
    /// `STATIC_MESH` == true means that dirty vertices will not the checked
    pub(super) fn evaluate_collapse_candidate_qem<const STATIC_MESH: bool>(
        &self,
        candidate: CollapseCandidate<S>,
        c0p_fan: &VertexFan,
        twin_fan: &VertexFan,
    ) -> Option<CollapseCandidate<S>> {
        integrity_println!("evaluating candidate :{candidate:?}");

        // Skip if either vertex was already collapsed/touched in this iteration
        if !STATIC_MESH {
            let c0p = c0p_fan[0];
            if self.corner_table.is_corner_deleted(c0p) {
                return None;
            }
            let twin = twin_fan[0];
            if self.corner_table.is_corner_deleted(twin) {
                return None;
            }
            if self.dirty_vertices.is_dirty(self.corner_table.vertex(c0p)) {
                integrity_println!(
                    "bail1: V₀({}) already collapsed in this iteration",
                    self.corner_table.data.dbg_corner(c0p)
                );
                return None;
            }
            if self.dirty_vertices.is_dirty(self.corner_table.vertex(twin)) {
                integrity_println!(
                    "ce_can_still_collapse_edge bail vertex R({}) already collapsed in this iteration",
                    self.corner_table.data.dbg_corner(twin)
                );
                return None;
            }
        }

        if !self
            .evaluate_valence_for_collapse(c0p_fan.len() as i16, twin_fan.len() as i16)
            .0
        {
            return None;
        }
        #[cfg(any(feature = "integrity_check", debug_assertions))]
        {
            use crate::common::macros::integrity_assert_eq;
            integrity_assert_eq!(candidate.c0p, c0p_fan[0]);
            let twin = self.corner_table.twin(candidate.c0p);
            integrity_assert_eq!(twin, twin_fan[0]);
        }

        #[allow(clippy::let_and_return)]
        {
            // TODO: avoid re-building the c0 fan by copying twin_fan
            let cd = self.collapse_data_from_c0p(candidate.c0p);
            let new_v0 = candidate.new_pos.map(|v| v.to_simd());

            let rv = if !self.ce_passes_geometric_checks_qem(&cd, new_v0) {
                None
            } else {
                Some(candidate)
            };

            integrity_println!(
                "CollapseCandidate:: : {:?} corner:{:?}",
                candidate,
                self.dbg_corner(candidate.c0p)
            );
            rv
        }
    }

    /// Checks if the collapse edge operation passes geometric tests.
    /// ```text
    ///             tₖo
    ///     Vₖ ─────────────── Vₖ₋₁
    ///     │ \             / │
    ///     │   \    tₖ    /  │
    ///     │     \      /tₖ₋₁│
    ///     │       \cₖ/      │
    /// t₀o │ t₀ c₀  R ────── V₂
    ///     │      / c₁ \ t₂  │
    ///     │c₀p /       \    │ t₂o
    ///     │  /     t₁   \   │
    ///     │/c₁n           \ │
    ///     V₀ ─────────────── V₁
    ///             t₁o
    /// ```
    pub(super) fn ce_passes_geometric_checks_qem(
        &self,
        cd: &CollapseData,
        new_v0: Option<S::Vec3Simd>,
    ) -> bool {
        if match cd.c0_fan.len() {
            3 => !self.ce_passes_geometric_checks_valence_3_qem(cd, new_v0),
            _ => !self.ce_passes_geometric_checks_valence_4plus_qem(cd, new_v0),
        } {
            return false;
        }

        if let Some(new_v0) = new_v0 {
            // check if it is possible to move V₀ to `new_v0`
            let c0p_fan = self.corner_table.ccw_vertex_fan(cd.c0p);
            integrity_println!(
                "c0p/c₀ surrounding vertices: {} V(fan.prev)",
                self.dbg_fan_prev(&self.corner_table.ccw_vertex_fan(cd.c0p))
            );
            for i in 1..c0p_fan.len().saturating_sub(1) {
                let c = c0p_fan[i];
                let (cn, cp) = self.corner_table.next_prev(c);
                let vcn = self.vertex_of_corner(cn).to_simd();
                let vcp = self.vertex_of_corner(cp).to_simd();

                // Check this triangle with new_v0
                let co = self.corner_table.opposite(c);
                let vco = self.vertex_of_corner(co).to_simd();

                // Check dihedral angle between triangle at c and its opposite
                match Self::normal_similarity_crease_angle(
                    new_v0, // V₀ at new position
                    vcn,    // next vertex in current triangle
                    vcp,    // prev vertex in current triangle
                    vco,    // vertex at opposite corner
                    self.params.crease_limit_threshold_sq,
                ) {
                    Some(true) => {
                        /*integrity_println!(
                            "c0p fan quad V0:{:?},{:?},{:?},{:?} (modified V0) was ok",
                            self.corner_table.get_vertex_of_corner(c),
                            self.corner_table.get_vertex_of_corner(cn),
                            self.corner_table.get_vertex_of_corner(cp),
                            self.corner_table.get_vertex_of_corner(co),
                        );*/
                    }
                    _ => {
                        integrity_println!(
                            "quad check failed: new V₀ fan triangle #{i} vs opposite failed.",
                        );
                        integrity_println!(
                            "c0p fan quad V0:{:?},{:?},{:?},{:?} (modified V0) was NOT ok",
                            self.corner_table.vertex(c),
                            self.corner_table.vertex(cn),
                            self.corner_table.vertex(cp),
                            self.corner_table.vertex(co),
                        );
                        return false;
                    }
                }
            }

            #[allow(clippy::match_like_matches_macro)]
            // Check consecutive triangles in the V₀ fan
            // The fan may be concave, so moving V₀ can cause consecutive triangles to collide
            return Self::validate_consecutive_triangles(
                c0p_fan
                    .iter()
                    .map(|&c| self.vertex_of_corner(self.corner_table.prev(c)).to_simd()),
                new_v0,
                1, // skip first triangle (will be deleted)
                1, // don't check last triangle either
                |normal1, len1_sq, normal2, len2_sq| match Self::validate_fan_triangle_pair(
                    normal1,
                    len1_sq,
                    normal2,
                    len2_sq,
                    self.params.crease_limit_threshold_sq,
                ) {
                    Some(true) => {
                        integrity_println!(
                            "consecutive_fan_test_fast() successfully compared normal{:?} with normal{:?}",
                            normal1.into(),
                            normal2.into()
                        );
                        true
                    }
                    _ => {
                        integrity_println!(
                            "consecutive_fan_test_fast() failed: V₀ fan consecutive triangle normals {:?} l:{} and {:?} {}",
                            normal1.into(),
                            len1_sq,
                            normal2.into(),
                            len2_sq
                        );
                        false
                    }
                },
            );
        }
        true
    }

    //#[rustfmt::skip]
    /// Checks if the collapse edge operation passes geometric tests when valence is larger than 3.
    /// It specially checks if the triangle normals does not cause overlapping or inverted adjacent
    /// triangles
    ///
    /// ```text
    ///             tₖo                                    tₖo
    ///     Vₖ ─────────────── Vₖ₋₁                Vₖ ─────────────── Vₖ₋₁
    ///     │ \cₖp        cₖn/ │                    │cₖp         cₖn/ │
    ///     │c₀n\    tₖ    /  │                    │       tₖ    /   │
    ///     │     \      /tₖ₋₁│                    │           /tₖ₋₁ │ tₖ₋₁o
    ///     │       \cₖ/      .                    │         /      .
    /// t₀o │ t₀ c₀  R ────── V₂     =>   (old)t₀o │       /       V₂
    ///     │      / c₁ \ c₂  │                    │     /       /  │
    ///     │c₀p /       \ t₂ │ t₂o                │    /     /  c₂p│ t₂o
    ///     │  /     t₁   \   │                    │cₖ /   /   t₂   │
    ///     │/c₁n       c₁p \ │                    │ // c₂      c₂n │
    ///     V₀ ─────────────── V₁                  V₀ ───────────── V₁
    ///             t₁o                                 (old)t₁o
    ///
    /// check :
    ///   normal_similarity_opposite_winding(V₀,Vr,Vₖ,Vₖ₋₁) ie. t₀ vs tₖ
    ///   normal_similarity_opposite_winding(V₀,V₁,Vr,V₂)  ie. t₂ vs t₁
    ///
    /// check :
    ///   normal_similarity_same_winding(V₀,Vₙ,Vₙ₋₁,Vr) (for all n>=2, n<k)
    ///
    /// check for opposite normals for:
    ///   (cₙ,cₙn,cₙp) vs tₙo (for all n>=2)
    ///   (c₂n,c₂p,c₂) vs t₁o
    ///   (cₖp,cₖ,cₖn) vs t₀o
    ///   (cₙn,cₙp,cₙ) vs (cₙ₋₁p,cₙ₋₁n,cₙ₋₁) (for all n>=3)
    /// ```
    pub(crate) fn ce_passes_geometric_checks_valence_4plus_qem(
        &self,
        cd: &CollapseData,
        new_v0: Option<S::Vec3Simd>,
    ) -> bool {
        let new_v0 = if let Some(new_v0) = new_v0 {
            new_v0
        } else {
            self.vertex_of_corner(cd.c0p).to_simd()
        };
        let c0_fan = &(cd.c0_fan);

        integrity_assert!(c0_fan.len() >= 4);
        integrity_assert_eq!(cd.c0, c0_fan[0]);
        integrity_assert_eq!(self.corner_table.vertex(cd.c0), cd.vic0);
        integrity_assert_eq!(self.corner_table.vertex(cd.c0p), cd.vic0p);
        integrity_assert_eq!(cd.c0, self.corner_table.next(cd.c0p));
        #[cfg(any(feature = "integrity_check", debug_assertions))]
        let vr = self.vertex_of_corner(cd.c0).to_simd();

        /*#[cfg(feature = "integrity_check")]
        let _dbg = if cd.vic0.0 == 10 && cd.vic0p.0 == 0
        { println!("hello R:10 V₀:V0"); true }
        else { false };*/

        for &c in c0_fan[..].iter().skip(2) {
            integrity_assert_eq!(vr, self.vertex_of_corner(c).to_simd());
            let o = self.corner_table.opposite(c);
            // it's impossible to set V(c)==V(c.opposite())
            if self.corner_table.vertex(o) == cd.vic0p {
                integrity_println!(
                    "geometric checks failed!!: c₀p:{} c₀:{} would cause non-manifold mesh",
                    self.corner_table.data.dbg_corner(cd.c0p),
                    self.corner_table.data.dbg_corner(cd.c0),
                );
                return false;
            }

            let (cn, cp) = self.corner_table.next_prev(c);
            let co = self.corner_table.opposite(c);
            let vcn = self.vertex_of_corner(cn).to_simd();
            let vcp = self.vertex_of_corner(cp).to_simd();
            let vco = self.vertex_of_corner(co).to_simd();

            match Self::normal_similarity_crease_angle(
                new_v0,
                vcn,
                vcp,
                vco,
                self.params.crease_limit_threshold_sq,
            ) {
                Some(true) => continue,
                Some(false) => return false,
                None => {
                    if cd.vic0 == VertexIndex(7) {
                        integrity_println!(
                            "dihedral_angle checks failed. c:{}",
                            self.corner_table.dbg_triangle(c)
                        );
                    }
                    return false;
                }
            }
        }

        // Check tₖ (last triangle in fan) vs t₀o
        let ck = c0_fan[c0_fan.len().saturating_sub(1)];
        let (ckn, ckp) = self.corner_table.next_prev(ck);
        let vckn = self.vertex_of_corner(ckn).to_simd(); // Vₖ

        let vickp = self.corner_table.vertex(ckp); // Vₖ₋₁'
        let vckp = self.vertex(vickp).to_simd(); // Vₖ₋₁

        let t0o = self.corner_table.opposite(cd.c0);
        let vt0o = self.vertex_of_corner(t0o).to_simd();

        match Self::normal_similarity_crease_angle(
            vt0o,
            vckp,
            new_v0,
            vckn,
            self.params.crease_limit_threshold_sq,
        ) {
            Some(true) => {}
            _ => return false,
        }
        /*integrity_println!(
            "current cₖ:{} vs current t₀o:{} with V₀:V{}@cₖ was ok!",
            self.corner_table.dbg_triangle(ck),
            self.corner_table.dbg_triangle(t0o),
            cd.vic0p.0
        );*/

        // Check t₂ vs t₁o (assuming c0_fan has at least t₀, t₁, t₂)
        if c0_fan.len() >= 3 {
            let c2 = c0_fan[2];
            let c1 = c0_fan[1];
            let t1o = self.corner_table.opposite(c1);
            let (c2n, c2p) = self.corner_table.next_prev(c2);
            let vc2n = self.vertex_of_corner(c2n).to_simd();
            let vc2p = self.vertex_of_corner(c2p).to_simd();
            let vt1o = self.vertex_of_corner(t1o).to_simd();

            match Self::normal_similarity_crease_angle(
                vt1o,
                new_v0,
                vc2n,
                vc2p,
                self.params.crease_limit_threshold_sq,
            ) {
                Some(true) => {}
                _ => {
                    /*if cd.vic0 == VertexIndex(7) {
                        integrity_println!("geometric checks failed: t₂ vs t₁o clash");
                    }*/
                    return false;
                }
            }

            /*integrity_println!(
                "current c₂:{} vs current t₁o:{} with V₀:V{}@c₂ was ok! ",
                self.corner_table.dbg_triangle(c2),
                self.corner_table.dbg_triangle(t1o),
                cd.vic0p.0
            );*/
        }

        //integrity_println!(" slow: {slow}");
        #[allow(clippy::match_like_matches_macro)]
        let is_ok = Self::validate_consecutive_triangles(
            c0_fan
                .iter()
                .map(|&c| self.vertex_of_corner(self.corner_table.prev(c)).to_simd()),
            new_v0,
            2, // skip first two triangles (will be deleted)
            0,
            |normal1, len1_sq, normal2, len2_sq| match Self::validate_fan_triangle_pair(
                normal1,
                len1_sq,
                normal2,
                len2_sq,
                self.params.crease_limit_threshold_sq,
            ) {
                Some(true) => true,
                _ => {
                    integrity_println!(
                        "consecutive_fan_test_fast() failed: V₀ fan consecutive triangles {:?} l:{} and {:?} {}",
                        normal1.into(),
                        len1_sq,
                        normal2.into(),
                        len2_sq
                    );
                    false
                }
            },
        );

        if !is_ok {
            return false;
        }

        integrity_println!(
            "geometric checks passed v4+: c₀p:{} c₀:{} new_v0:{:?}",
            self.dbg_corner(cd.c0p),
            self.dbg_corner(cd.c0),
            new_v0.into(),
        );
        true
    }

    /// ```text
    /// Specialized triangle check for valence 3 case: tests if R vertex lies in the plane of V₀V₁V₂.
    /// Only three triangles exist in the R fan: t₀, t₁, and t₂.
    ///     V₂ ─────────────
    ///     │ \              \  t₂o
    ///     │   \        t₂   │
    ///     │     \           │
    ///     │       \  c₂     │
    /// t₀o │ t₀ c₀  R        │
    ///     │      / c₁ \     │
    ///     │c₀p /       \    │
    ///     │  /     t₁   \   │
    ///     │/              \ │
    ///     V₀ ────────────── V₁
    ///              t₁o
    ///
    /// Input: c₀p: The corner index at V₀ in the RV₂V₀=t₀ triangle.
    /// V(c₀)==R, V(c₀.prev())==V₀, V(c₀.next())==V₂
    /// ```
    pub(crate) fn ce_passes_geometric_checks_valence_3_qem(
        &self,
        _cd: &CollapseData,
        _new_v0: Option<S::Vec3Simd>,
    ) -> bool {
        integrity_assert_eq!(_cd.c0_fan.len(), 3);
        integrity_assert_eq!(_cd.c0, self.corner_table.next(_cd.c0p));
        #[cfg(any(feature = "integrity_check", debug_assertions))]
        let c1 = _cd.c0_fan[1];
        #[cfg(any(feature = "integrity_check", debug_assertions))]
        let c2 = _cd.c0_fan[2];

        #[cfg(any(feature = "integrity_check", debug_assertions))]
        let v1 = self.vertex_of_corner(self.corner_table.prev(c1)).to_simd();
        #[cfg(any(feature = "integrity_check", debug_assertions))]
        let v2 = self
            .vertex_of_corner(self.corner_table.next(_cd.c0))
            .to_simd();
        integrity_assert_eq!(
            v1,
            self.vertex_of_corner(self.corner_table.next(c2)).to_simd()
        );
        integrity_assert_eq!(
            v2,
            self.vertex_of_corner(self.corner_table.prev(c2)).to_simd()
        );

        integrity_println!(
            "geometric checks v3 passed: c₀p:{} c₀:{} new_v₀ {_new_v0:?}",
            self.corner_table.data.dbg_corner(_cd.c0p),
            self.corner_table.data.dbg_corner(_cd.c0),
        );

        true
    }
}