qchull 2.0.1-beta.0

// Copyright (C) 2020-2025 qchull authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use std::mem;

use glam_det::nums::num_traits::Signed;
use glam_det::nums::Float;
use glam_det::{Cross, Dot, Mat3, Point3, UnitVec3, Vec3};
use rustc_hash::FxHashMap;
use smallvec::SmallVec;

use crate::common::normalize_and_len;
use crate::convex_hull::HalfPlane;
use crate::face::{
    Face, FaceEdgeIter, FaceEdgeIterInv, FaceIndexSmallVec, FaceKey, FaceMap, FaceState,
};
use crate::half_edge::{EdgeIndexSmallVec, EdgeMap, HalfEdge, HalfEdgeKey};
#[cfg(feature = "safe_check")]
use crate::quick_hull_3d::convex_hull_safe_check::SimplexFace;
#[cfg(feature = "dump")]
use crate::quick_hull_3d::quick_hull_dump::{DebugInfo, MergeFaceCheck};
use crate::ConvexHull;
use crate::ConvexHullConstructState::{
    FaceCountLimitHit, PointCountLessThanFour, PointCountOverflow, Success, VolumeTooSmall,
};

const DEFAULT_FACE_CAPACITY: usize = 4;
const DEFAULT_EDGE_CAPACITY: usize = 64;

pub(crate) type RescheduleConflictPointIndexSmallVec = SmallVec<[usize; 64]>;

#[cfg(any(feature = "safe_check", feature = "dump"))]
pub(crate) const MAX_DEBUG_ITER_COUNT: usize = 0;
pub(crate) const MAX_POINT_COUNT: u32 = 65536; //2^16

const DEFAULT_FACE_MAX: u32 = 255;

const DEFAULT_VALID_POINT_MAX: usize = 256;
type PointSmallVec<PointType> = SmallVec<[PointType; DEFAULT_VALID_POINT_MAX]>;

struct ShiftResult {
    pub shift_center: Vec3,
    pub shifted_points: Vec<Point3>,
}
impl ShiftResult {
    pub fn new(shift_center: Vec3, shifted_points: Vec<Point3>) -> Self {
        Self {
            shift_center,
            shifted_points,
        }
    }
}

///
///
/// # Arguments
///
/// * `points`: the points to build convex hull
/// * `max_face_count`: the max face count of the convex hull,if the face count exceeds this
///   value,return error,The recommended number of `max_face_count` is 255.
///
/// Returns: Result<`ConvexHull`, `ConvexHullConstructState`>
///
/// # Examples
///
/// ```
/// use approx_det::assert_relative_eq;
/// use glam_det::{Point3, Vec3};
/// use qchull::{Config, ConvexHullConstructState, FaceEdgeIter};
/// let points = vec![
///         Point3::new(0.0, 0.0, 0.0),
///         Point3::new(1.0, 0.0, 0.0),
///         Point3::new(0.0, 1.0, 0.0),
///         Point3::new(0.0, 0.0, 1.0),
/// ];
/// let config = Config::default();
/// let convex_hull_result = qchull::build_convex_hull(&points,&config);
/// let (convex_hull,_) = convex_hull_result.unwrap();
/// assert_eq!(convex_hull.bounding_planes.len() , 4);
/// assert!(convex_hull.volume > 0.0);
/// assert_eq!(convex_hull.points.len(), 4);
/// assert_eq!(convex_hull.face_vertex_indices.len(), 12);
/// assert_eq!(convex_hull.face_indices_start.len(), 4);
/// assert_eq!(convex_hull.center, Vec3::new(0.25,0.25,0.25));
/// for (i,face_index) in convex_hull.face_indices_start.iter().enumerate() {
///     let face_vertex_indices = &convex_hull.face_vertex_indices;
///     let start = *face_index as usize;
///     let end = if i == convex_hull.face_indices_start.len() - 1 {
///         face_vertex_indices.len()
///     } else {
///         convex_hull.face_indices_start[i + 1] as usize
///     };
///     let face_vertex_indices = &face_vertex_indices[start..end];
///     let mut face_vertices = Vec::<Point3>::with_capacity(face_vertex_indices.len());
///     for &index in face_vertex_indices {
///         face_vertices.push(convex_hull.points[index as usize]);
///     }
/// }
/// ```
///
/// # Errors
///
/// If the point count is less than 4,return `ConvexHullConstructState::PointCountLessThanFour`
///
/// If the point count is larger than 2^16,return `ConvexHullConstructState::PointCountOverflow`
///
/// If the volume of convex hull is too small,return `ConvexHullConstructState::VolumeTooSmall`
///
/// If the face count exceeds the `max_face_count`,return
/// `ConvexHullConstructState::FaceCountLimitedHit`
///
/// If a face has the same edges with another face,or the face count is less than 4,that cant be
/// handled in the next step,return `ConvexHullConstructState::Degenerate`
///
/// # Panics
///
/// Panic if the face's edge count is less than 3,or the face edge's topology is not valid,the face
/// edge's topology will be invalid for some float epsilon case. You can change the epsilon for each
/// mesh to avoid this panic.
pub fn build_convex_hull(
    points: &[Point3],
    config: &Config,
) -> Result<(ConvexHull, ConvexHullConstructState), ConvexHullConstructError> {
    let shift_result = if config.shift_point_align_aabb_center {
        let shift_center = calculate_aabb_center(points);
        let shifted_points = shift_points_align_center(points, shift_center);
        Some(ShiftResult::new(shift_center, shifted_points))
    } else {
        None
    };

    let points = match &shift_result {
        Some(p) => p.shifted_points.as_slice(),
        None => points,
    };

    let convex_faces_result = build_convex_face(points, config)?;
    // if the state is Success,just continue
    // if the state is degenerate,set the status as degenerate,set the convex_hull as None, return
    // the result if the state is others,recalculate the convex hull with aabb points,return the new
    // convex hull
    let rebuild_result = validate_and_rebuild_if_needed(convex_faces_result, points, config)?;
    let (points, convex_faces, state) = rebuild_result.triple(points);
    let mut points_valid = SmallVec::<[bool; DEFAULT_VALID_POINT_MAX]>::with_capacity(points.len());
    points_valid.resize(points.len(), false);
    for (_, face) in convex_faces.faces.iter() {
        let iter = FaceEdgeIter::new(face.edge_index, &convex_faces.edges);
        for (_, edge) in iter {
            points_valid[edge.origin] = true;
            points_valid[edge.destination] = true;
        }
    }
    let mut valid_points = Vec::<Point3>::with_capacity(points.len());
    let mut index_map = Vec::<usize>::with_capacity(points.len());
    index_map.resize(points.len(), usize::MAX);
    for (i, &valid) in points_valid.iter().enumerate() {
        if valid {
            index_map[i] = valid_points.len();
            valid_points.push(points[i]);
        }
    }
    let mut convex_hull = ConvexHull::with_points(valid_points);
    convex_hull.shift_center = shift_result.as_ref().map(|r| r.shift_center);
    for (_, face) in convex_faces.faces.iter() {
        let center = face.center;
        let offset = face.normal.dot(center.as_vec3());
        let half_plane = HalfPlane::new(center, face.normal, offset);
        convex_hull.bounding_planes.push(half_plane);
    }
    let mut center = Vec3::ZERO;
    let mut volume = 0.0;
    for (_, face) in convex_faces.faces.iter() {
        convex_hull
            .face_indices_start
            .push(convex_hull.face_vertex_indices.len() as u32);
        let iter = &mut FaceEdgeIterInv::new(face.edge_index, &convex_faces.edges);
        let (_, first_edge) = iter
            .take(1)
            .next()
            .expect("face has no edge,it should not happen");
        let index_a = index_map[first_edge.origin];
        let index_b = index_map[first_edge.destination];
        convex_hull.face_vertex_indices.push(index_b as u32);

        for (_, edge) in iter {
            let index_b = index_map[edge.origin];
            let index_c = index_map[edge.destination];
            convex_hull.face_vertex_indices.push(index_c as u32);
            let a = convex_hull.points[index_a].as_vec3();
            let b = convex_hull.points[index_b].as_vec3();
            let c = convex_hull.points[index_c].as_vec3();
            let sub_volume = b.cross(a).dot(c) * 6f32.recip();
            center += sub_volume * (a + b + c);
            volume += sub_volume;
        }
    }
    center /= volume * 4f32;
    convex_hull.center = if let Some(shift_center) = convex_hull.shift_center {
        center + shift_center
    } else {
        center
    };
    convex_hull.volume = volume;
    Ok((convex_hull, state))
}

fn calculate_aabb_center(points: &[Point3]) -> Vec3 {
    if points.is_empty() {
        Vec3::ZERO
    } else {
        let mut min = Vec3::splat(f32::MAX);
        let mut max = Vec3::splat(f32::MIN);

        for point in points {
            min = min.min(point.as_vec3());
            max = max.max(point.as_vec3());
        }

        (min + max) * 2f32.recip()
    }
}

fn shift_points_align_center(points: &[Point3], center: Vec3) -> Vec<Point3> {
    let mut shifted_points = Vec::with_capacity(points.len());

    for point in points {
        let shifted = point - center;
        shifted_points.push(shifted);
    }

    shifted_points
}

///
///
/// # Arguments
///
/// * `points`: the points to build convex hull
/// * `max_face_count`: the max face count of the convex hull,if the face count exceeds this
///   value,return error,The recommended number of `max_face_count` is 255.
///
/// Returns: Result<`ConvexFinalFaces`, `ConvexHullConstructState`>
///
/// # Examples
///
/// ```
/// use glam_det::Point3;
/// use qchull::{Config, ConvexFinalFacesConstructResult, FaceEdgeIter};
/// let points = vec![
///         Point3::new(0.0, 0.0, 0.0),
///         Point3::new(1.0, 0.0, 0.0),
///         Point3::new(0.0, 1.0, 0.0),
///         Point3::new(0.0, 0.0, 1.0),
/// ];
/// let mut config = Config::default();
/// let convex_faces = qchull::build_convex_face(&points, &mut config).unwrap();
/// let convex_faces = match convex_faces {
///     ConvexFinalFacesConstructResult::Success(v) => {v}
///     ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(_) => {unreachable!("should not be warning")}
///     ConvexFinalFacesConstructResult::PartialSuccess(partial_v,_) => {partial_v}
/// };
/// let face_count = convex_faces.faces.len();
/// assert_eq!(face_count, 4);
/// let (_,first_face) = convex_faces.faces.iter().next().unwrap();
/// assert_eq!(first_face.num_points, 3);
/// let edge_iter = FaceEdgeIter::new(first_face.edge_index, &convex_faces.edges);
/// for (_,edge) in edge_iter {
///    let origin =points[edge.origin];
///    let destination = points[edge.destination];
///    println!("edge origin:{:?},destination:{:?}",origin,destination);
/// }
/// ```
///
/// # Errors
///
/// If the point count is less than 4,return `ConvexHullConstructState::PointCountLessThanFour`
///
/// If the point count is larger than 2^16,return `ConvexHullConstructState::PointCountOverflow`
///
/// If the volume of convex hull is too small,return `ConvexHullConstructState::VolumeTooSmall`
///
/// If the face count exceeds the `max_face_count`,return
/// `ConvexHullConstructState::FaceCountLimitedHit`
///
/// If a face has the same edges with another face,or the face count is less than 4,that cant be
/// handled in the next step,return `ConvexHullConstructState::Degenerate`
pub fn build_convex_face(
    points: &[Point3],
    config: &Config,
) -> Result<ConvexFinalFacesConstructResult, ConvexHullConstructError> {
    let mut convex_final_faces = ConvexFinalFaces::new();
    if points.len() < 4 {
        let warning =
            ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(PointCountLessThanFour);
        return Ok(warning);
    }
    if points.len() > config.max_point_count as usize {
        let warning =
            ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(PointCountOverflow);
        return Ok(warning);
    }
    let extreme_indices = get_max_min_index_in_xyz_direction(points);
    let [x_min_index, x_max_index, y_min_index, y_max_index, z_min_index, z_max_index] =
        extreme_indices;
    let x_thickness = abs_max::<0>(x_max_index, x_min_index, points);
    let y_thickness = abs_max::<1>(y_max_index, y_min_index, points);
    let z_thickness = abs_max::<2>(z_max_index, z_min_index, points);
    let epsilon_scale = x_thickness + y_thickness + z_thickness;
    let config_recalculate = config.recalculate_epsilon_if_need(epsilon_scale);
    let config = config_recalculate.as_ref().unwrap_or(config);
    let line_index = max_distance_line(points, &extreme_indices);
    if line_index.0 == line_index.1 {
        let warning = ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(VolumeTooSmall);
        return Ok(warning);
    }
    let third_index = get_max_distance_index_from_line(line_index, points);
    let a = points[line_index.0];
    let b = points[line_index.1];
    let c = points[third_index];
    let neg_triangle_normal = (c - a).cross(b - a);
    if neg_triangle_normal.length() < 1e-8 {
        let warning = ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(VolumeTooSmall);
        return Ok(warning);
    }
    let unit_neg_triangle_normal = neg_triangle_normal.normalize_to_unit();
    let offset = a.as_vec3().dot(unit_neg_triangle_normal.as_vec3());
    let plane = Plane::new(unit_neg_triangle_normal, offset);
    let index_a = line_index.0;
    let mut index_b = line_index.1;
    let mut index_c = third_index;
    let invalid_points = [index_a, index_b, index_c];
    let (max_distance, max_distance_abs, fourth_index) =
        get_farthest_index_from_plane(&plane, points, &invalid_points);
    if max_distance_abs < config.min_init_distance {
        let warning = ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(VolumeTooSmall);
        return Ok(warning);
    }
    let index_d = fourth_index;
    if max_distance > 0.0 {
        mem::swap(&mut index_b, &mut index_c);
    }
    let simplex = SimplexIndex::new(index_a, index_b, index_c, index_d);
    convex_final_faces.init_simplex(points, simplex);
    convex_final_faces.init_conflict_points(points, &simplex, config.conflict_point_epsilon);
    let status = convex_final_faces.update(
        points,
        config.max_face_count,
        config.epsilon,
        config.epsilon,
        config.conflict_point_epsilon,
        config.plane_test_epsilon,
    )?;
    #[cfg(feature = "dump")]
    convex_final_faces.dump_to_json_file();
    Ok(convert_to_construct_result(convex_final_faces, status))
}

fn convert_to_construct_result(
    convex_final_faces: ConvexFinalFaces,
    status: ConvexHullConstructState,
) -> ConvexFinalFacesConstructResult {
    match status {
        Success => ConvexFinalFacesConstructResult::Success(convex_final_faces),
        PointCountLessThanFour | VolumeTooSmall | PointCountOverflow => {
            ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(status)
        }
        FaceCountLimitHit => {
            ConvexFinalFacesConstructResult::PartialSuccess(convex_final_faces, status)
        }
    }
}
impl ConvexFinalFacesConstructResult {
    #[inline]
    #[must_use]
    pub fn state(&self) -> ConvexHullConstructState {
        match self {
            ConvexFinalFacesConstructResult::Success(_) => Success,
            ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(state) => *state,
            ConvexFinalFacesConstructResult::PartialSuccess(_, partial_state) => *partial_state,
        }
    }
}

