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

use crate::common::VertexIndex;
use crate::corner_table::{CornerIndex, TriangleIndex};
use crate::isotropic_remesh::IsotropicRemeshAlgo;
use crate::isotropic_remesh::collapse_edges::CollapseCandidate;
use faer::linalg::solvers::Solve;
use faer::prelude::*;
use rayon::iter::IntoParallelIterator;
#[allow(unused_imports)]
use rayon::prelude::*;
use std::fmt::Debug;
use vector_traits::glam::DVec3;
use vector_traits::num_traits::AsPrimitive;
use vector_traits::prelude::GenericVector3;

fn calculate_quadric_error_faer(q: &Mat<f64>, pos: DVec3) -> f64 {
    // Create homogeneous coordinate vector [x, y, z, 1]
    let v = mat![[pos.x], [pos.y], [pos.z], [1.0]];

    // Calculate v^T * Q * v
    let qv = q * &v;
    let result = v.transpose() * qv;

    result[(0, 0)]
}

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,
{
    /// Generate the quadratics of a triangle
    fn triangle_quadric_faer(&self, t: TriangleIndex) -> Mat<f64> {
        let (x, y, z, w) = {
            let [c, cn, cp] = t.corners();
            let vc = self.vertex_of_corner_f64(c);
            let vcn = self.vertex_of_corner_f64(cn);
            let vcp = self.vertex_of_corner_f64(cp);

            let edge1 = vcn - vc;
            let edge2 = vcp - vc;
            let normal = edge1.cross(edge2).normalize();
            (normal.x, normal.y, normal.z, -normal.dot(vc))
        };

        // Create 4x4 matrix
        mat![
            [x * x, x * y, x * z, x * w],
            [x * y, y * y, y * z, y * w],
            [x * z, y * z, z * z, z * w],
            [x * w, y * w, z * w, w * w],
        ]
    }

    pub(super) fn build_qem_quadratics_faer(&self) -> Vec<Mat<f64>> {
        if self.vertices.len() > 500 {
            // Triangle quadrics
            let triangle_quadrics: Vec<_> = (0..self.corner_table.total_triangle_count())
                .into_par_iter()
                .with_min_len(100)
                .map(TriangleIndex)
                .map(|t| {
                    if !self.corner_table.is_triangle_deleted(t) {
                        self.triangle_quadric_faer(t)
                    } else {
                        Mat::zeros(4, 4)
                    }
                })
                .collect();

            // Vertex quadrics
            (0..self.vertices.len() as u32)
                .into_par_iter()
                .with_min_len(100)
                .map(VertexIndex)
                .map(|vertex| {
                    let mut q = Mat::<f64>::zeros(4, 4);
                    if !self.is_vertex_deleted(vertex) {
                        let corner_of_vertex = self.corner_table.corner(vertex);
                        for corner in self.corner_table.iter_ccw_swing(corner_of_vertex) {
                            // Add matrices element-wise
                            q += &triangle_quadrics[corner.triangle().0 as usize];
                        }
                    }
                    q
                })
                .collect()
        } else {
            // Triangle quadrics
            let triangle_quadrics: Vec<_> = (0..self.corner_table.total_triangle_count())
                .map(TriangleIndex)
                .map(|t| {
                    if !self.corner_table.is_triangle_deleted(t) {
                        self.triangle_quadric_faer(t)
                    } else {
                        Mat::zeros(4, 4)
                    }
                })
                .collect();

            // Vertex quadrics
            (0..self.vertices.len() as u32)
                .map(VertexIndex)
                .map(|vertex| {
                    let mut q = Mat::<f64>::zeros(4, 4);
                    if !self.is_vertex_deleted(vertex) {
                        let corner_of_vertex = self.corner_table.corner(vertex);
                        for corner in self.corner_table.iter_ccw_swing(corner_of_vertex) {
                            // Add matrices element-wise
                            q += &triangle_quadrics[corner.triangle().0 as usize];
                        }
                    }
                    q
                })
                .collect()
        }
    }

    pub(super) fn calculate_edge_collapse_qem_cost_faer(
        &self,
        corner: CornerIndex, // edge is between vertices : V(corner) -> V(corner.twin())
        vertex_quadrics: &[Mat<f64>],
    ) -> Option<(CollapseCandidate<S>, f64, u64)> {
        let twin = self.corner_table.twin(corner);
        let corner_fan = self.corner_table.ccw_vertex_fan(corner);
        let twin_fan = self.corner_table.ccw_vertex_fan(twin);

        let (can_collapse_corner, can_collapse_twin) =
            self.evaluate_valence_for_collapse(corner_fan.len() as i16, twin_fan.len() as i16);
        if !can_collapse_corner && !can_collapse_twin {
            return None;
        }
        let vi0 = self.corner_table.vertex(corner);
        let vi1 = self.corner_table.vertex(twin);

        /*if (vi0.0 == 1 || vi1.0 == 1) && (vi0.0 == 4 || vi1.0 == 4) {
            integrity_println!("#### hello debugger! create candidate corner:{:?} twin:{:?}" ,self.dbg_corner(corner),self.dbg_corner(twin) );
        }*/

        let v0 = self.vertex_f64(vi0);
        let v1 = self.vertex_f64(vi1);

        // Combined quadric - add matrices
        let mut q_combined = Mat::zeros(4, 4);
        q_combined += &vertex_quadrics[vi0.0 as usize];
        q_combined += &vertex_quadrics[vi1.0 as usize];

        // Extract 3x3 upper-left block and 3x1 right column
        let q_3x3 = q_combined.submatrix(0, 0, 3, 3);
        let q_3x1 = q_combined.submatrix(0, 3, 3, 1);

        // Solve using LU decomposition: A * x = -b
        // TODO: remove the half-point and keep optimal_pos an Option<_>
        let optimal_pos = {
            let lu = q_3x3.partial_piv_lu();
            let solution = lu.solve(-q_3x1);
            let solution = DVec3::new(solution[(0, 0)], solution[(1, 0)], solution[(2, 0)]);
            solution.is_finite().then_some(solution).filter(|solution| {
                // if the solution is too far from both endpoints we return None.
                // if either endpoint is very close to the solution we return None.
                // in any of those cases the best endpoint will still be picked.
                let d0 = solution.distance_sq(v0);
                let d1 = solution.distance_sq(v1);

                d0 >= self.params.collapse_qem_min_dist_sq
                    && d1 >= self.params.collapse_qem_min_dist_sq
                    && !(d0 > self.params.collapse_qem_max_dist_sq
                        && d1 > self.params.collapse_qem_max_dist_sq)
            })
        };

        let mut candidates: heapless::Vec<(CollapseCandidate<S>, f64, u64), 4> =
            heapless::Vec::new();

        if can_collapse_twin {
            let cost = calculate_quadric_error_faer(&q_combined, v0);
            if cost < self.params.collapse_qem_threshold_sq {
                let _ = candidates.push((
                    CollapseCandidate::keep_pos(corner),
                    cost,
                    self.corner_table.canonical_edge_hash(corner),
                ));
            }
        }
        if can_collapse_corner {
            let cost = calculate_quadric_error_faer(&q_combined, v1);
            if cost < self.params.collapse_qem_threshold_sq {
                let _ = candidates.push((
                    CollapseCandidate::keep_pos(twin),
                    cost,
                    self.corner_table.canonical_edge_hash(twin),
                ));
            }
        }
        if let Some(optimal_pos) = optimal_pos {
            let optimal_cost = calculate_quadric_error_faer(&q_combined, optimal_pos);
            if optimal_cost < self.params.collapse_qem_threshold_sq {
                let optimal_s_pos = S::Vec3::new(
                    optimal_pos.x.as_(),
                    optimal_pos.y.as_(),
                    optimal_pos.z.as_(),
                );

                let _ = candidates.push((
                    CollapseCandidate::move_pos(corner, optimal_s_pos),
                    optimal_cost,
                    self.corner_table.canonical_edge_hash(corner),
                ));
                let _ = candidates.push((
                    CollapseCandidate::move_pos(twin, optimal_s_pos),
                    optimal_cost,
                    self.corner_table.canonical_edge_hash(twin),
                ));
            }
        }

        // sort the candidates in ascending order
        candidates.sort_unstable_by(|(_, a, ah), (_, b, bh)| {
            (a, ah)
                .partial_cmp(&(b, bh)) // This gives DESCENDING order
                .unwrap_or(std::cmp::Ordering::Equal)
        });

        for (candidate, cost, hash) in candidates.into_iter() {
            let (fan1, fan2) = if candidate.c0p != corner {
                (&twin_fan, &corner_fan)
            } else {
                (&corner_fan, &twin_fan)
            };

            // return the first success
            if let Some(candidate) =
                self.evaluate_collapse_candidate_qem::<true>(candidate, fan1, fan2)
            {
                return Some((candidate, cost, hash));
            }
        }
        None
    }
}