impl RebuildResult {
    #[inline]
    pub fn triple<'a>(
        &'a self,
        origin_points: &'a [Point3],
    ) -> (&'a [Point3], &'a ConvexFinalFaces, ConvexHullConstructState) {
        match self {
            RebuildResult::Origin(v) => (origin_points, v, Success),
            RebuildResult::Rebuild(points, v, state) => (points, v, *state),
            RebuildResult::Partial(v, state) => (origin_points, v, *state),
        }
    }

    #[inline]
    pub fn convex_final_faces(&self) -> &ConvexFinalFaces {
        match self {
            RebuildResult::Origin(v) => v,
            RebuildResult::Rebuild(_, rebuild_v, _) => rebuild_v,
            RebuildResult::Partial(partial_v, _) => partial_v,
        }
    }
}

#[derive(PartialEq, Clone, Copy)]
enum MergeType {
    NonConvexWrtLargerFace,
    NonConvex,
}

struct ConnectEdgeResult {
    pub get_ride_of_pre_edge: bool,
    pub connect_edge_change: bool,
    pub discard_face: Option<FaceKey>,
}

impl Default for ConnectEdgeResult {
    #[inline]
    fn default() -> Self {
        Self {
            get_ride_of_pre_edge: false,
            connect_edge_change: false,
            discard_face: None,
        }
    }
}

struct Plane {
    offset: f32,
    normal: UnitVec3,
}
impl Plane {
    #[inline]
    fn new(normal: UnitVec3, offset: f32) -> Self {
        Self { offset, normal }
    }

    #[inline]
    fn distance(&self, point: Point3) -> f32 {
        let dot_value = point.as_vec3().dot(self.normal);
        dot_value - self.offset
    }
}

#[derive(Debug, PartialEq, Copy, Clone)]
pub enum ConvexHullConstructState {
    Success,
    //warning
    PointCountLessThanFour,
    VolumeTooSmall,
    PointCountOverflow,
    FaceCountLimitHit,
}

#[derive(Debug, PartialEq, Copy, Clone)]
pub enum ConvexHullConstructError {
    Degenerate,
}

impl Default for ConvexHullConstructState {
    #[inline]
    fn default() -> Self {
        Self::Success
    }
}

struct CreateTriangleFaceResult {
    face_index: FaceKey,
    edge_ab: HalfEdgeKey,
    edge_ca: HalfEdgeKey,
}

impl CreateTriangleFaceResult {
    #[inline]
    pub fn new(face_index: FaceKey, edge_ab: HalfEdgeKey, edge_ca: HalfEdgeKey) -> Self {
        Self {
            face_index,
            edge_ab,
            edge_ca,
        }
    }
}

fn cal_plane_merge_epsilon(area: f32, epsilon: f32) -> f32 {
    let epsilon_scale = area.sqrtf() * 2f32;
    let epsilon_scale = f32::maxf(epsilon_scale, 1.0f32);
    let plane_test_epsilon = epsilon_scale * epsilon;
    plane_test_epsilon.minf(0.0001_f32)
}

fn get_max_min_index_in_xyz_direction(points: &[Point3]) -> [usize; 6] {
    let mut min_x_index = 0;
    let mut max_x_index = 0;
    let mut min_y_index = 0;
    let mut max_y_index = 0;
    let mut min_z_index = 0;
    let mut max_z_index = 0;

    for (i, point) in points.iter().enumerate() {
        if point[0] < points[min_x_index][0] {
            min_x_index = i;
        }
        if point[0] > points[max_x_index][0] {
            max_x_index = i;
        }
        if point[1] < points[min_y_index][1] {
            min_y_index = i;
        }
        if point[1] > points[max_y_index][1] {
            max_y_index = i;
        }
        if point[2] < points[min_z_index][2] {
            min_z_index = i;
        }
        if point[2] > points[max_z_index][2] {
            max_z_index = i;
        }
    }
    [
        min_x_index,
        max_x_index,
        min_y_index,
        max_y_index,
        min_z_index,
        max_z_index,
    ]
}

#[inline]
fn abs_max<const AXIS: usize>(index0: usize, index1: usize, points: &[Point3]) -> f32 {
    let p0 = points[index0];
    let p1 = points[index1];
    f32::maxf(p0[AXIS].absf(), p1[AXIS].absf())
}

fn get_max_distance_index_from_line(line_index_pair: (usize, usize), points: &[Point3]) -> usize {
    let point0 = points[line_index_pair.0];
    let point1 = points[line_index_pair.1];
    let line_dir = (point1 - point0).normalize();
    let mut max_distance = 0f32;
    let mut max_index = 0_usize;
    for (i, point) in points.iter().enumerate() {
        let dot_len = (point - point0).dot(line_dir);
        let point_project = point0 + line_dir * dot_len;
        let distance = point.distance_squared(point_project);
        if distance > max_distance {
            max_distance = distance;
            max_index = i;
        }
    }
    max_index
}

fn get_farthest_index_from_plane(
    plane: &Plane,
    points: &[Point3],
    invalid_points: &[usize],
) -> (f32, f32, usize) {
    let mut max_distance = f32::MIN;
    let mut max_distance_abs = f32::MIN;
    let mut max_index = usize::MAX;
    for (i, point) in points.iter().enumerate() {
        if invalid_points.contains(&i) {
            continue;
        }
        let distance = plane.distance(*point);
        let dist_abs = distance.absf();
        if dist_abs > max_distance_abs {
            max_distance_abs = dist_abs;
            max_distance = distance;
            max_index = i;
        }
    }
    (max_distance, max_distance_abs, max_index)
}

fn max_distance_line(points: &[Point3], extreme_indices: &[usize; 6]) -> (usize, usize) {
    let mut line_index: (usize, usize) = (0, 0);
    let mut max_distance = 0.0_f32;
    for i in 0..6 {
        for j in 0..i {
            let index_i = extreme_indices[i];
            let index_j = extreme_indices[j];
            let point_i = points[index_i];
            let point_j = points[index_j];
            let distance = (point_i - point_j).length();
            if distance > max_distance {
                max_distance = distance;
                line_index = (index_i, index_j);
            }
        }
    }
    line_index
}

#[derive(Copy, Clone)]
pub(crate) struct SimplexIndex {
    pub index_a: usize,
    pub index_b: usize,
    pub index_c: usize,
    pub index_d: usize,
}

impl SimplexIndex {
    #[inline]
    pub(crate) fn new(index_a: usize, index_b: usize, index_c: usize, index_d: usize) -> Self {
        Self {
            index_a,
            index_b,
            index_c,
            index_d,
        }
    }

    #[inline]
    pub(crate) fn contains(&self, index: usize) -> bool {
        self.index_a == index
            || self.index_b == index
            || self.index_c == index
            || self.index_d == index
    }
}

#[allow(clippy::large_enum_variant)]
pub enum ConvexFinalFacesConstructResult {
    Success(ConvexFinalFaces),
    NeedRebuildWithAabbPoints(ConvexHullConstructState),
    PartialSuccess(ConvexFinalFaces, ConvexHullConstructState),
}

pub enum RebuildResult {
    Origin(ConvexFinalFaces),
    Rebuild(Vec<Point3>, ConvexFinalFaces, ConvexHullConstructState),
    Partial(ConvexFinalFaces, ConvexHullConstructState),
}
fn validate_and_rebuild_if_needed(
    value: ConvexFinalFacesConstructResult,
    points: &[Point3],
    config: &Config,
) -> Result<RebuildResult, ConvexHullConstructError> {
    match value {
        ConvexFinalFacesConstructResult::Success(v) => Ok(RebuildResult::Origin(v)),
        ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(state) => {
            let aabb_points = ConvexFinalFaces::calculate_aabb_points(points, config.epsilon);
            let result = build_convex_face(&aabb_points, config)?;
            construct_result_to_rebuild_result(state, aabb_points, result)
        }
        ConvexFinalFacesConstructResult::PartialSuccess(value, state) => {
            if config.expand_faces_in_partial_success_state {
                let expanded_points = expand_convex_hull(points, value);
                let result = build_convex_face(&expanded_points, config)?;
                construct_result_to_rebuild_result(state, expanded_points, result)
            } else {
                Ok(RebuildResult::Partial(value, state))
            }
        }
    }
}

fn construct_result_to_rebuild_result(
    state: ConvexHullConstructState,
    new_points: Vec<Point3>,
    result: ConvexFinalFacesConstructResult,
) -> Result<RebuildResult, ConvexHullConstructError> {
    match result {
        ConvexFinalFacesConstructResult::Success(value) => {
            Ok(RebuildResult::Rebuild(new_points, value, state))
        }
        ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(_)
        | ConvexFinalFacesConstructResult::PartialSuccess(_, _) => {
            debug_assert!(false);
            Err(ConvexHullConstructError::Degenerate)
        }
    }
}

pub struct ConvexFinalFaces {
    pub faces: FaceMap,
    pub edges: EdgeMap,
    visited_edges: EdgeIndexSmallVec,
    visited_faces: FaceIndexSmallVec,
    iter_count: usize,
    #[cfg(feature = "dump")]
    debug: DebugInfo,
}

impl ConvexFinalFaces {
    #[inline]
    fn new() -> Self {
        Self {
            faces: FaceMap::with_capacity(DEFAULT_FACE_CAPACITY),
            edges: EdgeMap::with_capacity(DEFAULT_EDGE_CAPACITY),
            visited_edges: EdgeIndexSmallVec::default(),
            visited_faces: FaceIndexSmallVec::default(),
            iter_count: 0,
            #[cfg(feature = "dump")]
            debug: DebugInfo::new(),
        }
    }

    fn calculate_aabb_points(points: &[Point3], base_epsilon: f32) -> Vec<Point3> {
        const THICKNESS_EPSILON: f32 = 1e-6;
        const MIN_SIZE: f32 = 0.01;
        let mut min = Vec3::splat(f32::MAX);
        let mut max = Vec3::splat(f32::MIN);
        let (min_valid_thickness, x_thickness, y_thickness, z_thickness) = if points.is_empty() {
            min = Vec3::splat(0.0);
            max = Vec3::splat(0.0);
            (None, 0_f32, 0_f32, 0_f32)
        } else {
            for point in points {
                min = min.min(point.as_vec3());
                max = max.max(point.as_vec3());
            }
            let x_thickness = max.x - min.x;
            let y_thickness = max.y - min.y;
            let z_thickness = max.z - min.z;
            let thickness = [x_thickness, y_thickness, z_thickness];
            let min_valid_thickness = thickness
                .into_iter()
                .filter(|&v| v > THICKNESS_EPSILON && v.is_finite())
                .min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Less));

            (min_valid_thickness, x_thickness, y_thickness, z_thickness)
        };
        let x_max_abs = max.x.absf().max(min.x.absf());
        let y_max_abs = max.y.absf().max(min.y.absf());
        let z_max_abs = max.z.absf().max(min.z.absf());
        let epsilon_scale = x_max_abs + y_max_abs + z_max_abs;
        let (_, _, min_init_distance) = Config::calculate_epsilon(epsilon_scale, base_epsilon);
        if let Some(min_valid_thickness) = min_valid_thickness {
            let min_init_distance = (min_valid_thickness * 0.1f32)
                .max(MIN_SIZE)
                .max(min_init_distance * 2f32);
            if x_thickness < min_init_distance {
                let expand = (min_init_distance - x_thickness) * 0.5;
                min.x -= expand;
                max.x += expand;
            }
            if y_thickness < min_init_distance {
                let expand = (min_init_distance - y_thickness) * 0.5;
                min.y -= expand;
                max.y += expand;
            }
            if z_thickness < min_init_distance {
                let expand = (min_init_distance - z_thickness) * 0.5;
                min.z -= expand;
                max.z += expand;
            }
        } else {
            let min_init_distance = if min_init_distance.is_finite() {
                MIN_SIZE.max(min_init_distance)
            } else {
                MIN_SIZE
            };
            let center = (min + max) * 0.5;
            min = center + Vec3::splat(-min_init_distance);
            max = center + Vec3::splat(min_init_distance);
        }

        let aabb_points = vec![
            Point3::new(min.x, min.y, min.z),
            Point3::new(max.x, min.y, min.z),
            Point3::new(min.x, max.y, min.z),
            Point3::new(max.x, max.y, min.z),
            Point3::new(min.x, min.y, max.z),
            Point3::new(max.x, min.y, max.z),
            Point3::new(min.x, max.y, max.z),
            Point3::new(max.x, max.y, max.z),
        ];
        aabb_points
    }

    //        b
    //       / \
    //      /   \
    //    a----- c
    //      \   /
    //       \ /
    //        d
    fn init_simplex(&mut self, points: &[Point3], simplex: SimplexIndex) {
        let (_face_abc, ab, bc, ca) =
            self.init_triangle_face(points, simplex.index_a, simplex.index_b, simplex.index_c);
        //abd
        let (_face_adb, ad, db, ba) =
            self.init_triangle_face(points, simplex.index_a, simplex.index_d, simplex.index_b);
        //cad
        let (_face_acd, ac, cd, da) =
            self.init_triangle_face(points, simplex.index_a, simplex.index_c, simplex.index_d);
        //bcd
        let (_face_cbd, cb, bd, dc) =
            self.init_triangle_face(points, simplex.index_c, simplex.index_b, simplex.index_d);
        //check the fourth point is always inside the plane
        #[cfg(feature = "safe_check")]
        #[allow(clippy::used_underscore_binding)]
        {
            let simplex_faces = SimplexFace::new(_face_abc, _face_adb, _face_acd, _face_cbd);
            ConvexFinalFaces::check_init_simplex_valid(
                &self.edges,
                &self.faces,
                points,
                simplex,
                simplex_faces,
            );
        }

        self.set_twin_with_check(ab, ba);
        self.set_twin_with_check(bc, cb);
        self.set_twin_with_check(ca, ac);
        self.set_twin_with_check(ad, da);
        self.set_twin_with_check(db, bd);
        self.set_twin_with_check(dc, cd);
        for (_, face) in self.faces.iter_mut() {
            face.update_normal_and_center(&self.edges, points);
        }
    }

    /// # Panics
    ///
    /// Panic if a face is not valid.
    /// A face is not valid if the edge list is not a circle, or edge count less than 3
    pub fn get_triangles(&self, triangles: &mut Vec<usize>) {
        for (_, face) in self.faces.iter() {
            face.get_face_triangles(&self.edges, triangles);
        }
    }

    fn init_triangle_face(
        &mut self,
        points: &[Point3],
        index_a: usize,
        index_b: usize,
        index_c: usize,
    ) -> (FaceKey, HalfEdgeKey, HalfEdgeKey, HalfEdgeKey) {
        let a = points[index_a];
        let b = points[index_b];
        let c = points[index_c];
        let ab = b - a;
        let ac = c - a;
        let normal = ac.cross(ab);
        let index_ab = self.edges.insert(HalfEdge::new(index_a, index_b));
        let index_bc = self.edges.insert(HalfEdge::new(index_b, index_c));
        let index_ca = self.edges.insert(HalfEdge::new(index_c, index_a));
        let center = (a.as_vec3() + b.as_vec3() + c.as_vec3()) / 3.0;
        let center = Point3::from_vec3(center);
        let (normal, area) = normal.normalize_to_unit_and_length();
        let face_index = self
            .faces
            .insert(Face::new(index_ab, center, normal, area, 3));
        let edge_ab = &mut self.edges[index_ab];
        edge_ab.set_next(index_bc);
        edge_ab.set_prev(index_ca);
        edge_ab.set_owner_face(face_index);
        let edge_bc = &mut self.edges[index_bc];
        edge_bc.set_next(index_ca);
        edge_bc.set_prev(index_ab);
        edge_bc.set_owner_face(face_index);
        let edge_ca = &mut self.edges[index_ca];
        edge_ca.set_next(index_ab);
        edge_ca.set_prev(index_bc);
        edge_ca.set_owner_face(face_index);
        (face_index, index_ab, index_bc, index_ca)
    }

    #[inline]
    fn init_conflict_points(&mut self, points: &[Point3], invalid: &SimplexIndex, epsilon: f32) {
        for (i, point) in points.iter().enumerate() {
            if invalid.contains(i) {
                continue;
            }
            let mut min_distance = f32::MAX;
            let mut min_face_index = None;
            for (face_index, face) in self.faces.iter_mut() {
                let distance = face.distance(*point);
                // ignore the point which is too close to the face
                if distance > epsilon && distance < min_distance {
                    min_distance = distance;
                    min_face_index = Some(face_index);
                }
            }
            if let Some(min_face_index) = min_face_index {
                self.faces[min_face_index].conflict_points.push(i);
            }
        }
    }

    #[inline]
    fn get_neighbour_face(&self, edge_index: HalfEdgeKey) -> (HalfEdgeKey, FaceKey, &Face) {
        let edge = self.edges.get(edge_index).expect("edge must exist");
        let twin_edge_key = edge.twin();
        let twin_edge = &self.edges[twin_edge_key];
        let twin_face = twin_edge.owner_face();
        (twin_edge_key, twin_face, &self.faces[twin_face])
    }

    #[inline]
    fn get_neighbour_face_mut(
        &mut self,
        edge_index: HalfEdgeKey,
    ) -> (HalfEdgeKey, FaceKey, &mut Face) {
        let edge = self.edges.get(edge_index).expect("edge must exist");
        let twin_edge_key = edge.twin();
        let twin_edge = &self.edges[twin_edge_key];
        let twin_face = twin_edge.owner_face();
        (twin_edge_key, twin_face, &mut self.faces[twin_face])
    }

    #[inline]
    fn set_edge_twin_visit(&mut self, key: HalfEdgeKey, value: bool) {
        let edge = &mut self.edges[key];
        edge.visited = value;
        let twin_edge_key = edge.twin();
        let twin_edge = &mut self.edges[twin_edge_key];
        twin_edge.visited = value;
    }

    #[inline]
    fn edge_visited(&self, key: HalfEdgeKey) -> bool {
        self.edges[key].visited
    }

    #[inline]
    fn next_edge(&self, key: HalfEdgeKey) -> HalfEdgeKey {
        self.edges[key].next()
    }

    #[inline]
    fn add_twin_edge_visited(&mut self, key: HalfEdgeKey) {
        self.visited_edges.push(key);
        let edge = &mut self.edges[key];
        edge.visited = true;
        let twin_edge = edge.twin();
        self.visited_edges.push(twin_edge);
        let twin_edge = &mut self.edges[twin_edge];
        twin_edge.visited = true;
    }

    fn get_horizon_edges(
        &mut self,
        owner_face_index: FaceKey,
        point_index: usize,
        points: &[Point3],
        epsilon: f32,
        enter_edge_key: HalfEdgeKey,
        horizon_edges: &mut EdgeIndexSmallVec,
    ) {
        let face = &mut self.faces[owner_face_index];
        if face.visited {
            return;
        }
        self.visited_faces.push(owner_face_index);
        face.visited = true;
        // go next face
        // foreach face's edge. if the twin's face is invisible, add the edge to the result, and go
        // to the next edge if the
        //  twin's face is visible, get_horizon_edges of the face
        let mut now_edge_index = enter_edge_key;
        let mut loop_end = false;
        while !loop_end {
            if !self.edge_visited(now_edge_index) {
                let (twin_edge_key, neighbour_face_index, neighbour_face) =
                    self.get_neighbour_face(now_edge_index);
                let neighbour_distance = neighbour_face.distance(points[point_index]);
                if neighbour_distance > epsilon {
                    // visible
                    self.add_twin_edge_visited(now_edge_index);
                    self.get_horizon_edges(
                        neighbour_face_index,
                        point_index,
                        points,
                        epsilon,
                        twin_edge_key,
                        horizon_edges,
                    );
                } else {
                    #[cfg(feature = "safe_check")]
                    {
                        let edge = &self.edges[now_edge_index];
                        assert!(!edge.visited);
                    }
                    horizon_edges.push(now_edge_index);
                }
            }
            let next_edge_index = self.next_edge(now_edge_index);
            loop_end = enter_edge_key == next_edge_index;
            now_edge_index = next_edge_index;
        }
    }

    #[inline]
    fn opp_face_distance(&self, edge_index: HalfEdgeKey) -> f32 {
        let (_, _, opp_face) = self.get_neighbour_face(edge_index);
        let edge = self.edges.get(edge_index).expect("edge must exist");
        let face = &self.faces[edge.owner_face()];
        face.distance(opp_face.center)
    }

    fn do_adjacent_merge(
        &mut self,
        face_key: FaceKey,
        merge_type: MergeType,
        epsilon: f32,
        points: &[Point3],
    ) -> Result<bool, ConvexHullConstructError> {
        let edge_iter = FaceEdgeIter::new(self.faces[face_key].edge_index, &self.edges);

        let mut convex = true;

        for (edge_index, _edge) in edge_iter {
            if self.visited_edges.contains(&edge_index) {
                return Err(ConvexHullConstructError::Degenerate);
            }
            let (opposite_edge_key, opp_face_key, opp_face) = self.get_neighbour_face(edge_index);
            if opp_face_key == face_key {
                continue;
            }
            let opp_face_area = opp_face.area;
            let face_area = self.faces[face_key].area;
            let max_area = opp_face_area.maxf(face_area);
            let merge_epsilon = cal_plane_merge_epsilon(max_area, epsilon);
            let mut merge = false;
            if merge_type == MergeType::NonConvex {
                // then merge faces if they are definitively non-convex
                merge = self.none_convex_should_merge(edge_index, opposite_edge_key, merge_epsilon);
            } else {
                // merge_type == `MergeType::NonConvexWrtLargerFace`
                // merge faces if they are parallel or non-convex
                // wrt to the larger face; otherwise, just mark
                // the face non-convex for the second pass.

                self.check_should_merge_larger_face(
                    &mut convex,
                    edge_index,
                    opposite_edge_key,
                    opp_face,
                    merge_epsilon,
                    &mut merge,
                );
            }
            #[cfg(feature = "dump")]
            {
                if merge {
                    let face1 = opp_face.get_face_vertex(&self.edges, true);
                    let edge = self.edges.get(edge_index).expect("edge must exist");
                    let face = &self.faces[edge.owner_face()];
                    let face0 = face.get_face_vertex(&self.edges, true);
                    self.debug
                        .current_frame_merge_face_checks
                        .push(MergeFaceCheck::new(face0, face1, merge));
                }
                self.dump_to_json_file();
            }
            if merge {
                if let Some(e) = self.merge_adjacent_face(face_key, edge_index, points) {
                    return Err(e);
                }

                #[cfg(feature = "safe_check")]
                self.check_twin_edge_valid(true);
                return Ok(true);
            }
        }

        if !convex {
            self.faces[face_key].mark = FaceState::NonConvex;
        }
        Ok(false)
    }

    fn none_convex_should_merge(
        &self,
        edge_index: HalfEdgeKey,
        opposite_edge_key: HalfEdgeKey,
        merge_epsilon: f32,
    ) -> bool {
        if self.opp_face_distance(edge_index) > -merge_epsilon
            || self.opp_face_distance(opposite_edge_key) > -merge_epsilon
        {
            return true;
        }
        false
    }

    fn check_should_merge_larger_face(
        &self,
        convex: &mut bool,
        edge_index: HalfEdgeKey,
        opposite_edge_key: HalfEdgeKey,
        opp_face: &Face,
        merge_epsilon: f32,
        merge: &mut bool,
    ) {
        let edge = self.edges.get(edge_index).expect("edge must exist");
        let face = &self.faces[edge.owner_face()];

        let face_area = face.area;
        let opp_face_area = opp_face.area;

        let (larger_edge_index, smaller_edge_index) = if face_area > opp_face_area {
            (edge_index, opposite_edge_key)
        } else {
            (opposite_edge_key, edge_index)
        };

        if self.opp_face_distance(larger_edge_index) > -merge_epsilon {
            *merge = true;
        } else if self.opp_face_distance(smaller_edge_index) > -merge_epsilon {
            *convex = false;
        }
    }

    // the focus thing here is to delete the opp face, and merge the uncommon edges，so the opp face
    // will be deleted if all the edges of opp face are common, connect the edges of the
    // face that has common point
    fn merge_adjacent_face(
        &mut self,
        face_key: FaceKey,
        hedge_adj: HalfEdgeKey,
        points: &[Point3],
    ) -> Option<ConvexHullConstructError> {
        let (hedge_opp, opp_face_key, opp_face) = self.get_neighbour_face_mut(hedge_adj);
        opp_face.mark = FaceState::Delete;
        self.visited_edges.push(hedge_adj);
        self.visited_edges.push(hedge_opp);
        self.visited_faces.push(opp_face_key);
        let hedge_adj_instance = &mut self.edges[hedge_adj];
        let mut hedge_adj_prev = hedge_adj_instance.prev();
        let mut hedge_adj_next = hedge_adj_instance.next();
        let hedge_opp_instance = &mut self.edges[hedge_opp];
        let mut hedge_opp_prev = hedge_opp_instance.prev();
        let mut hedge_opp_next = hedge_opp_instance.next();
        let mut contains_full_opp_face = false;
        let first_hedge_adj_prev = hedge_adj_prev;
        let (_, mut test_face_key, _) = self.get_neighbour_face(hedge_adj_prev);
        while test_face_key == opp_face_key {
            self.visited_edges.push(hedge_adj_prev);
            self.visited_edges.push(hedge_opp_next);
            let hedge_adj_prev_instance = &self.edges[hedge_adj_prev];
            let hedge_pre_pre = hedge_adj_prev_instance.prev();
            let hedge_opp_next_instance = &self.edges[hedge_opp_next];
            let hedge_opp_next_next = hedge_opp_next_instance.next();
            //in some case,the face has duplicate point,and the opposite face's edge can be all
            // contain in the face,so here can have a loop in the opposite face. so we need
            // to check if the `hedge_opp_next_next` == `hedge_opp`,if true,we stop the while
            // loop,and let hedge_opp_next stay on the last edge,and hedge_adj_prev
            // continue advance
            hedge_adj_prev = hedge_pre_pre;
            if hedge_opp_next_next == hedge_opp {
                contains_full_opp_face = true;
                break;
            }
            hedge_opp_next = hedge_opp_next_next;

            if first_hedge_adj_prev == hedge_adj_prev {
                // two faces have the same edges,that's happening in large plane_epsilon and small
                // mesh. just return error in this case
                return Some(ConvexHullConstructError::Degenerate);
            }
            (_, test_face_key, _) = self.get_neighbour_face(hedge_adj_prev);
        }
        if contains_full_opp_face {
            let face = &mut self.faces[face_key];
            if hedge_adj == face.edge_index {
                face.edge_index = hedge_adj_next;
            }
        }

        let first_hedge_opp_prev = hedge_opp_prev;
        let (_, mut test_face_key, _test_face) = self.get_neighbour_face(hedge_adj_next);
        let mut _test_face2;
        while test_face_key == opp_face_key {
            self.visited_edges.push(hedge_opp_prev);
            self.visited_edges.push(hedge_adj_next);
            let hedge_opp_prev_instance = &self.edges[hedge_opp_prev];
            let hedge_opp_prev_prev = hedge_opp_prev_instance.prev();
            let hedge_adj_next_instance = &self.edges[hedge_adj_next];
            let hedge_adj_next_next = hedge_adj_next_instance.next();
            // in some case,the face has duplicate points, and the opposite face's edge can be all
            // contain in the face,so here can have a loop in the opposite face. so we need
            // to check if the `hedge_opp_prev` == `hedge_opp`,if true,we stop the while loop,and
            // let hedge_opp_prev stay on the last edge,and hedge_adj_next continue
            // advance

            hedge_adj_next = hedge_adj_next_next;
            if hedge_opp_prev_prev == hedge_opp {
                contains_full_opp_face = true;
                break;
            }

            hedge_opp_prev = hedge_opp_prev_prev;

            if first_hedge_opp_prev == hedge_opp_prev {
                // two faces have the same edges,that's happening in large plane_epsilon and small
                // mesh. just return error in this case
                return Some(ConvexHullConstructError::Degenerate);
            }
            (_, test_face_key, _test_face2) = self.get_neighbour_face(hedge_adj_next);
        }

        if contains_full_opp_face {
            let result =
                self.connect_half_edges(face_key, &mut hedge_adj_prev, &mut hedge_adj_next, points);
            debug_assert!(result.discard_face.is_none());
            debug_assert!(!result.get_ride_of_pre_edge);

            let edge_iter = FaceEdgeIter::new(hedge_opp, &self.edges);
            for (key, _) in edge_iter {
                self.visited_edges.push(key);
            }
            let face = &mut self.faces[face_key];
            if hedge_adj == face.edge_index {
                face.edge_index = hedge_adj_next;
            }
        } else {
            let mut opp_edges = SmallVec::<[HalfEdgeKey; 16]>::new();
            self.set_owner_face(face_key, hedge_opp_prev, hedge_opp_next, &mut opp_edges);
            let face = &mut self.faces[face_key];
            if hedge_adj == face.edge_index {
                face.edge_index = hedge_adj_next;
            }
            // handle the half-edges at the head
            let result =
                self.connect_half_edges(face_key, &mut hedge_opp_prev, &mut hedge_adj_next, points);
            if let Some(discarded_face) = result.discard_face {
                assert_ne!(face_key, discarded_face);
                self.visited_faces.push(discarded_face);
            }
            if result.connect_edge_change {
                let face = &mut self.faces[face_key];
                face.edge_index = hedge_adj_next;
            }
            // if the variable get_ride_of_pre_edge is true, and hedge_adj_next changed. it means
            // the owner face of hedge_adj_next.opposite.prev is face_key,and it has been
            // deleted. so we need to update the edge index of face_key
            if result.get_ride_of_pre_edge && hedge_opp_next == hedge_opp_prev {
                hedge_opp_next = hedge_adj_next;
                let opp_next_owner_face = self.edges[hedge_opp_next].owner_face();
                // self connect
                if !opp_edges.contains(&hedge_opp_next) && opp_next_owner_face == face_key {
                    opp_edges.iter().for_each(|&key| {
                        self.visited_edges.push(key);
                    });
                }
            }
            // handle the half-edges at the tail
            let result =
                self.connect_half_edges(face_key, &mut hedge_adj_prev, &mut hedge_opp_next, points);
            if let Some(discarded_face) = result.discard_face {
                assert_ne!(face_key, discarded_face);
                self.visited_faces.push(discarded_face);
            }
            if result.connect_edge_change {
                let face = &mut self.faces[face_key];
                face.edge_index = hedge_opp_next;
            }
        }

        let face = &mut self.faces[face_key];
        face.update_normal_and_center(&self.edges, points);
        None
    }

    fn set_owner_face(
        &mut self,
        face_key: FaceKey,
        hedge_opp_prev: HalfEdgeKey,
        hedge_opp_next: HalfEdgeKey,
        opp_edges: &mut SmallVec<[HalfEdgeKey; 16]>,
    ) {
        let mut hedge = hedge_opp_next;
        opp_edges.push(hedge);
        let hedge_opp_prev_next = self.edges[hedge_opp_prev].next();
        while hedge != hedge_opp_prev_next {
            let hedge_instance = &mut self.edges[hedge];
            #[cfg(test)]
            let _owner_face = &self.faces[hedge_instance.owner_face()];
            hedge_instance.set_owner_face(face_key);
            hedge = hedge_instance.next();
            opp_edges.push(hedge);
        }
    }

    fn connect_half_edges(
        &mut self,
        face_key: FaceKey,
        hedge_prev: &mut HalfEdgeKey,
        hedge: &mut HalfEdgeKey,
        points: &[Point3],
    ) -> ConnectEdgeResult {
        let (_hedge_opposite, hedge_opp_face_key, _hedge_opp_face) =
            self.get_neighbour_face_mut(*hedge);
        let (_hedge_prev_opposite, hedge_prev_opp_face_key, _hedge_prev_opp_face) =
            self.get_neighbour_face_mut(*hedge_prev);
        let mut result = ConnectEdgeResult::default();
        let mut same_opp_face = hedge_prev_opp_face_key == hedge_opp_face_key;
        let mut same_face = hedge_prev_opp_face_key == face_key;
        if same_opp_face && !same_face {
            // then there is a redundant edge that we can get rid of.
            let face = &mut self.faces[face_key];

            let mut hedge_opp;

            if *hedge_prev == face.edge_index {
                face.edge_index = *hedge;
            }
            let (hedge_opposite, opp_face_key, opp_face) = self.get_neighbour_face_mut(*hedge);
            let opp_face_num_vertex = opp_face.num_points;
            let hedge_opposite_instance = &self.edges[hedge_opposite];
            let mut new_opp_face_is_face_self = false;
            self.visited_edges.push(*hedge_prev);
            let hedge_prev_instance = &self.edges[*hedge_prev];
            let hedge_prev_prev = hedge_prev_instance.prev();
            if opp_face_num_vertex == 3 {
                // then we can get rid of the opposite face altogether
                let hedge_opposite_prev_edge_instance = &self.edges[hedge_opposite_instance.prev()];
                hedge_opp = hedge_opposite_prev_edge_instance.twin();
                let hedge_opp_instance = &self.edges[hedge_opp];
                if hedge_opp_instance.owner_face() == face_key {
                    *hedge = hedge_opp_instance.next();
                    hedge_opp = self.edges[*hedge].twin();
                    new_opp_face_is_face_self = true;
                    result.connect_edge_change = true;
                }
                let opp_face_instance = &mut self.faces[opp_face_key];
                let face_edge_iter = FaceEdgeIter::new(hedge_opposite, &self.edges);
                for (key, _) in face_edge_iter {
                    if new_opp_face_is_face_self {
                        if key != hedge_prev_prev {
                            self.visited_edges.push(key);
                        }
                    } else {
                        self.visited_edges.push(key);
                    }
                }
                opp_face_instance.mark = FaceState::Delete;
                result.discard_face = Some(opp_face_key);
            } else {
                hedge_opp = hedge_opposite_instance.next();
                self.visited_edges.push(hedge_opposite);
                let hedge_opp_instance_prev_edge = self.edges[hedge_opp].prev();
                let opp_face_instance = &mut self.faces[opp_face_key];
                if opp_face_instance.edge_index == hedge_opp_instance_prev_edge {
                    opp_face_instance.edge_index = hedge_opp;
                }
                let hedge_opp_prev_instance = &self.edges[hedge_opp_instance_prev_edge];
                let hedge_opp_prev_prev = hedge_opp_prev_instance.prev();
                let hedge_opp_prev_prev_destination = self.edges[hedge_opp_prev_prev].destination;
                let hedge_opp_instance = &mut self.edges[hedge_opp];
                hedge_opp_instance.set_prev(hedge_opp_prev_prev);
                hedge_opp_instance.origin = hedge_opp_prev_prev_destination;
                self.edges[hedge_opp_prev_prev].set_next(hedge_opp);
            }
            result.get_ride_of_pre_edge = true;

            let hedge_prev_prev_destination = self.edges[hedge_prev_prev].destination;
            let hedge_instance = &mut self.edges[*hedge];
            hedge_instance.origin = hedge_prev_prev_destination;
            hedge_instance.set_prev(hedge_prev_prev);
            hedge_instance.set_twin(hedge_opp);
            let hedge_prev_instance = &mut self.edges[hedge_prev_prev];
            hedge_prev_instance.set_next(*hedge);
            let hedge_opp_instance = &mut self.edges[hedge_opp];
            hedge_opp_instance.set_twin(*hedge);

            // opp_face was modified, so need to recompute normal and center
            let opp_face_instance = &mut self.faces[opp_face_key];
            opp_face_instance.update_normal_and_center(&self.edges, points);
        } else {
            while same_opp_face && same_face {
                *hedge_prev = self.edges[*hedge_prev].prev();
                *hedge = self.edges[*hedge].next();
                result.connect_edge_change = true;
                let (_hedge_opposite, hedge_opp_face_key, _hedge_opp_face) =
                    self.get_neighbour_face_mut(*hedge);
                let (_hedge_prev_opposite, hedge_prev_opp_face_key, _hedge_prev_opp_face) =
                    self.get_neighbour_face_mut(*hedge_prev);
                same_opp_face = hedge_prev_opp_face_key == hedge_opp_face_key;
                same_face = hedge_prev_opp_face_key == face_key;
            }
            self.edges[*hedge_prev].set_next(*hedge);
            self.edges[*hedge].set_prev(*hedge_prev);
        }
        result
    }

    #[inline]
    fn remove_faces(&mut self) {
        for face in &self.visited_faces {
            self.faces.remove(*face);
        }
    }

    #[inline]
    fn remove_edges(&mut self) {
        for edge in &self.visited_edges {
            self.edges.remove(*edge);
        }
    }

    #[inline]
    fn set_twin_with_check(&mut self, edge_index: HalfEdgeKey, twin_index: HalfEdgeKey) {
        let edge = &mut self.edges[edge_index];
        edge.set_twin(twin_index);
        let edge_origin = edge.origin;
        let edge_destination = edge.destination;
        let twin_edge = &mut self.edges[twin_index];
        twin_edge.set_twin(edge_index);
        debug_assert_eq!(twin_edge.origin, edge_destination);
        debug_assert_eq!(twin_edge.destination, edge_origin);
    }

    fn connect_new_faces(
        &mut self,
        point_index: usize,
        horizon_edges: &EdgeIndexSmallVec,
        new_faces: &mut FaceIndexSmallVec,
        new_edges: &mut EdgeIndexSmallVec,
        points: &[Point3],
    ) {
        // horizon_edges has already in ccw order,that's great!
        let iter = &mut horizon_edges.iter();
        let mut first_iter = iter.take(1);
        let first_horizon_edge = *first_iter.next().expect("horizon_edges must have edge");
        self.set_edge_twin_visit(first_horizon_edge, false);
        let first_face_result = self.create_triangle_face(point_index, points, first_horizon_edge);
        new_edges.push(first_face_result.edge_ab);
        new_edges.push(first_face_result.edge_ca);
        new_faces.push(first_face_result.face_index);
        let mut last_edge0_index = first_face_result.edge_ab;
        let mut last_edge1_index = first_face_result.edge_ca;
        for &edge_key in iter {
            //link the twin edge to the new triangle, the new triangle's edge0 is twin
            // with the edge1 in the last iteration.
            self.set_edge_twin_visit(edge_key, false);
            let result = self.create_triangle_face(point_index, points, edge_key);
            new_faces.push(result.face_index);
            self.set_twin_with_check(result.edge_ab, last_edge1_index);
            new_edges.push(result.edge_ab);
            new_edges.push(result.edge_ca);
            last_edge0_index = result.edge_ab;
            last_edge1_index = result.edge_ca;
        }
        assert_ne!(last_edge0_index, first_face_result.edge_ab);
        self.set_twin_with_check(last_edge1_index, first_face_result.edge_ab);
    }

    fn create_triangle_face(
        &mut self,
        point_index: usize,
        points: &[Point3],
        edge_key: HalfEdgeKey,
    ) -> CreateTriangleFaceResult {
        let edge = &self.edges[edge_key];
        let origin = edge.origin;
        let destination = edge.destination;
        let index_a = point_index;
        let index_b = origin;
        let index_c = destination;
        let a = points[index_a];
        let b = points[index_b];
        let c = points[index_c];
        // if a,b,c is collinear, let the normal be unit x-axis,this will merge after create new
        // faces
        let un_normalized_normal = (c - a).cross(b - a);
        let (normal, area) = normalize_and_len(un_normalized_normal, 0.0);
        let edge_ab_index = self.edges.insert(HalfEdge::new(index_a, index_b));
        let edge_ca_index = self.edges.insert(HalfEdge::new(index_c, index_a));
        let center = (a.as_vec3() + b.as_vec3() + c.as_vec3()) / 3.0;
        let center = Point3::from_vec3(center);
        let face = Face::new(edge_key, center, normal, area, 3);
        let face_key = self.faces.insert(face);
        let ab = &mut self.edges[edge_ab_index];
        ab.set_owner_face(face_key);
        ab.set_prev(edge_ca_index);
        ab.set_next(edge_key);

        let ca = &mut self.edges[edge_ca_index];
        ca.set_owner_face(face_key);
        ca.set_prev(edge_key);
        ca.set_next(edge_ab_index);
        let edge = &mut self.edges[edge_key];
        edge.set_owner_face(face_key);
        edge.set_prev(edge_ab_index);
        edge.set_next(edge_ca_index);
        self.faces[face_key].update_normal_and_center(&self.edges, points);
        CreateTriangleFaceResult::new(face_key, edge_ab_index, edge_ca_index)
    }

    fn update(
        &mut self,
        points: &[Point3],
        max_face_count: u32,
        horizon_epsilon: f32,
        base_epsilon: f32,
        conflict_point_epsilon: f32,
        _merge_epsilon: f32,
    ) -> Result<ConvexHullConstructState, ConvexHullConstructError> {
        #[cfg(feature = "dump")]
        {
            self.debug.points.append(&mut points.to_vec());
            self.collect_debug_info();
            self.dump_to_json_file();
        }
        #[cfg(feature = "safe_check")]
        self.check_every_face_edge_list_is_circle(true);

        let mut has_conflict_point_face = self.faces.iter_mut().find(
            #[inline]
            |(_key, face)| !face.conflict_points.is_empty(),
        );

        let mut horizon_edges = EdgeIndexSmallVec::new();
        let mut new_edges = EdgeIndexSmallVec::new();
        let mut reschedule_points = RescheduleConflictPointIndexSmallVec::new();
        let mut new_faces = FaceIndexSmallVec::new();

        while let Some((face_key, face)) = has_conflict_point_face {
            #[cfg(feature = "safe_check")]
            assert!(face.check_edge_list_is_circle(&self.edges));
            let face_edge_index = face.edge_index;
            let conflict_point_index = *face
                .get_max_distance_conflict_index(points)
                .expect("index must exist");
            face.remove_conflict_point(conflict_point_index);
            horizon_edges.clear();
            self.visited_faces.clear();
            self.visited_edges.clear();
            new_edges.clear();
            reschedule_points.clear();
            new_faces.clear();
            self.get_horizon_edges(
                face_key,
                conflict_point_index,
                points,
                horizon_epsilon,
                face_edge_index,
                &mut horizon_edges,
            );
            if self.faces.len() + horizon_edges.len() > max_face_count as usize {
                return Ok(FaceCountLimitHit);
            }

            #[cfg(feature = "safe_check")]
            if !self.horizon_edges_circle_check(&horizon_edges) {
                #[cfg(feature = "dump")]
                {
                    self.collect_horizons(&horizon_edges);
                    self.dump_to_json_file();
                }
                panic!("horizon_edges_circle_check failed")
            }
            self.connect_new_faces(
                conflict_point_index,
                &horizon_edges,
                &mut new_faces,
                &mut new_edges,
                points,
            );
            #[cfg(feature = "safe_check")]
            {
                self.check_every_face_edge_list_is_circle(true);
                self.check_twin_edge_valid(false);
                #[cfg(feature = "dump")]
                self.debug.current_frame_merge_face_checks.clear();
            }

            if let Some(value) = self.merge_face(
                points,
                FaceState::Visible,
                MergeType::NonConvexWrtLargerFace,
                base_epsilon,
                &new_faces,
            ) {
                return Err(value);
            }

            // second merge pass ... merge faces which are non-convex
            // wrt either face
            if let Some(value) = self.merge_face(
                points,
                FaceState::NonConvex,
                MergeType::NonConvex,
                base_epsilon,
                &new_faces,
            ) {
                return Err(value);
            }
            self.collect_need_reschedule_points(&mut reschedule_points);
            self.reschedule_conflict_points(
                &reschedule_points,
                points,
                &new_faces,
                conflict_point_epsilon,
            );
            #[cfg(feature = "safe_check")]
            {
                self.check_twin_edge_valid(false);
                self.check_every_face_edge_list_is_circle(true);
            }
            self.remove_faces();
            self.remove_edges();

            #[cfg(feature = "safe_check")]
            self.check_twin_edge_valid(false);

            if self.faces.len() < 4 {
                return Err(ConvexHullConstructError::Degenerate);
            }
            self.iter_count += 1;

            #[cfg(feature = "safe_check")]
            {
                self.check_twin_edge_valid(false);
                #[cfg(feature = "dump")]
                self.collect_debug_info();
            }

            #[cfg(feature = "safe_check")]
            {
                #[allow(clippy::used_underscore_binding)]
                if !self.check_every_face_vertex_is_inside(points, _merge_epsilon) {
                    #[cfg(feature = "dump")]
                    self.dump_to_json_file();
                    panic!("check_every_face_vertex_is_inside check failed")
                }
            }
            has_conflict_point_face = self
                .faces
                .iter_mut()
                .find(|(_key, face)| !face.conflict_points.is_empty());
        }
        #[cfg(feature = "dump")]
        self.dump_to_json_file();
        Ok(Success)
    }

    #[inline(always)]
    fn merge_face(
        &mut self,
        points: &[Point3],
        mark_filter: FaceState,
        merge_type: MergeType,
        base_epsilon: f32,
        new_faces: &FaceIndexSmallVec,
    ) -> Option<ConvexHullConstructError> {
        for &new_face_key in new_faces {
            let new_face = &self.faces[new_face_key];
            if new_face.mark == mark_filter {
                let mut need_continue_merge = true;

                while need_continue_merge {
                    let result =
                        self.do_adjacent_merge(new_face_key, merge_type, base_epsilon, points);
                    match result {
                        Ok(b) => {
                            need_continue_merge = b;
                        }
                        Err(e) => {
                            return Some(e);
                        }
                    }
                    #[cfg(feature = "dump")]
                    {
                        self.dump_to_json_file();
                    }
                    #[cfg(feature = "safe_check")]
                    {
                        self.check_every_face_edge_list_is_circle(false);
                        self.check_twin_edge_valid(true);
                    }
                }
            }
        }
        None
    }

    fn collect_need_reschedule_points(
        &mut self,
        out_put_points: &mut RescheduleConflictPointIndexSmallVec,
    ) {
        for face_key in &self.visited_faces {
            let face = &mut self.faces[*face_key];
            for conflict_point in &face.conflict_points {
                out_put_points.push(*conflict_point);
            }
            face.conflict_points.clear();
        }
    }

    fn reschedule_conflict_points(
        &mut self,
        reschedule_points: &RescheduleConflictPointIndexSmallVec,
        points: &[Point3],
        new_faces: &FaceIndexSmallVec,
        epsilon: f32,
    ) {
        for &conflict_point_index in reschedule_points {
            let mut min_distance = f32::MAX;
            let mut min_face_index = None;
            for &new_face_key in new_faces {
                let new_face = &self.faces[new_face_key];
                if new_face.mark == FaceState::Delete {
                    continue;
                }
                let distance = new_face.distance(points[conflict_point_index]);
                if distance > epsilon && distance < min_distance {
                    min_distance = distance;
                    min_face_index = Some(new_face_key);
                }
            }
            if let Some(min_face_index) = min_face_index {
                self.faces[min_face_index]
                    .conflict_points
                    .push(conflict_point_index);
            }
        }
    }
}

#[inline]
fn try_inverse_mat3(mat: &Mat3) -> Option<Mat3> {
    let det = mat.determinant();
    if det.abs() < 1e-6 {
        return None;
    }
    Some(mat.inverse())
}
fn to_plane_equation(v: &Plane) -> (f32, f32, f32, f32) {
    let normal = v.normal;

    // plane function: ax + by + cz = d
    let d = v.offset;
    // a,b,c,d
    (normal.x, normal.y, normal.z, d)
}
fn find_intersection(planes: &[Plane; 3]) -> Option<Vec3> {
    let mut matrix = Mat3::default();
    let mut rhs = Vec3::ZERO;

    let mut rows = [Vec3::ZERO; 3];
    for (i, plane) in planes.iter().enumerate() {
        let (a, b, c, d) = to_plane_equation(plane);

        rows[i] = Vec3::new(a, b, c);
        rhs[i] = d;
    }
    matrix.x_axis = Vec3::new(rows[0].x, rows[1].x, rows[2].x);
    matrix.y_axis = Vec3::new(rows[0].y, rows[1].y, rows[2].y);
    matrix.z_axis = Vec3::new(rows[0].z, rows[1].z, rows[2].z);

    try_inverse_mat3(&matrix).map(|inv_matrix| inv_matrix * rhs)
}
/// 1.iterate the faces get all the points that are on the convex hull
/// 2.iterate all the points and update the every face's `min_distance_for_expand` that should have
/// on face's normal
/// 3.iterate all the points on the convex hull , get the near three faces,calculate the expanded
/// three plane and their intersection point,add these points to a new vector
/// 4.construct the new convex hull with the new points
fn expand_convex_hull(points: &[Point3], mut convex_final_faces: ConvexFinalFaces) -> Vec<Point3> {
    struct OnFacePoint {
        point_index: usize,
        face_indices: [Option<FaceKey>; 3],
    }

    fn can_push_point(points: &PointSmallVec<OnFacePoint>, index: usize) -> bool {
        points.iter().all(|point| point.point_index != index)
    }

    // 1
    let mut points_on_face = SmallVec::<[OnFacePoint; 256]>::new();
    for (_, face) in convex_final_faces.faces.iter() {
        let edge_iter = FaceEdgeIter::new(face.edge_index, &convex_final_faces.edges);
        for (_, edge) in edge_iter {
            let point_index = edge.origin;
            if !can_push_point(&points_on_face, point_index) {
                continue;
            }
            let mut on_face_point = OnFacePoint {
                point_index: edge.origin,
                face_indices: [None; 3],
            };
            on_face_point.face_indices[0] = Some(edge.owner_face());
            on_face_point.face_indices[1] =
                Some(convex_final_faces.edges[edge.twin()].owner_face());
            let prev_edge = &convex_final_faces.edges[edge.prev()];
            let prev_twin_edge = &convex_final_faces.edges[prev_edge.twin()];
            on_face_point.face_indices[2] = Some(prev_twin_edge.owner_face());
            points_on_face.push(on_face_point);
        }
    }
    // 2
    for point in points {
        for (_, face) in convex_final_faces.faces.iter_mut() {
            let distance = face.distance(*point);
            if distance < 0.0 {
                continue;
            }
            if distance > face.max_distance_for_expand {
                face.max_distance_for_expand = distance;
            }
        }
    }
    let mut new_points = Vec::<Point3>::new();
    // 3
    for point in points_on_face {
        let face0 = &convex_final_faces.faces[point.face_indices[0].unwrap()];
        let face1 = &convex_final_faces.faces[point.face_indices[1].unwrap()];
        let face2 = &convex_final_faces.faces[point.face_indices[2].unwrap()];
        let plane0 = new_plane(face0);
        let plane1 = new_plane(face1);
        let plane2 = new_plane(face2);
        let planes = [plane0, plane1, plane2];
        if let Some(intersection) = find_intersection(&planes) {
            new_points.push(Point3::from_vec3(intersection));
        }
    }
    new_points
}

fn new_plane(face0: &Face) -> Plane {
    Plane {
        offset: face0.center.as_vec3().dot(face0.normal) + face0.max_distance_for_expand,
        normal: face0.normal,
    }
}

#[cfg(feature = "safe_check")]
mod convex_hull_safe_check {
    use glam_det::{Dot, Point3};
    use rustc_hash::FxHashSet;

    use super::{
        ConvexFinalFaces, EdgeIndexSmallVec, EdgeMap, Face, FaceKey, FaceMap, Plane, SimplexIndex,
        MAX_DEBUG_ITER_COUNT,
    };
    use crate::face::FaceEdgeIter;

    #[derive(Copy, Clone)]
    pub(crate) struct SimplexFace {
        pub face_abc: FaceKey,
        pub face_adb: FaceKey,
        pub face_acd: FaceKey,
        pub face_cbd: FaceKey,
    }
    impl SimplexFace {
        #[inline]
        pub fn new(
            face_abc: FaceKey,
            face_adb: FaceKey,
            face_acd: FaceKey,
            face_cbd: FaceKey,
        ) -> Self {
            Self {
                face_abc,
                face_adb,
                face_acd,
                face_cbd,
            }
        }
    }
    impl ConvexFinalFaces {
        #[inline]
        pub(crate) fn horizon_edges_circle_check(
            &mut self,
            horizon_edges: &EdgeIndexSmallVec,
        ) -> bool {
            if horizon_edges.len() < 3 {
                return false;
            }
            let mut pre_edge = horizon_edges[0];

            for i in 1..horizon_edges.len() {
                let pre_edge_instance = &self.edges[pre_edge];
                let next_edge = horizon_edges[i];
                let next_edge_instance = &self.edges[next_edge];

                if next_edge_instance.origin != pre_edge_instance.destination {
                    return false;
                }
                pre_edge = next_edge;
            }
            let pre_edge_instance = &self.edges[pre_edge];
            let next_edge = horizon_edges[0];
            let next_edge_instance = &self.edges[next_edge];
            if next_edge_instance.origin != pre_edge_instance.destination {
                return false;
            }
            true
        }

        pub(crate) fn check_every_face_vertex_is_inside(
            &self,
            points: &[Point3],
            epsilon: f32,
        ) -> bool {
            #[allow(clippy::absurd_extreme_comparisons)]
            if self.iter_count < MAX_DEBUG_ITER_COUNT {
                return true;
            }
            let epsilon = epsilon * 5.0f32;
            for (key0, face) in self.faces.iter() {
                let vertices = face.get_face_vertex(&self.edges, true);
                for vertex in vertices {
                    let point = points[vertex];
                    for (key1, face) in self.faces.iter() {
                        if key0 == key1 {
                            continue;
                        }
                        let vertices = face.get_face_vertex(&self.edges, true);
                        if vertices.contains(&vertex) {
                            continue;
                        }
                        let normal = face.normal;
                        let distance = (point - face.center).dot(normal);
                        if distance > epsilon {
                            println!("check_every_face_vertex_is_inside");
                            println!("point:{vertex:?}");
                            println!("face:{vertices:?} distance:{distance}");
                            return false;
                        }
                    }
                }
            }
            true
        }

        pub(crate) fn check_every_face_edge_list_is_circle(&self, check_duplicate_point: bool) {
            for (face_key, face) in self.faces.iter() {
                if self.visited_faces.contains(&face_key) {
                    continue;
                }
                self.check_face_edge_list_is_circle(face, check_duplicate_point);
            }
        }

        pub(crate) fn check_face_edge_list_is_circle(
            &self,
            face: &Face,
            check_duplicate_point: bool,
        ) {
            #[allow(clippy::absurd_extreme_comparisons)]
            if self.iter_count < MAX_DEBUG_ITER_COUNT {
                return;
            }
            let mut hash_set = FxHashSet::default();
            let edge_index = face.edge_index;
            let edge = &self.edges[edge_index];
            if check_duplicate_point {
                hash_set.insert(edge.origin);
            }
            let mut now_edge_index = edge.next();
            let mut is_circle = false;
            for _i in 0..1024 {
                let edge = &self.edges[now_edge_index];
                let pre_key = edge.prev();
                let pre_edge = &self.edges[pre_key];
                now_edge_index = edge.next();
                let next_edge = &self.edges[now_edge_index];
                assert_eq!(edge.destination, next_edge.origin);
                assert_eq!(pre_edge.destination, edge.origin);
                if now_edge_index == edge_index {
                    is_circle = true;
                    break;
                }
                if check_duplicate_point {
                    assert!(
                        !hash_set.contains(&edge.origin),
                        "edge duplicate point check failed"
                    );
                    hash_set.insert(edge.origin);
                }
            }
            assert!(
                is_circle,
                "edge list is not circle or the edge list is more than 1024"
            );
        }

        #[inline]
        pub(crate) fn check_twin_edge_valid(&self, ignore_twin_face_owner_check: bool) {
            #[allow(clippy::absurd_extreme_comparisons)]
            if self.iter_count < MAX_DEBUG_ITER_COUNT {
                return;
            }
            for (face_key, face) in self.faces.iter() {
                if self.visited_faces.contains(&face_key) {
                    continue;
                }
                for (edge_index, edge) in FaceEdgeIter::new(face.edge_index, &self.edges) {
                    assert!(
                        !self.visited_edges.contains(&edge_index),
                        "edge has been visited"
                    );
                    let edge_owner_face = self.faces.get(edge.owner_face());
                    debug_assert!(edge_owner_face.is_some());
                    let twin_edge = edge.twin();
                    assert!(
                        !self.visited_edges.contains(&twin_edge),
                        "twin_edge has been visited"
                    );
                    let twin_edge = &self.edges[twin_edge];
                    let twin_edge_owner_face_key = twin_edge.owner_face();
                    debug_assert!(!self.visited_faces.contains(&twin_edge_owner_face_key));
                    debug_assert!(self.faces.get(twin_edge_owner_face_key).is_some());
                    debug_assert_eq!(twin_edge.twin(), edge_index);
                    debug_assert_eq!(twin_edge.origin, edge.destination);
                    debug_assert_eq!(twin_edge.destination, edge.origin);
                    // if there are some faces on the same plane, that means have to merge them in a
                    // sequence, that can let the owner face be same.
                    if !ignore_twin_face_owner_check {
                        debug_assert_ne!(twin_edge.owner_face(), edge.owner_face());
                    }
                }
            }
        }

        pub(crate) fn check_init_simplex_valid(
            edges: &EdgeMap,
            faces: &FaceMap,
            points: &[Point3],
            simplex: SimplexIndex,
            simplex_faces: SimplexFace,
        ) {
            {
                let face = &faces[simplex_faces.face_abc];
                let point = points[face.first_vertex_index(edges)];
                let plane = Plane {
                    offset: point.as_vec3().dot(face.normal),
                    normal: face.normal,
                };
                assert!(plane.distance(points[simplex.index_d]) < 0.0);
            }
            {
                let face = &faces[simplex_faces.face_adb];
                let point = points[face.first_vertex_index(edges)];
                let plane = Plane {
                    offset: point.as_vec3().dot(face.normal),
                    normal: face.normal,
                };
                assert!(plane.distance(points[simplex.index_c]) < 0.0);
            }
            {
                let face = &faces[simplex_faces.face_acd];
                let point = points[face.first_vertex_index(edges)];
                let plane = Plane {
                    offset: point.as_vec3().dot(face.normal),
                    normal: face.normal,
                };
                assert!(plane.distance(points[simplex.index_b]) < 0.0);
            }
            {
                let face = &faces[simplex_faces.face_cbd];
                let point = points[face.first_vertex_index(edges)];
                let plane = Plane {
                    offset: point.as_vec3().dot(face.normal),
                    normal: face.normal,
                };
                assert!(plane.distance(points[simplex.index_a]) < 0.0);
            }
        }
    }
    impl Face {
        pub(crate) fn check_edge_list_is_circle(&self, edges: &EdgeMap) -> bool {
            let mut now_index = self.edge_index;
            let max_iter = 1000;
            for _ in 0..max_iter {
                let next = edges[now_index].next();
                now_index = next;
                if next == self.edge_index {
                    return true;
                }
            }
            false
        }
    }
}

#[cfg(feature = "dump")]
mod quick_hull_dump {
    use glam_det::{Point3, Vec3};
    use serde::{Deserialize, Serialize};

    #[cfg(feature = "safe_check")]
    use super::EdgeIndexSmallVec;
    use super::{ConvexFinalFaces, MAX_DEBUG_ITER_COUNT};
    impl ConvexFinalFaces {
        #[cfg(feature = "safe_check")]
        pub(crate) fn collect_horizons(&mut self, horizon_edges: &EdgeIndexSmallVec) {
            #[allow(clippy::absurd_extreme_comparisons)]
            if self.iter_count < MAX_DEBUG_ITER_COUNT {
                return;
            }
            self.debug.current_horizon_lines.clear();
            for &edge in horizon_edges {
                let edge = &self.edges[edge];
                self.debug.current_horizon_lines.push(edge.origin);
                self.debug.current_horizon_lines.push(edge.destination);
            }
        }

        pub(crate) fn dump_to_json_file(&self) {
            #[allow(clippy::absurd_extreme_comparisons)]
            if self.iter_count < MAX_DEBUG_ITER_COUNT {
                return;
            }
            let debug = &self.debug;
            let str = serde_json::to_string(debug).unwrap();

            let _ = std::fs::write("./dump.json", str);
        }

        pub(crate) fn collect_debug_info(&mut self) {
            #[allow(clippy::absurd_extreme_comparisons)]
            if self.iter_count < MAX_DEBUG_ITER_COUNT {
                return;
            }
            self.debug.frame_faces.push(
                self.faces
                    .iter()
                    .map(|(_face_key, face)| face.get_face_vertex(&self.edges, true))
                    .collect(),
            );
            self.debug.frame_faces_normal.push(
                self.faces
                    .iter()
                    .map(|(_, face)| face.normal.as_vec3())
                    .collect(),
            );
            let mut triangles = Vec::<usize>::new();
            self.faces
                .iter()
                .for_each(|(_face_key, face)| face.get_face_triangles(&self.edges, &mut triangles));
            self.debug.frame_mesh.push(triangles);
        }
    }

    #[cfg(feature = "dump")]
    #[derive(Serialize, Deserialize)]
    pub(crate) struct DebugInfo {
        pub points: Vec<Point3>,
        pub frame_faces: Vec<Vec<Vec<usize>>>,
        pub frame_faces_normal: Vec<Vec<Vec3>>,
        pub frame_mesh: Vec<Vec<usize>>,
        pub frame_merge_face_checks: Vec<Vec<MergeFaceCheck>>,
        pub current_frame_merge_face_checks: Vec<MergeFaceCheck>,
        pub current_horizon_lines: Vec<usize>,
    }
    impl DebugInfo {
        pub fn new() -> Self {
            Self {
                points: vec![],
                frame_faces: vec![],
                frame_faces_normal: vec![],
                frame_mesh: vec![],
                frame_merge_face_checks: vec![],
                current_frame_merge_face_checks: vec![],
                current_horizon_lines: vec![],
            }
        }
    }
    #[derive(Serialize, Deserialize)]
    pub(crate) struct MergeFaceCheck {
        pub face0: Vec<usize>,
        pub face1: Vec<usize>,
        pub coplanar: bool,
    }

    impl MergeFaceCheck {
        pub fn new(face0: Vec<usize>, face1: Vec<usize>, coplanar: bool) -> Self {
            Self {
                face0,
                face1,
                coplanar,
            }
        }
    }
}

#[cfg(any(test, feature = "dump", feature = "safe_check"))]
impl Face {
    pub(crate) fn get_face_vertex(&self, edges: &EdgeMap, duplicate: bool) -> Vec<usize> {
        let mut face_vertices = Vec::new();
        for (_, vertex) in FaceEdgeIter::new(self.edge_index, edges) {
            face_vertices.push(vertex.origin);
            if duplicate {
                face_vertices.push(vertex.destination);
            }
        }
        face_vertices
    }
}

pub(crate) enum TriangleResult {
    Origin(Vec<usize>, ConvexHullConstructState),
    NewPoints(Vec<Point3>, Vec<usize>, ConvexHullConstructState),
}
impl TriangleResult {
    #[must_use]
    pub fn triple<'a>(
        &'a self,
        points: &'a [Point3],
    ) -> (&'a [Point3], &'a Vec<usize>, ConvexHullConstructState) {
        match self {
            TriangleResult::Origin(indices, state) => (points, indices, *state),
            TriangleResult::NewPoints(new_points, indices, state) => (new_points, indices, *state),
        }
    }
}
fn get_convex_triangles(points: &[Point3]) -> Result<TriangleResult, ConvexHullConstructError> {
    let mut config = Config::default();
    config.set_shift_point_align_aabb_center(false);
    let config = &mut config;
    let convex_faces_result = build_convex_face(points, config)?;

    let convex_faces_result = validate_and_rebuild_if_needed(convex_faces_result, points, config)?;

    let convex_face = convex_faces_result.convex_final_faces();

    let mut indices = Vec::new();
    convex_face.faces.iter().for_each(|(_, face)| {
        face.get_face_triangles(&convex_face.edges, &mut indices);
    });
    let result = match convex_faces_result {
        RebuildResult::Origin(_) => TriangleResult::Origin(indices, Success),
        RebuildResult::Partial(_, state) => TriangleResult::Origin(indices, state),
        RebuildResult::Rebuild(new_points, _, partial_state) => {
            TriangleResult::NewPoints(new_points, indices, partial_state)
        }
    };
    Ok(result)
}

#[derive(Default)]
pub struct ConvexMesh {
    vertices: Vec<Point3>,
    triangles: Vec<u32>,
}

impl ConvexMesh {
    /// # Errors
    ///
    /// Will return `ConvexHullConstructState` if points are not suitable to build a convex hull.
    pub fn new(
        points: &[Point3],
    ) -> Result<(ConvexMesh, ConvexHullConstructState), ConvexHullConstructError> {
        let raw_triangles = get_convex_triangles(points)?;
        let mut unique_indices = FxHashMap::default();

        let mut vertices = vec![];
        let mut triangles = vec![];
        let (points, indices, state) = raw_triangles.triple(points);
        for index in indices {
            let real_index = unique_indices.entry(*index).or_insert_with(|| {
                let value = vertices.len();
                vertices.push(points[*index]);
                value as u32
            });

            triangles.push(*real_index);
        }

        let convex_mesh = ConvexMesh {
            vertices,
            triangles,
        };
        Ok((convex_mesh, state))
    }

    #[must_use]
    #[inline]
    pub fn num_triangles(&self) -> u32 {
        let triangles = self.triangles.len();
        (triangles / 3) as u32
    }

    #[must_use]
    #[inline]
    pub fn num_vertices(&self) -> u32 {
        self.vertices.len() as u32
    }

    #[must_use]
    #[inline]
    pub fn vertices(&self) -> &[Point3] {
        &self.vertices
    }

    #[must_use]
    #[inline]
    pub fn triangles(&self) -> &[u32] {
        &self.triangles
    }
}

/// Configuration struct for quick hull algorithm parameters
///
/// This struct encapsulates various configuration parameters that control the limit and various of
/// the algorithm . Users can customize the algorithm's execution by adjusting these settings, such
/// as the number of faces, points, distance thresholds, etc., depending on the specific needs of
/// the application.
///
/// # Fields
/// - `max_face_count`: The maximum number of faces. This limits the number of faces involved in the
///   computation, preventing the algorithm from becoming too complex with large inputs.
/// - `max_point_count`: The maximum number of points. This limits the number of points that can be
///   processed, helping optimize performance by capping the input size.
/// - `epsilon`: The primary distance threshold used to determine whether two points are close
///   enough. The exact value depends on the scale of the problem domain.
/// - `plane_test_epsilon`: The distance threshold used during plane tests to determine if a point
///   lies on a plane.
/// - `min_init_distance`: The minimum distance threshold during init the first simplex.
/// - `conflict_point_epsilon`: The distance threshold used to determine if there are conflicting
///   points in the computation.
/// - `auto_calculate_epsilons`: A flag indicating whether to automatically calculate the `epsilon`
///   values. If set to `true`, the algorithm will dynamically adjust the `epsilon` values based on
///   the input data.
#[must_use]
pub struct Config {
    max_face_count: u32,
    max_point_count: u32,
    epsilon: f32,
    plane_test_epsilon: f32,
    min_init_distance: f32,
    conflict_point_epsilon: f32,
    auto_calculate_epsilons: bool,
    expand_faces_in_partial_success_state: bool,
    shift_point_align_aabb_center: bool,
}
/// Provides accessors and modifiers for the `Config` struct.
impl Config {
    /// Gets the maximum number of faces allowed in the convex hull.
    #[must_use]
    #[inline]
    pub fn max_face_count(&self) -> u32 {
        self.max_face_count
    }

    /// Gets the maximum number of points to be considered for the convex hull.
    #[must_use]
    #[inline]
    pub fn max_point_count(&self) -> u32 {
        self.max_point_count
    }

    /// Gets the general epsilon value for floating-point comparisons.
    #[must_use]
    #[inline]
    pub fn epsilon(&self) -> f32 {
        self.epsilon
    }

    /// Gets the epsilon value used for testing if a point is on a plane.
    #[must_use]
    #[inline]
    pub fn plane_test_epsilon(&self) -> f32 {
        self.plane_test_epsilon
    }

    /// Gets the minimum distance required for the initial simplex points.
    #[must_use]
    #[inline]
    pub fn min_init_distance(&self) -> f32 {
        self.min_init_distance
    }

    /// Gets the epsilon value used when assigning points to the conflict list of a face.
    #[must_use]
    #[inline]
    pub fn conflict_point_epsilon(&self) -> f32 {
        self.conflict_point_epsilon
    }

    /// Returns `true` if epsilon values are automatically calculated based on the input point
    /// cloud's AABB.
    #[must_use]
    #[inline]
    pub fn auto_calculate_epsilons(&self) -> bool {
        self.auto_calculate_epsilons
    }

    /// Sets the maximum number of faces allowed in the convex hull.
    ///
    /// # Panics
    /// Panics if `max_face_count` is less than 4.
    #[inline]
    pub fn set_max_face_count(&mut self, max_face_count: u32) {
        assert!(max_face_count >= 4);
        self.max_face_count = max_face_count;
    }

    /// Sets the maximum number of points to be considered for the convex hull.
    ///
    /// # Panics
    /// Panics if `max_point_count` is less than 4.
    #[inline]
    pub fn set_max_point_count(&mut self, max_point_count: u32) {
        assert!(max_point_count >= 4);
        self.max_point_count = max_point_count;
    }

    /// Sets the general epsilon value for floating-point comparisons.
    ///
    /// # Panics
    /// Panics if `epsilon` is not a positive number.
    #[inline]
    pub fn set_epsilon(&mut self, epsilon: f32) {
        assert!(epsilon > 0.0);
        self.epsilon = epsilon;
    }

    /// Sets the epsilon value for testing if a point is on a plane.
    ///
    /// # Panics
    /// Panics if `plane_test_epsilon` is not a positive number.
    #[inline]
    pub fn set_plane_test_epsilon(&mut self, plane_test_epsilon: f32) {
        assert!(plane_test_epsilon > 0.0);
        self.plane_test_epsilon = plane_test_epsilon;
    }

    /// Sets the minimum distance required for the initial simplex points.
    ///
    /// # Panics
    /// Panics if `min_init_distance` is not a positive number.
    #[inline]
    pub fn set_min_init_distance(&mut self, min_init_distance: f32) {
        assert!(min_init_distance > 0.0);
        self.min_init_distance = min_init_distance;
    }

    /// Sets the epsilon value used when assigning points to the conflict list of a face.
    ///
    /// # Panics
    /// Panics if `conflict_point_epsilon` is not a positive number.
    #[inline]
    pub fn set_conflict_point_epsilon(&mut self, conflict_point_epsilon: f32) {
        assert!(conflict_point_epsilon > 0.0);
        self.conflict_point_epsilon = conflict_point_epsilon;
    }

    /// Sets whether to automatically calculate epsilon values based on the input point cloud's
    /// AABB.
    #[inline]
    pub fn set_auto_calculate_epsilons(&mut self, auto_calculate_epsilons: bool) {
        self.auto_calculate_epsilons = auto_calculate_epsilons;
    }

    /// Sets whether to shift the input points to align their AABB center with the origin.
    /// This can improve numerical stability for point clouds far from the origin.
    #[inline]
    pub fn set_shift_point_align_aabb_center(&mut self, align_to_center: bool) {
        self.shift_point_align_aabb_center = align_to_center;
    }

    /// Returns `true` if the input points are shifted to align their AABB center with the origin.
    #[inline]
    #[must_use]
    pub fn shift_point_align_aabb_center(&self) -> bool {
        self.shift_point_align_aabb_center
    }

    /// Sets whether to expand the hull with on-plane points when the construction ends in a partial
    /// success state (e.g., due to face count limits).
    #[inline]
    pub fn set_expand_faces_in_partial_success_state(&mut self, v: bool) {
        self.expand_faces_in_partial_success_state = v;
    }

    /// Returns `true` if the hull should be expanded with on-plane points when the construction
    /// ends in a partial success state.
    #[inline]
    #[must_use]
    pub fn expand_faces_in_partial_success_state(&self) -> bool {
        self.expand_faces_in_partial_success_state
    }
}

/// Provides a default configuration for the `QuickHull` algorithm.
///
/// The default values are suitable for general use cases but may need tuning for specific point
/// distributions.
impl Default for Config {
    /// Creates a default `Config` instance.
    ///
    /// - `max_face_count`: `255`
    /// - `max_point_count`: `65536`
    /// - `base_epsilon`: `1e-6`
    /// - `auto_calculate_epsilons`: `true`
    /// - `shift_point_align_aabb_center`: `false`
    fn default() -> Self {
        Self::new(DEFAULT_FACE_MAX, MAX_POINT_COUNT, 1e-6, true, false)
    }
}

/// Provides constructors and internal methods for `Config`.
impl Config {
    /// Creates a new `Config` with specified parameters.
    ///
    /// This constructor allows for fine-grained control over the hull generation process.
    ///
    /// # Arguments
    ///
    /// * `max_face_count` - The maximum number of faces allowed in the resulting convex hull.
    /// * `max_point_count` - The maximum number of points to use for hull construction.
    /// * `base_epsilon` - The base epsilon value for floating-point comparisons. This is used to
    ///   derive other epsilon values if `auto_calculate_epsilons` is false.
    /// * `auto_calculate_epsilons` - If `true`, various epsilon values are dynamically calculated
    ///   based on the scale of the input points.
    /// * `shift_point_align_aabb_center` - If `true`, input points will be translated so their AABB
    ///   center is at the origin. This can improve numerical stability.
    pub fn new(
        max_face_count: u32,
        max_point_count: u32,
        base_epsilon: f32,
        auto_calculate_epsilons: bool,
        shift_point_align_aabb_center: bool,
    ) -> Self {
        let plane_test_epsilon = base_epsilon;
        Self {
            max_face_count,
            max_point_count,
            epsilon: base_epsilon,
            plane_test_epsilon,
            min_init_distance: plane_test_epsilon,
            conflict_point_epsilon: plane_test_epsilon,
            auto_calculate_epsilons,
            expand_faces_in_partial_success_state: false,
            shift_point_align_aabb_center,
        }
    }

    #[inline]
    fn recalculate_epsilon_if_need(&self, epsilon_scale: f32) -> Option<Self> {
        if self.auto_calculate_epsilons {
            let (epsilon_scale, plane_test_epsilon, min_init_distance) =
                Self::calculate_epsilon(epsilon_scale, self.epsilon);
            let conflict_point_epsilon = epsilon_scale * 3f32 * self.epsilon;
            let conflict_point_epsilon = conflict_point_epsilon.maxf(0.001_f32);
            let result = Self {
                max_face_count: self.max_face_count,
                max_point_count: self.max_point_count,
                epsilon: self.epsilon,
                plane_test_epsilon,
                min_init_distance,
                conflict_point_epsilon,
                auto_calculate_epsilons: false,
                expand_faces_in_partial_success_state: false,
                shift_point_align_aabb_center: false,
            };
            Some(result)
        } else {
            None
        }
    }

    fn calculate_epsilon(epsilon_scale: f32, epsilon: f32) -> (f32, f32, f32) {
        let epsilon_scale = f32::maxf(epsilon_scale, 1.0f32);
        let plane_test_scale = (epsilon_scale * epsilon_scale * epsilon_scale).sqrtf();
        // need an issue here,that shift and normalize cloud point should be done in the future
        let plane_test_epsilon = plane_test_scale * epsilon;
        let min_init_distance = plane_test_epsilon * 10f32;
        (epsilon_scale, plane_test_epsilon, min_init_distance)
    }
}

#[cfg(test)]
mod tests {

    use approx_det::relative_eq;
    use glam_det::{Mat3, Point3, UnitVec3, Vec3};

    use super::{calculate_aabb_center, shift_points_align_center};
    use crate::face::{FaceIndexSmallVec, FaceKey, FaceState};
    use crate::half_edge::{EdgeIndexSmallVec, EdgeMap};
    use crate::quick_hull_3d::{
        convert_to_construct_result, expand_convex_hull, try_inverse_mat3, MergeType,
        RescheduleConflictPointIndexSmallVec, SimplexIndex,
    };
    use crate::ConvexHullConstructState::{FaceCountLimitHit, PointCountLessThanFour, Success};
    use crate::{
        build_convex_face, build_convex_hull, Config, ConvexFinalFaces,
        ConvexFinalFacesConstructResult, Face, HalfEdge,
    };

    impl ConvexFinalFaces {
        // the edge sequence is ccw in left hand and cw in right hand
        #[allow(clippy::missing_panics_doc)]
        pub fn create_face(
            &mut self,
            vertices_circle: &[usize],
            points: &[Point3],
            check_normal: Option<UnitVec3>,
        ) -> (FaceKey, EdgeIndexSmallVec) {
            let mut iter = vertices_circle.iter();
            let first = iter.next().expect("first vertex must exist");
            let mut prev = *first;
            let mut edges_vec = EdgeIndexSmallVec::new();
            for &vertex in iter {
                let edge = HalfEdge::new(prev, vertex);
                let edge_key = self.edges.insert(edge);
                edges_vec.push(edge_key);
                prev = vertex;
            }
            let last = *first;
            let edge = HalfEdge::new(prev, last);
            let edge_key = self.edges.insert(edge);
            edges_vec.push(edge_key);
            Self::connect_edges(&edges_vec, &mut self.edges);
            let mut face = Face::new(
                edge_key,
                Point3::new(0.0, 0.0, 0.0),
                UnitVec3::X,
                0.0,
                vertices_circle.len() as u32,
            );
            face.update_normal_and_center(&self.edges, points);
            if let Some(check_normal) = check_normal {
                assert_eq!(face.normal, check_normal);
            }
            let face_key = self.faces.insert(face);
            for edge_key in &edges_vec {
                let edge = &mut self.edges[*edge_key];
                edge.set_owner_face(face_key);
            }
            (face_key, edges_vec)
        }

        // the edge sequence is ccw in left hand and cw in right hand
        fn connect_edges(edges_vec: &EdgeIndexSmallVec, edges: &mut EdgeMap) {
            for i in 0..edges_vec.len() {
                let current_edge_key = edges_vec[i];
                let next_edge_key = edges_vec[(i + 1) % edges_vec.len()];
                let current_edge = &mut edges[current_edge_key];
                current_edge.set_next(next_edge_key);
                let next_edge = &mut edges[next_edge_key];
                next_edge.set_prev(current_edge_key);
            }
        }
    }

    //       7---------6
    //      /|        /|
    //     4---------5 |
    //     | |       | |
    //     | 3-------|-2
    //     |/        |/
    //     0---------1

    fn cube_points() -> Vec<Point3> {
        let points = vec![
            Point3::new(-1.0, -1.0, 1.0),
            Point3::new(1.0, -1.0, 1.0),
            Point3::new(1.0, -1.0, -1.0),
            Point3::new(-1.0, -1.0, -1.0),
            Point3::new(-1.0, 1.0, 1.0),
            Point3::new(1.0, 1.0, 1.0),
            Point3::new(1.0, 1.0, -1.0),
            Point3::new(-1.0, 1.0, -1.0),
        ];
        points
    }
    //       7---------6
    //      /|        /|
    //     4---------5 |
    //     | |       | |
    //     | 3-------|-2
    //     |/        |/
    //     0---------1

    fn init_cube_simplex(cube_points: &[Point3]) -> ConvexFinalFaces {
        let mut convex_hull = ConvexFinalFaces::new();
        convex_hull.init_simplex(cube_points, SimplexIndex::new(0, 4, 1, 3));
        convex_hull
    }
    #[test]
    fn test_calculate_aabb_center() {
        let points = vec![Point3::new(-1., -1., -1.), Point3::new(3., 3., 3.)];
        let center = calculate_aabb_center(points.as_slice());

        assert!(center.abs_diff_eq(Vec3::new(1., 1., 1.), 1e-5));
    }

    #[test]
    fn test_calculate_aabb_center_empty() {
        let points: Vec<Point3> = vec![];
        let center = calculate_aabb_center(points.as_slice());

        assert!(center.abs_diff_eq(Vec3::ZERO, 1e-5));
    }

    #[test]
    fn test_shift_points_align_center() {
        let points = vec![Point3::new(-1., -1., -1.), Point3::new(3., 3., 3.)];
        let center = Vec3::new(1., 1., 1.);

        let shifted_points = shift_points_align_center(points.as_slice(), center);
        let expect_points = [Point3::new(-2., -2., -2.), Point3::new(2., 2., 2.)];

        for i in 0..shifted_points.len() {
            assert!(shifted_points[i].abs_diff_eq(expect_points[i], 1e-5));
        }
    }

    #[test]
    fn test_convexhull_center() {
        let points = vec![
            Point3::new(0.0, 0.0, 0.0),
            Point3::new(1.0, 0.0, 0.0),
            Point3::new(0.0, 2.0, 0.0),
            Point3::new(0.0, 0.0, 4.0),
        ];
        let mut config = Config::default();
        config.set_shift_point_align_aabb_center(true);
        let convex_hull_result = build_convex_hull(&points, &config);
        let (convex_hull, _) = convex_hull_result.unwrap();
        assert_eq!(convex_hull.shift_center, Some(Vec3::new(0.5, 1.0, 2.0)));
        assert_eq!(convex_hull.center, Vec3::new(0.25, 0.5, 1.0));
    }

    #[test]
    fn test_init_simplex() {
        let points = cube_points();
        let convex_hull = init_cube_simplex(&points);
        let expect_faces = [[0, 4, 1], [0, 3, 4], [0, 1, 3], [1, 4, 3]];
        convex_hull
            .faces
            .iter()
            .enumerate()
            .for_each(|(i, (_, face))| {
                let vertices = face.get_face_vertex(&convex_hull.edges, false);
                let expect = expect_faces[i];
                assert_eq!(vertices, expect);
            });
    }

    #[test]
    fn test_init_conflict_points() {
        let points = cube_points();
        let mut convex_hull = ConvexFinalFaces::new();
        let init_simplex = SimplexIndex::new(0, 4, 1, 3);
        convex_hull.init_simplex(&points, init_simplex);
        convex_hull.init_conflict_points(&points, &init_simplex, 0.00001f32);
        let (_, face) = convex_hull.faces.iter().last().expect("face must exist");
        let vertices = face.get_face_vertex(&convex_hull.edges, false);
        assert_eq!(vertices, [1, 4, 3]);
        let len = face.conflict_points.len();
        assert_eq!(len, 4);
        assert!(face.conflict_points.contains(&2));
        assert!(face.conflict_points.contains(&5));
        assert!(face.conflict_points.contains(&6));
        assert!(face.conflict_points.contains(&7));
    }

    #[test]
    fn test_get_triangles() {
        let points = cube_points();
        let convex_hull = init_cube_simplex(&points);
        let mut triangles = Vec::<usize>::new();
        convex_hull.faces.iter().for_each(|(_face_key, face)| {
            face.get_face_triangles(&convex_hull.edges, &mut triangles);
        });
        assert_eq!(triangles.len(), 12);
        let expect_result = [0, 1, 4, 0, 4, 3, 0, 3, 1, 1, 3, 4];
        for (i, &index) in triangles.iter().enumerate() {
            assert_eq!(index, expect_result[i]);
        }
    }

    #[test]
    fn test_add_twin_edge_visited() {
        let points = cube_points();
        let mut convex_hull = init_cube_simplex(&points);
        let face = &mut convex_hull.faces.iter().next().expect("face must exist").1;
        let edge_index = face.edge_index;
        let edge = &convex_hull.edges[edge_index];
        let twin_edge = edge.twin();
        convex_hull.add_twin_edge_visited(edge_index);
        assert!(convex_hull.visited_edges.contains(&edge_index));
        assert!(convex_hull.visited_edges.contains(&twin_edge));
    }

    #[test]
    fn test_reschedule_conflict_points() {
        let mut points = cube_points();
        let face_center = (points[4].as_vec3() + points[1].as_vec3() + points[3].as_vec3()) / 3f32;
        let simplex_center =
            (points[4].as_vec3() + points[1].as_vec3() + points[3].as_vec3() + points[0].as_vec3())
                / 4.0f32;
        points.push(Point3::from_vec3(face_center));
        points.push(Point3::from_vec3(simplex_center));
        let mut convex_hull = init_cube_simplex(&points);
        let faces: FaceIndexSmallVec = convex_hull.faces.iter().map(|(key, _)| key).collect();
        let mut reschedule_points = RescheduleConflictPointIndexSmallVec::new();
        reschedule_points.extend_from_slice(&[2, 5, 6, 7, 8, 9]);
        let reschedule_points = reschedule_points;
        convex_hull.reschedule_conflict_points(&reschedule_points, &points, &faces, 0.01f32);
        let last_face = &convex_hull.faces[*faces.last().unwrap()];
        let last_face_conflict_point = last_face.conflict_points.len();
        assert_eq!(last_face_conflict_point, 4);
        assert!(last_face.conflict_points.contains(&2));
        assert!(last_face.conflict_points.contains(&5));
        assert!(last_face.conflict_points.contains(&6));
        assert!(last_face.conflict_points.contains(&7));
        let last_face = &mut convex_hull.faces[*faces.last().unwrap()];
        last_face.mark = FaceState::Delete;
        last_face.conflict_points.clear();
        convex_hull.reschedule_conflict_points(&reschedule_points, &points, &faces, 0.01f32);
        convex_hull
            .faces
            .iter()
            .for_each(|(_key, face)| assert!(face.conflict_points.is_empty()));
    }

    #[test]
    fn test_collect_need_reschedule_points() {
        let points = cube_points();
        let mut convex_hull = init_cube_simplex(&points);

        let mut reschedule_points = RescheduleConflictPointIndexSmallVec::new();
        reschedule_points.extend_from_slice(&[2, 5, 6, 7]);
        let reschedule_points = reschedule_points;
        let faces: FaceIndexSmallVec = convex_hull.faces.iter().map(|(key, _)| key).collect();
        convex_hull.reschedule_conflict_points(&reschedule_points, &points, &faces, 0.01f32);
        convex_hull.visited_faces.extend_from_slice(&faces);
        let mut reschedule_conflict_points = RescheduleConflictPointIndexSmallVec::new();
        convex_hull.collect_need_reschedule_points(&mut reschedule_conflict_points);
        convex_hull.faces.iter().for_each(|(_key, face)| {
            assert!(face.conflict_points.is_empty());
        });
    }

    // 0---------------1
    // | \           / |
    // |   5-------6   |
    // |   | \   / |   |
    // |   |   4   |   |
    // |   | /   \ |   |
    // |   8-------7   |
    // | /           \ |
    // 3---------------2

    #[test]
    fn test_horizon_edges() {
        // Define the four corner vertices of the bottom quadrilateral
        let vertex_0 = Point3::new(-1.0, 0.0, 1.0);
        let vertex_1 = Point3::new(1.0, 0.0, 1.0);
        let vertex_2 = Point3::new(1.0, 0.0, -1.0);
        let vertex_3 = Point3::new(-1.0, 0.0, -1.0);

        // Define the top vertex
        let vertex_4 = Point3::new(0.0, 3.0, 0.0);

        // Define the midpoints of the edges
        let vertex_5 = Point3::new(-0.4, 1.5, 0.4);
        let vertex_6 = Point3::new(0.4, 1.5, 0.4);
        let vertex_7 = Point3::new(0.4, 1.5, -0.4);
        let vertex_8 = Point3::new(-0.4, 1.5, -0.4);
        let points = vec![
            vertex_0, vertex_1, vertex_2, vertex_3, vertex_4, vertex_5, vertex_6, vertex_7,
            vertex_8,
        ];
        let mut convex_hull = ConvexFinalFaces::new();
        let face_5876_key = create_shape_for_horizon_edge_test(&points, &mut convex_hull);
        convex_hull.faces[face_5876_key].conflict_points.push(4);
        let face_5876 = &convex_hull.faces[face_5876_key];
        let mut horizon_edges = EdgeIndexSmallVec::new();
        convex_hull.get_horizon_edges(
            face_5876_key,
            4,
            &points,
            0.01,
            face_5876.edge_index,
            &mut horizon_edges,
        );
        assert_eq!(horizon_edges.len(), 4);
        let expect = [[0, 3], [3, 2], [2, 1], [1, 0]];
        for (i, edge) in horizon_edges.iter().enumerate() {
            let edge = &convex_hull.edges[*edge];
            let expect = expect[i];
            assert_eq!(edge.origin, expect[0]);
            assert_eq!(edge.destination, expect[1]);
        }
    }
    // 0---------------1
    // | \           / |
    // |   5-------6   |
    // |   | \   / |   |
    // |   |   4   |   |
    // |   | /   \ |   |
    // |   8-------7   |
    // | /           \ |
    // 3---------------2
    fn create_shape_for_horizon_edge_test(
        points: &[Point3],
        convex_hull: &mut ConvexFinalFaces,
    ) -> FaceKey {
        let v_0385 = convex_hull.create_face(&[0, 3, 8, 5], points, None);
        let v_5610 = convex_hull.create_face(&[5, 6, 1, 0], points, None);
        let v_6721 = convex_hull.create_face(&[6, 7, 2, 1], points, None);
        let v_7832 = convex_hull.create_face(&[7, 8, 3, 2], points, None);
        let v_5876 = convex_hull.create_face(&[5, 8, 7, 6], points, Some(UnitVec3::Y));
        let v_0123 = convex_hull.create_face(&[0, 1, 2, 3], points, Some(-UnitVec3::Y));
        let edge_03 = v_0385.1[0];
        let edge_38 = v_0385.1[1];
        let edge_85 = v_0385.1[2];
        let edge_50 = v_0385.1[3];
        let edge_56 = v_5610.1[0];
        let edge_61 = v_5610.1[1];
        let edge_10 = v_5610.1[2];
        let edge_05 = v_5610.1[3];
        let edge_67 = v_6721.1[0];
        let edge_72 = v_6721.1[1];
        let edge_21 = v_6721.1[2];
        let edge_16 = v_6721.1[3];
        let edge_78 = v_7832.1[0];
        let edge_83 = v_7832.1[1];
        let edge_32 = v_7832.1[2];
        let edge_27 = v_7832.1[3];
        let edge_58 = v_5876.1[0];
        let edge_87 = v_5876.1[1];
        let edge_76 = v_5876.1[2];
        let edge_65 = v_5876.1[3];
        let edge_01 = v_0123.1[0];
        let edge_12 = v_0123.1[1];
        let edge_23 = v_0123.1[2];
        let edge_30 = v_0123.1[3];
        convex_hull.edges[edge_03].set_twin(edge_30);
        convex_hull.edges[edge_38].set_twin(edge_83);
        convex_hull.edges[edge_85].set_twin(edge_58);
        convex_hull.edges[edge_50].set_twin(edge_05);
        convex_hull.edges[edge_56].set_twin(edge_65);
        convex_hull.edges[edge_61].set_twin(edge_16);
        convex_hull.edges[edge_10].set_twin(edge_01);
        convex_hull.edges[edge_05].set_twin(edge_50);
        convex_hull.edges[edge_67].set_twin(edge_76);
        convex_hull.edges[edge_72].set_twin(edge_27);
        convex_hull.edges[edge_21].set_twin(edge_12);
        convex_hull.edges[edge_16].set_twin(edge_61);
        convex_hull.edges[edge_78].set_twin(edge_87);
        convex_hull.edges[edge_83].set_twin(edge_38);
        convex_hull.edges[edge_32].set_twin(edge_23);
        convex_hull.edges[edge_27].set_twin(edge_72);
        convex_hull.edges[edge_58].set_twin(edge_85);
        convex_hull.edges[edge_87].set_twin(edge_78);
        convex_hull.edges[edge_76].set_twin(edge_67);
        convex_hull.edges[edge_65].set_twin(edge_56);
        convex_hull.edges[edge_01].set_twin(edge_10);
        convex_hull.edges[edge_12].set_twin(edge_21);
        convex_hull.edges[edge_23].set_twin(edge_32);
        convex_hull.edges[edge_30].set_twin(edge_03);
        v_5876.0
    }

    #[test]
    fn test_connect_new_faces() {
        let points = cube_points();
        let mut convex_hull = init_cube_simplex(&points);
        let faces: FaceIndexSmallVec = convex_hull.faces.iter().map(|(key, _)| key).collect();
        let face_134 = &mut convex_hull.faces[*faces.last().unwrap()];
        let mut horizon_edges = EdgeIndexSmallVec::new();
        let edge_index = face_134.edge_index;
        convex_hull.get_horizon_edges(
            *faces.last().unwrap(),
            2,
            &points,
            0.01,
            edge_index,
            &mut horizon_edges,
        );
        let mut new_faces = FaceIndexSmallVec::new();
        let mut new_edges = EdgeIndexSmallVec::new();
        convex_hull.connect_new_faces(2, &horizon_edges, &mut new_faces, &mut new_edges, &points);
        assert_eq!(new_faces.len(), 3);
        assert_eq!(new_edges.len(), 6);
        new_faces.iter().for_each(|face| {
            let face = &convex_hull.faces[*face];
            assert_eq!(face.mark, FaceState::Visible);
        });
        let expect_edges = [[2, 1], [4, 2], [2, 4], [3, 2], [2, 3], [1, 2]];
        for (i, edge) in new_edges.iter().enumerate() {
            let edge = &convex_hull.edges[*edge];
            let expect = expect_edges[i];
            assert_eq!(edge.origin, expect[0]);
            assert_eq!(edge.destination, expect[1]);
        }
    }

    //       7---------6
    //      /|        /|
    //     4---------5 |
    //     | |       | |
    //     | 3-------|-2
    //     |/        |/
    //     0---------1
    #[test]
    fn test_merge_face() {
        let points = cube_points();
        let mut convex_hull = init_cube_simplex(&points);
        let faces: FaceIndexSmallVec = convex_hull.faces.iter().map(|(key, _)| key).collect();
        let face_134 = &mut convex_hull.faces[*faces.last().unwrap()];
        let mut horizon_edges = EdgeIndexSmallVec::new();
        let edge_index = face_134.edge_index;
        convex_hull.get_horizon_edges(
            *faces.last().unwrap(),
            2,
            &points,
            0.01,
            edge_index,
            &mut horizon_edges,
        );
        let mut new_faces = FaceIndexSmallVec::new();
        let mut new_edges = EdgeIndexSmallVec::new();
        let conflict_point = 2;
        convex_hull.connect_new_faces(
            conflict_point,
            &horizon_edges,
            &mut new_faces,
            &mut new_edges,
            &points,
        );
        let base_epsilon = 1e-6f32;
        let merge_result = convex_hull.merge_face(
            &points,
            FaceState::Visible,
            MergeType::NonConvexWrtLargerFace,
            base_epsilon,
            &new_faces,
        );
        assert!(merge_result.is_none());
    }

    #[test]
    fn test_convert_to_construct_result_success() {
        let convex_final_faces = ConvexFinalFaces::new();
        let status = Success;
        let result = convert_to_construct_result(convex_final_faces, status);
        match result {
            ConvexFinalFacesConstructResult::Success(_) => {}
            _ => panic!("Expected Success result"),
        }
    }

    #[test]
    fn test_convert_to_construct_result_need_rebuild() {
        let convex_final_faces = ConvexFinalFaces::new();
        let status = PointCountLessThanFour;
        let result = convert_to_construct_result(convex_final_faces, status);
        match result {
            ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(state) => {
                assert_eq!(state, PointCountLessThanFour);
            }
            _ => panic!("Expected NeedRebuildWithAabbPoints result"),
        }
    }

    #[test]
    fn test_convert_to_construct_result_partial_success() {
        let convex_final_faces = ConvexFinalFaces::new();
        let status = FaceCountLimitHit;
        let result = convert_to_construct_result(convex_final_faces, status);
        match result {
            ConvexFinalFacesConstructResult::PartialSuccess(_, state) => {
                assert_eq!(state, FaceCountLimitHit);
            }
            _ => panic!("Expected PartialSuccess result"),
        }
    }

    #[test]
    fn test_set_max_face_count() {
        let mut config = Config::default();
        config.set_max_face_count(300);
        assert_eq!(config.max_face_count(), 300);
    }

    #[test]
    fn test_set_max_point_count() {
        let mut config = Config::default();
        config.set_max_point_count(70000);
        assert_eq!(config.max_point_count(), 70000);
    }

    #[test]
    #[allow(clippy::float_cmp)]
    fn test_set_epsilon() {
        let mut config = Config::default();
        config.set_epsilon(0.002);

        assert_eq!(config.epsilon(), 0.002_f32);
    }

    #[test]
    #[allow(clippy::float_cmp)]
    fn test_set_plane_test_epsilon() {
        let mut config = Config::default();
        config.set_plane_test_epsilon(0.002);

        assert_eq!(config.plane_test_epsilon(), 0.002_f32);
    }

    #[test]
    #[allow(clippy::float_cmp)]
    fn test_set_min_init_distance() {
        let mut config = Config::default();
        config.set_min_init_distance(0.02);
        assert_eq!(config.min_init_distance(), 0.02_f32);
    }

    #[test]
    #[allow(clippy::float_cmp)]
    fn test_set_conflict_point_epsilon() {
        let mut config = Config::default();
        config.set_conflict_point_epsilon(0.002);
        assert_eq!(config.conflict_point_epsilon(), 0.002);
    }

    #[test]
    fn test_set_auto_calculate_epsilons() {
        let mut config = Config::default();
        config.set_auto_calculate_epsilons(false);
        assert!(!config.auto_calculate_epsilons());
    }
    #[test]
    #[should_panic(expected = "assertion failed: conflict_point_epsilon > 0.0")]
    fn test_set_conflict_point_epsilon_panic() {
        let mut config = Config::default();
        config.set_conflict_point_epsilon(0.0); // This should trigger the assert and panic
    }
    #[test]
    #[should_panic(expected = "assertion failed: max_face_count >= 4")]
    fn test_set_max_face_count_panic() {
        let mut config = Config::default();
        config.set_max_face_count(3); // This should trigger the assert and panic
    }

    #[test]
    #[should_panic(expected = "assertion failed: max_point_count >= 4")]
    fn test_set_max_point_count_panic() {
        let mut config = Config::default();
        config.set_max_point_count(3); // This should trigger the assert and panic
    }

    #[test]
    #[should_panic(expected = "assertion failed: epsilon > 0.0")]
    fn test_set_epsilon_panic() {
        let mut config = Config::default();
        config.set_epsilon(0.0); // This should trigger the assert and panic
    }

    #[test]
    #[should_panic(expected = "assertion failed: plane_test_epsilon > 0.0")]
    fn test_set_plane_test_epsilon_panic() {
        let mut config = Config::default();
        config.set_plane_test_epsilon(0.0); // This should trigger the assert and panic
    }

    #[test]
    #[should_panic(expected = "assertion failed: min_init_distance > 0.0")]
    fn test_set_min_init_distance_panic() {
        let mut config = Config::default();
        config.set_min_init_distance(0.0); // This should trigger the assert and panic
    }
    /// Checks if two arrays are circularly equal.
    ///
    /// Circular equality means that one array can be rotated to match the other array.
    ///
    /// # Arguments
    ///
    /// * `arr1` - The first array to compare.
    /// * `arr2` - The second array to compare.
    ///
    /// # Returns
    ///
    /// * `true` if the arrays are circularly equal, `false` otherwise.
    fn are_circular_equal<T: PartialEq + Clone>(arr1: &[T], arr2: &[T]) -> bool {
        if arr1.len() != arr2.len() {
            return false;
        }

        let doubled_arr1: Vec<T> = arr1.iter().chain(arr1.iter()).cloned().collect();

        is_subarray(&doubled_arr1, arr2)
    }

    fn is_subarray<T: PartialEq>(arr1: &[T], arr2: &[T]) -> bool {
        for i in 0..=arr1.len() - arr2.len() {
            if &arr1[i..i + arr2.len()] == arr2 {
                return true;
            }
        }
        false
    }
    // 2
    // | \
    // |  3
    // | /  \
    // 0-----1

    #[test]
    fn test_face_normal() {
        let vertex_0 = Point3::new(0.0, 0.0, 0.0);
        let vertex_1 = Point3::new(1.0, 0.0, 0.0);
        let vertex_2 = Point3::new(0.0, 0.0, 1.0);
        let vertex_3 = Point3::new(0.5, 1.0, 0.5);
        let points = vec![vertex_0, vertex_1, vertex_2, vertex_3];
        let config = Config {
            max_face_count: 4,
            ..Default::default()
        };
        let result = build_convex_face(&points, &config);
        let result = result.unwrap();
        let mut convex_hull = match result {
            ConvexFinalFacesConstructResult::Success(v) => v,
            ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(_) => {
                panic!("should not be warning")
            }
            ConvexFinalFacesConstructResult::PartialSuccess(_, _) => {
                panic!("should not be face limit")
            }
        };
        let mut checked = false;

        // let's image the face [0,1,2],and we look at the face from the up to down, the normal
        // should be -y, and the index sequence should be [2,1,0]
        convex_hull.faces.iter_mut().for_each(|(_, face)| {
            if face.vertex.contains(&0) && face.vertex.contains(&1) && face.vertex.contains(&2) {
                let equal = are_circular_equal(&face.vertex, &[2, 1, 0]);
                assert!(equal);
                assert_eq!(face.normal, -UnitVec3::Y);
                checked = true;
            }
        });
        assert!(checked);
    }

    fn check_contain(points: &[Point3], new_point: Point3) {
        for point in points {
            if relative_eq!(point, &new_point, epsilon = 1e-6) {
                return;
            }
        }
        let debug_str = format!("{new_point:?}");
        debug_assert!(false, "point not found:{debug_str}");
    }
    // 2
    // | \
    // |  3
    // | /  \
    // 0-----1
    #[test]
    fn test_expand() {
        let vertex_0 = Point3::new(0.0, 0.0, 0.0);
        let vertex_1 = Point3::new(1.0, 0.0, 0.0);
        let vertex_2 = Point3::new(0.0, 0.0, 1.0);
        let vertex_3 = Point3::new(0.5, 1.0, 0.5);
        let vertex_4 = Point3::new(0.5, 2.0, 0.5);
        let points = vec![vertex_0, vertex_1, vertex_2, vertex_3, vertex_4];
        let result = build_convex_face(&points[0..4], &Config::default());
        let result = result.unwrap();
        let convex_hull = match result {
            ConvexFinalFacesConstructResult::Success(v) => v,
            ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(_state) => {
                panic!("should not be warning")
            }
            ConvexFinalFacesConstructResult::PartialSuccess(_, _) => {
                panic!("should not be face limit")
            }
        };
        let new_points = expand_convex_hull(&points, convex_hull);
        assert_eq!(new_points.len(), 4);
        check_contain(&new_points, vertex_4);
        let result = build_convex_face(&new_points, &Config::default());
        let is_success = matches!(result, Ok(ConvexFinalFacesConstructResult::Success(_)));
        assert!(is_success);
    }
    // 2
    // | \
    // |  3
    // | /  \
    // 0-----1
    #[test]
    fn test_find_intersection_of_3_plane() {
        let vertex_0 = Point3::new(0.0, 0.0, 0.0);
        let vertex_1 = Point3::new(1.0, 0.0, 0.0);
        let vertex_2 = Point3::new(0.0, 0.0, 1.0);
        let vertex_3 = Point3::new(0.5, 1.0, 0.5);
        let points = vec![vertex_0, vertex_1, vertex_2, vertex_3];
        let config = Config::default();
        let result = build_convex_face(&points, &config);
        let result = result.unwrap();
        let convex_hull = match result {
            ConvexFinalFacesConstructResult::Success(v) => v,
            ConvexFinalFacesConstructResult::NeedRebuildWithAabbPoints(_state) => {
                panic!("should not be warning")
            }
            ConvexFinalFacesConstructResult::PartialSuccess(_, _) => {
                panic!("should not be face limit")
            }
        };
        let new_points = expand_convex_hull(&points, convex_hull);
        assert_eq!(new_points.len(), 4);

        new_points
            .iter()
            .for_each(|&new_point| check_contain(&points, new_point));
        let config = Config::default();
        let result = build_convex_face(&new_points, &config);
        let is_success = matches!(result, Ok(ConvexFinalFacesConstructResult::Success(_)));
        assert!(is_success);
    }

    // 2
    // | \
    // |  3
    // | /  \
    // 0-----1
    #[test]
    fn test_expand_api() {
        let vertex_0 = Point3::new(0.0, 0.0, 0.0);
        let vertex_1 = Point3::new(1.0, 0.0, 0.0);
        let vertex_2 = Point3::new(0.0, 0.0, 1.0);
        let vertex_3 = Point3::new(0.5, 1.0, 0.5);
        let vertex_4 = Point3::new(0.5, 0.5, 0.0);
        let points = vec![vertex_0, vertex_1, vertex_2, vertex_3, vertex_4];
        let config = Config {
            max_face_count: 4,
            expand_faces_in_partial_success_state: true,
            ..Default::default()
        };
        let convex_hull = build_convex_hull(&points, &config);
        assert!(convex_hull.is_ok());
        let config = Config {
            max_face_count: 4,
            expand_faces_in_partial_success_state: false,
            ..Default::default()
        };
        let convex_hull = build_convex_hull(&points, &config);
        assert!(convex_hull.is_ok());
    }

    #[test]
    fn test_aabb_rebuild_api() {
        let vertex_0 = Point3::new(0.0, 0.0, 0.0);
        let vertex_1 = Point3::new(1.0, 0.0, 0.0);
        let vertex_2 = Point3::new(0.0, 0.0, 1.0);
        let vertex_3 = Point3::new(1.0, 0.0, 1.0);
        let points = vec![vertex_0, vertex_1, vertex_2, vertex_3];
        let config = Config::default();
        let convex_hull = build_convex_hull(&points, &config);
        assert!(convex_hull.is_ok());
    }

    #[test]
    #[should_panic]
    fn test_aabb_rebuild_api_panic() {
        let vertex_0 = Point3::new(0.0, 0.0, 0.0);
        let vertex_1 = Point3::new(1.0, 0.0, 0.0);
        let vertex_2 = Point3::new(0.0, 0.0, 1.0);
        let vertex_3 = Point3::new(1.0, 0.0, 1.0);
        let points = vec![vertex_0, vertex_1, vertex_2, vertex_3];
        let config = Config {
            max_face_count: 4,
            ..Default::default()
        };
        let convex_hull = build_convex_hull(&points, &config);
        assert!(convex_hull.is_ok());
    }

    #[test]
    fn test_try_inverse_mat3() {
        // Test case where the matrix is invertible
        let mat = Mat3::from_cols_array(&[1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0]);
        let inv_mat = try_inverse_mat3(&mat);
        assert!(inv_mat.is_some());
        let inv_mat = inv_mat.unwrap();
        let identity = mat * inv_mat;
        assert_eq!(identity, Mat3::IDENTITY);

        // Test case where the matrix is not invertible (determinant is zero)
        let mat_singular = Mat3::from_cols_array(&[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0]);
        let inv_mat_singular = try_inverse_mat3(&mat_singular);
        assert!(inv_mat_singular.is_none());
    }
}