polyhedron 0.1.0

#![deny(missing_docs)]
#![doc = include_str!("../README.md")]

use std::cell::UnsafeCell;
use std::collections::{BTreeMap, BTreeSet};

/// Mesh algorithms, such as simplification, subdivision and remeshing.
pub mod algorithms;
/// Abstract trait to wrap over the contianers of geoemtric elements.
pub mod data_container;
/// Topological handles such as `VertHandle` or `FaceHandle` to facilitate mesh
/// queries.
pub mod handles;
/// Topological IDs.
pub mod topo_ids;
/// Verification methods to check if certain expected properties are respected
/// by a mesh.
pub mod verification;

mod helpers;
/// Prelude.
pub mod prelude
{
    pub use crate::data_container;
    pub use crate::handles::*;
    pub use crate::topo_ids;
    pub use crate::verification;
}

use data_container::*;
use topo_ids::*;

use crate::prelude::{
    edge_handle::EdgeHandle,
    face_handle::FaceHandle,
    half_edge_handle::HalfEdgeHandle,
    vert_handle::VertHandle,
};

/// A mesh that only has vertex position data.
pub type BasicPolyHedron<V, const MANIFOLD: bool> = PolyHedron<(V, (), ()), MANIFOLD>;
/// Versatile topology data structure ofr a mesh. If the generic parameter
/// `MANIFOLD` is true the mesh will be compiled assuming all inputs are
/// manifold. This is faster but restricted in the kinds of meshes it accepts.
/// If the parameter is false it will allow for the representation of
/// non-manifold geoemtry, but will use O(n) additional memory for the edges and
/// some operations will be slightly slower.
pub struct PolyHedron<C: MeshStorage, const MANIFOLD: bool>
{
    pub(crate) mesh_data: UnsafeCell<MeshData<C, MANIFOLD>>,
}

pub(crate) struct MeshData<C: MeshStorage, const MANIFOLD: bool>
{
    pub(crate) geometry_data: GeometryData<C>,
    pub(crate) topology_data: TopologyData,
}

impl<C: MeshStorage, const MANIFOLD: bool> PolyHedron<C, MANIFOLD>
{
    /// Construct a mesh from vert and face data. The `faces` parameter is
    /// an iterator over iterators. The first level correpsonds to each face,
    /// The second level corresponds to the vertices of the face.
    pub fn new(
        vert_data: C::VertStorage,
        face_data: C::FaceStorage,
        faces: impl Iterator<Item = impl Iterator<Item = usize>>,
    ) -> Self
    {
        let mesh = MeshData::new(vert_data, face_data, faces);
        Self {
            mesh_data: UnsafeCell::new(mesh),
        }
    }

    /// Handle to a topological vertex.
    pub fn vert_handle(&self, vid: VertId) -> VertHandle<C, MANIFOLD>
    {
        // In case the element was remove dwe need ot chekc it's current id, rahter than
        // the requested one.
        let true_id = unsafe {
            (&(*self.mesh_data.get()).topology_data.topo_verts)[vid.to_index()].id
        };
        VertHandle {
            mesh_data: self.mesh_data.get(),
            id: true_id,
        }
    }

    /// Handle to a topological Edge (only possible in a non manifold mesh).
    pub fn edge_handle(&self, eid: EdgeId) -> EdgeHandle<C, MANIFOLD>
    {
        // In case the element was remove dwe need ot chekc it's current id, rahter than
        // the requested one.
        let true_id = unsafe {
            (&(*self.mesh_data.get()).topology_data.topo_edges)[eid.to_index()].id
        };
        EdgeHandle {
            mesh_data: self.mesh_data.get(),
            id: true_id,
        }
    }

    /// Handle to a topological half edge.
    pub fn half_edge_handle(&self, hid: HalfEdgeId) -> HalfEdgeHandle<C, MANIFOLD>
    {
        // In case the element was remove dwe need ot chekc it's current id, rahter than
        // the requested one.
        let true_id = unsafe {
            (&(*self.mesh_data.get()).topology_data.topo_half_edges)[hid.to_index()].id
        };

        HalfEdgeHandle {
            mesh_data: self.mesh_data.get(),
            id: true_id,
        }
    }

    /// Handle to a topological face.
    pub fn face_handle(&self, fid: FaceId) -> FaceHandle<C, MANIFOLD>
    {
        // In case the element was remove dwe need ot chekc it's current id, rahter than
        // the requested one.
        let true_id = unsafe {
            (&(*self.mesh_data.get()).topology_data.topo_faces)[fid.to_index()].id
        };
        FaceHandle {
            mesh_data: self.mesh_data.get(),
            id: true_id,
        }
    }

    /// Number of vertices in the mesh.
    pub fn vert_count(&self) -> usize { unsafe { (*self.mesh_data.get()).vert_count() } }

    /// Number of edges in the mesh (works in both Manifold and Non manifold).
    pub fn edge_count(&self) -> usize { unsafe { (*self.mesh_data.get()).edge_count() } }

    /// Number of half edges in the mesh.
    pub fn half_edge_count(&self) -> usize
    {
        unsafe { (*self.mesh_data.get()).half_edge_count() }
    }

    /// Number of faces in the mesh.
    pub fn face_count(&self) -> usize { unsafe { (*self.mesh_data.get()).face_count() } }

    /// Add a vertex to the mesh without connecting it.
    pub fn add_vert(&mut self, data: VertData<C>) -> VertId
    {
        unsafe { (*self.mesh_data.get()).add_vert(data) }
    }

    /// Add an edge to the mesh, creating associated half edges and connecting
    /// them to their endpoints. Does *not* connect the face pointers.
    pub fn add_edge(
        &mut self,
        source: VertId,
        target: VertId,
        data: EdgeData<C>,
    ) -> HalfEdgeId
    {
        unsafe { (*self.mesh_data.get()).add_edge(source, target, data) }
    }

    /// Add a half edge around the specified edge, non-manifold only.
    pub fn add_half_edge(&mut self, source: VertId, edge: EdgeId) -> HalfEdgeId
    {
        unsafe { (*self.mesh_data.get()).add_half_edge(source, edge) }
    }

    /// Add a face around the specified vertices in the given orientation.
    /// Inserts edges if necessary and sets all connectivity.
    pub fn add_face(&mut self, vertices: &[VertId], data: FaceData<C>) -> FaceId
    {
        unsafe { (*self.mesh_data.get()).add_face(vertices, data) }
    }

    /// Iterator over the vertices.
    pub fn iter_verts(&self) -> impl Iterator<Item = VertHandle<C, MANIFOLD>>
    {
        VertIterator::new(self.mesh_data.get())
    }

    /// Iterator over the edges (always returns a half edge handle).
    pub fn iter_edges(&self) -> impl Iterator<Item = HalfEdgeHandle<C, MANIFOLD>>
    {
        EdgeIterator::new(self.mesh_data.get())
    }

    /// Iterator over the faces.
    pub fn iter_faces(&self) -> impl Iterator<Item = FaceHandle<C, MANIFOLD>>
    {
        FaceIterator::new(self.mesh_data.get())
    }

    /// Convert this mesh into a list of vertices and face groupings.
    pub fn face_list(&self) -> (Vec<VertData<C>>, Vec<Vec<usize>>)
    {
        unsafe { (*self.mesh_data.get()).face_list() }
    }
}

impl<C: MeshStorage, const MANIFOLD: bool> MeshData<C, MANIFOLD>
{
    pub(crate) fn new(
        vert_data: C::VertStorage,
        face_data: C::FaceStorage,
        faces: impl Iterator<Item = impl Iterator<Item = usize>>,
    ) -> Self
    {
        let mut mesh = Self {
            geometry_data: GeometryData {
                vert_data: C::VertStorage::default(),
                edge_data: C::EdgeStorage::default(),
                face_data: C::FaceStorage::default(),
            },

            topology_data: TopologyData {
                topo_verts: Vec::new(),
                topo_edges: Vec::new(),
                topo_half_edges: Vec::new(),
                topo_faces: Vec::new(),

                available_vert_ids: Vec::<VertId>::new(),
                available_edge_ids: Vec::<EdgeId>::new(),
                available_half_edge_ids: Vec::<HalfEdgeId>::new(),
                available_face_ids: Vec::<FaceId>::new(),
            },
        };

        for vert in vert_data.iter_ref()
        {
            mesh.add_vert(vert.clone());
        }

        for (i, face) in faces.enumerate()
        {
            let data = face_data.read(i as u64);
            let vids: Vec<_> = face.map(|i| VertId::new(i)).collect();

            mesh.add_face(&vids, data.clone());
        }

        mesh
    }

    pub(crate) fn vert_count(&self) -> usize { self.topology_data.topo_verts.len() }

    pub(crate) fn edge_count(&self) -> usize { self.topology_data.topo_edges.len() }

    pub(crate) fn half_edge_count(&self) -> usize
    {
        self.topology_data.topo_half_edges.len()
    }

    pub(crate) fn face_count(&self) -> usize { self.topology_data.topo_faces.len() }

    pub(crate) fn vert_ref(&self, vid: VertId) -> &TopoVert
    {
        &self.topology_data.topo_verts[vid.to_index()]
    }

    pub(crate) fn vert_mut(&mut self, vid: VertId) -> &mut TopoVert
    {
        &mut self.topology_data.topo_verts[vid.to_index()]
    }

    pub(crate) fn edge_ref(&self, eid: EdgeId) -> &TopoEdge
    {
        &self.topology_data.topo_edges[eid.to_index()]
    }

    pub(crate) fn edge_mut(&mut self, eid: EdgeId) -> &mut TopoEdge
    {
        &mut self.topology_data.topo_edges[eid.to_index()]
    }

    pub(crate) fn half_edge_ref(&self, hid: HalfEdgeId) -> &TopoHalfEdge
    {
        &self.topology_data.topo_half_edges[hid.to_index()]
    }

    pub(crate) fn half_edge_mut(&mut self, hid: HalfEdgeId) -> &mut TopoHalfEdge
    {
        &mut self.topology_data.topo_half_edges[hid.to_index()]
    }

    pub(crate) fn face_ref(&self, fid: FaceId) -> &TopoFace
    {
        &self.topology_data.topo_faces[fid.to_index()]
    }

    pub(crate) fn face_mut(&mut self, fid: FaceId) -> &mut TopoFace
    {
        &mut self.topology_data.topo_faces[fid.to_index()]
    }

    pub(crate) fn add_vert(&mut self, data: VertData<C>) -> VertId
    {
        if self.topology_data.available_vert_ids.is_empty()
        {
            let n = self.geometry_data.vert_data.len();
            self.geometry_data.vert_data.append(data);

            self.topology_data.topo_verts.push(TopoVert {
                id: VertId::new(n),
                ..Default::default()
            });

            VertId::new(n)
        }
        else
        {
            let vid = self.topology_data.available_vert_ids.pop().unwrap();

            self.vert_mut(vid).id = vid;

            self.geometry_data
                .vert_data
                .update(vid.to_index() as u64, move |v: &mut VertData<C>| {
                    *v = data.clone()
                });

            vid
        }
    }

    pub(crate) fn add_edge(
        &mut self,
        v1: VertId,
        v2: VertId,
        data: EdgeData<C>,
    ) -> HalfEdgeId
    {
        let (h1, h2, eid) = if !self.topology_data.available_edge_ids.is_empty()
        {
            let h1 = self.topology_data.available_half_edge_ids.pop().unwrap();
            let h2 = self.topology_data.available_half_edge_ids.pop().unwrap();
            let eid = self.topology_data.available_edge_ids.pop().unwrap();

            self.edge_mut(eid).id = eid;

            self.geometry_data
                .edge_data
                .update(eid.to_index() as u64, move |e: &mut EdgeData<C>| {
                    *e = data.clone()
                });

            (h1, h2, eid)
        }
        else
        {
            self.geometry_data.edge_data.append(data);

            let half_edge_count = self.topology_data.topo_half_edges.len();

            self.topology_data
                .topo_half_edges
                .push(TopoHalfEdge::default());
            self.topology_data
                .topo_half_edges
                .push(TopoHalfEdge::default());

            let edge_count = self.topology_data.topo_edges.len();
            if !MANIFOLD
            {
                self.topology_data.topo_edges.push(TopoEdge::default());
            }

            let h1 = HalfEdgeId::new(half_edge_count);
            let h2 = HalfEdgeId::new(half_edge_count + 1);
            let eid = EdgeId::new(edge_count);

            (h1, h2, eid)
        };

        let h1_ref = self.half_edge_mut(h1);
        h1_ref.id = h1;
        h1_ref.orbit_next = h2;
        h1_ref.source = v1;
        if !MANIFOLD
        {
            h1_ref.edge = eid;
        }

        let h2_ref = self.half_edge_mut(h2);
        h2_ref.id = h2;
        h2_ref.orbit_next = h1;
        h2_ref.source = v2;
        if !MANIFOLD
        {
            h2_ref.edge = eid;
        }

        if MANIFOLD
        {
            self.topology_data.topo_verts[v1.to_index()].arc = ArcId::HalfEdge(h1);
            self.topology_data.topo_verts[v2.to_index()].arc = ArcId::HalfEdge(h2);
        }
        else
        {
            let edge_ref = self.edge_mut(eid);
            edge_ref.id = eid;
            edge_ref.half_edge = h1;
            edge_ref.endpoints = [v1, v2];
            edge_ref.endpoint_cycles = [EndpointList {
                next: eid,
                prev: eid,
            }; 2];

            // Insert into the first point.
            if self.vert_ref(v1).arc.is_void()
            {
                // Set the vertex pointer.
                self.topology_data.topo_verts[v1.to_index()].arc = ArcId::Edge(eid);
            }
            else
            {
                // Insert the edge into the radial cycles of its neighbours.
                let edge1 = self.vert_ref(v1).arc.to_edge_id();

                EndpointList::join_at(&mut self.topology_data.topo_edges, eid, edge1, v1);
            }

            // Insert into the second point.
            if self.vert_ref(v2).arc.is_void()
            {
                // Set the vertex pointer.
                self.topology_data.topo_verts[v2.to_index()].arc = ArcId::Edge(eid);
            }
            else
            {
                // Insert the edge into the radial cycles of its neighbours.
                let edge2 = self.vert_ref(v2).arc.to_edge_id();
                EndpointList::join_at(&mut self.topology_data.topo_edges, eid, edge2, v2);
            }
        };

        h1
    }

    pub(crate) fn add_half_edge(&mut self, _vid: VertId, _edge_id: EdgeId) -> HalfEdgeId
    {
        todo!("this needs a rewrite before it can function");
        // assert!(!MANIFOLD);
        // unsafe {
        //     let other =
        //         (&(*self.topology_data.get()).topo_edges)[edge_id.
        // to_index()].half_edge;     let other_next =
        // (&(*self.topology_data.get()).topo_half_edges)
        //         [other.to_index()]
        //     .orbit_next;

        //     let n = (*self.topology_data.get()).topo_half_edges.len();
        //     (*self.topology_data.get())
        //         .topo_half_edges
        //         .push(TopoHalfEdge {
        //             id: HalfEdgeId::new(n),
        //             source: vid,
        //             orbit_next: other_next,
        //             edge: edge_id,
        //             ..Default::default()
        //         });

        //     self.half_edge_mut(other).orbit_next = HalfEdgeId::new(n);
        // }
    }

    pub(crate) fn add_face(&mut self, verts: &[VertId], data: FaceData<C>) -> FaceId
    {
        let (mut in_vertex_groups, mut out_vertex_groups): (Vec<_>, Vec<_>) = verts
            .iter()
            .map(|vid| {
                let [incident, out] = self.find_surrounding_blocks(*vid);
                (incident, out)
            })
            .unzip();

        let mut face_hedges = vec![HalfEdgeId::default(); verts.len()];

        for i in 0..verts.len()
        {
            let v1 = verts[i];
            let v2 = verts[(i + 1) % verts.len()];

            // See if we can find an edge connecting both endpoints of this face edge.
            let endpoint_test = |hid: HalfEdgeId| {
                let pair = self.half_edge_ref(hid).orbit_next;
                let endpoint = self.half_edge_ref(pair).source;

                endpoint == v2
            };

            // Attempt to find an edge connecting the two endpoints in this face edge.
            let half_edge = self.find_half_edge(v1, endpoint_test);
            if let Some(hid) = half_edge
            {
                debug_assert!(self.half_edge_ref(hid).source == v1);
                face_hedges[i] = hid;
            }
        }

        // Add missing edges.
        for i in 0..verts.len()
        {
            if face_hedges[i].is_void()
            {
                let v1 = verts[i];
                let v2 = verts[(i + 1) % verts.len()];

                // No connecting edge exists so we must make a new one.
                face_hedges[i] = self.add_edge(v1, v2, EdgeData::<C>::default());

                let pair = self.half_edge_ref(face_hedges[i]).orbit_next;

                let prev = self.half_edge_ref(face_hedges[i]).face_prev;
                let next = self.half_edge_ref(face_hedges[i]).face_next;

                let group1 = out_vertex_groups[(i + 1) % verts.len()]
                    .remove(&next)
                    .unwrap_or(usize::MAX);
                let group2 = in_vertex_groups[i].remove(&prev).unwrap_or(usize::MAX);

                in_vertex_groups[i].insert(pair, group2);
                out_vertex_groups[(i + 1) % verts.len()].insert(pair, group1);
            };
        }

        // Connect inside edges.
        // Add the face.
        let fid = if self.topology_data.available_face_ids.is_empty()
        {
            let fid = FaceId::new(self.topology_data.topo_faces.len());

            self.geometry_data
                .face_data
                .append(FaceData::<C>::default());
            self.topology_data.topo_faces.push(TopoFace::default());

            fid
        }
        else
        {
            self.topology_data.available_face_ids.pop().unwrap()
        };
        for i in 0..face_hedges.len()
        {
            let h1 = face_hedges[i].to_index();
            let h2 = face_hedges[(i + 1) % face_hedges.len()].to_index();

            let topo_hedges = &mut self.topology_data.topo_half_edges;
            let (hedge1, hedge2) = get_two_mut(topo_hedges, h1, h2);
            hedge1.attach_next_in_face(hedge2);

            self.half_edge_mut(face_hedges[i]).face = fid;
        }

        // Merge groups based on face insertion.
        for i in 0..face_hedges.len()
        {
            let h1 = face_hedges[i];
            let h0 = face_hedges[(i + verts.len() - 1) % verts.len()];

            if in_vertex_groups[i].contains_key(&h0)
                && out_vertex_groups[i].contains_key(&h1)
            {
                let old_group = *in_vertex_groups[i].get(&h0).unwrap();
                let new_group = *out_vertex_groups[i].get(&h1).unwrap();

                for (_, group) in out_vertex_groups[i]
                    .iter_mut()
                    .filter(|(_, group)| **group == old_group)
                {
                    *group = new_group
                }
            }
        }

        // Inside half edges are no longer part of the boundary, update.
        for i in 0..face_hedges.len()
        {
            let h1 = face_hedges[i];
            if let Some(_) = out_vertex_groups[i].remove(&h1)
            {
                in_vertex_groups[(i + 1) % verts.len()].remove(&h1);
            }
        }

        // Find incomplete groups between outgoing and incoming.
        // if a group is incomplete, insert the outside edges
        // of the current triangle to fill in the gap.
        for i in 0..face_hedges.len()
        {
            let h1 = face_hedges[i];
            let pair = self.half_edge_ref(h1).orbit_next;

            if !self.half_edge_ref(pair).face.is_void()
            {
                continue;
            }

            let groups1 = out_vertex_groups[(i + 1) % verts.len()]
                .iter()
                .map(|(_, g)| *g)
                .collect::<BTreeSet<_>>();
            let groups2 = in_vertex_groups[(i + 1) % verts.len()]
                .iter()
                .map(|(_, g)| *g)
                .collect::<BTreeSet<_>>();

            let sym_diff: Vec<_> =
                groups1.symmetric_difference(&groups2).cloned().collect();

            let group = if sym_diff.is_empty()
            {
                usize::MAX
            }
            else
            {
                sym_diff[0]
            };

            out_vertex_groups[(i + 1) % verts.len()].insert(pair, group);

            let groups1 = out_vertex_groups[i]
                .iter()
                .map(|(_, g)| g)
                .collect::<BTreeSet<_>>();
            let groups2 = in_vertex_groups[i]
                .iter()
                .map(|(_, g)| g)
                .collect::<BTreeSet<_>>();

            let sym_diff: Vec<_> =
                groups1.symmetric_difference(&groups2).cloned().collect();

            let group = if sym_diff.is_empty()
            {
                usize::MAX
            }
            else
            {
                *sym_diff[0]
            };

            in_vertex_groups[i].insert(pair, group);
        }

        let face_ref = self.face_mut(fid);
        face_ref.id = fid;
        face_ref.half_edge = face_hedges[0];

        self.geometry_data.face_data.update(
            fid.to_index() as u64,
            |face: &mut FaceData<C>| {
                *face = data.clone();
            },
        );

        // Reconnect the half edges, ensuring all regions around an umbrella vertex are
        // reachable through pointer iteration.
        for i in 0..verts.len()
        {
            let mut group_edge_map = BTreeMap::new();
            for (h, g) in &in_vertex_groups[i]
            {
                group_edge_map.insert(*g, [*h, HalfEdgeId::default()]);
            }

            for (h, g) in &out_vertex_groups[i]
            {
                group_edge_map.get_mut(g).unwrap()[1] = *h;
            }

            if group_edge_map.is_empty()
            {
                continue;
            }

            let (_, [first_incident, first_out]) = group_edge_map.pop_last().unwrap();
            let mut last_in = first_incident;
            while let Some((_, [input, output])) = group_edge_map.pop_last()
            {
                let topo_hedges = &mut self.topology_data.topo_half_edges;
                let (hedge1, hedge2) =
                    get_two_mut(topo_hedges, last_in.to_index(), output.to_index());
                hedge1.attach_next_in_face(hedge2);

                last_in = input;
            }

            let topo_hedges = &mut self.topology_data.topo_half_edges;
            let (hedge1, hedge2) =
                get_two_mut(topo_hedges, last_in.to_index(), first_out.to_index());
            hedge1.attach_next_in_face(hedge2);
        }

        fid
    }

    fn find_star_half_edge<F: FnMut(HalfEdgeId) -> bool>(
        &self,
        v1: VertId,
        mut test: F,
    ) -> Option<HalfEdgeId>
    {
        assert!(MANIFOLD);

        let mut current_half_edge = self.vert_ref(v1).arc.to_half_edge_id();
        if current_half_edge.is_void()
        {
            return None;
        }

        let starting_half_edge = current_half_edge;
        loop
        {
            if test(current_half_edge)
            {
                return Some(current_half_edge);
            }

            let pair = self.half_edge_ref(current_half_edge).orbit_next;

            current_half_edge = self.half_edge_ref(pair).face_next;

            if current_half_edge == starting_half_edge
            {
                break;
            }
        }

        None
    }

    fn find_star_edge<F: FnMut(HalfEdgeId) -> bool>(
        &self,
        v1: VertId,
        mut test: F,
    ) -> Option<HalfEdgeId>
    {
        assert!(!MANIFOLD);
        let mut current_edge = self.vert_ref(v1).arc.to_edge_id();

        if current_edge.is_void()
        {
            return None;
        }

        // Iterate over the *edges*.
        let starting_edge = current_edge;
        loop
        {
            let mut current_half_edge = self.edge_ref(current_edge).half_edge;

            // Iterate over the *half edges* orbitting the current edge.
            let starting_half_edge = current_half_edge;
            loop
            {
                let source = self.half_edge_ref(current_half_edge).source;
                if source == v1 && test(current_half_edge)
                {
                    return Some(current_half_edge);
                }

                current_half_edge = self.half_edge_ref(current_half_edge).orbit_next;

                if current_half_edge == starting_half_edge
                {
                    break;
                }
            }

            current_edge = self.edge_ref(current_edge).cycle_at(v1).unwrap().next;

            if current_edge == starting_edge
            {
                break;
            }
        }

        None
    }

    fn find_half_edge<F: FnMut(HalfEdgeId) -> bool>(
        &self,
        v1: VertId,
        test: F,
    ) -> Option<HalfEdgeId>
    {
        if MANIFOLD
        {
            self.find_star_half_edge(v1, test)
        }
        else
        {
            self.find_star_edge(v1, test)
        }
    }

    pub(crate) fn face_list(&self) -> (Vec<VertData<C>>, Vec<Vec<usize>>)
    {
        let mut vert_data = Vec::new();

        for vert in self.geometry_data.vert_data.iter_ref()
        {
            vert_data.push(vert.clone())
        }

        let mut face_data = Vec::new();
        let n = self.topology_data.topo_faces.len();
        for i in 0..n
        {
            let face = &self.topology_data.topo_faces[i];
            if face.id.is_void()
            {
                continue;
            }

            face_data.push(Vec::new());

            let mut current_half_edge = face.half_edge;
            let starting_half_edge = current_half_edge;
            loop
            {
                let hedge = self.half_edge_ref(current_half_edge);
                face_data.last_mut().unwrap().push(hedge.source.to_index());

                current_half_edge = hedge.face_next;
                if current_half_edge == starting_half_edge
                {
                    break;
                }
            }
        }

        (vert_data, face_data)
    }

    fn find_surrounding_blocks(&self, v1: VertId) -> [BTreeMap<HalfEdgeId, usize>; 2]
    {
        let mut candidates = Vec::new();
        let _ = self.find_half_edge(v1, |hid| {
            if self.half_edge_ref(hid).face.is_void()
            {
                candidates.push(hid);
            }
            false
        });

        let mut group = 1;
        let mut incoming_group_map = BTreeMap::new();
        let mut outgoing_group_map = BTreeMap::new();
        while let Some(hid) = candidates.pop()
        {
            outgoing_group_map.insert(hid, group);

            let mut current = hid;
            loop
            {
                let pair_id = self.half_edge_ref(current).orbit_next;
                let pair = self.half_edge_ref(pair_id);
                if pair.face.is_void()
                {
                    incoming_group_map.insert(pair_id, group);
                    break;
                }

                current = pair.face_next;

                if current == hid
                {
                    break;
                }
            }

            group += 1;
        }

        [incoming_group_map, outgoing_group_map]
    }
}

fn get_two_mut<T>(arr: &mut [T], i: usize, j: usize) -> (&mut T, &mut T)
{
    assert!(i != j, "i!=j i:{} j:{}", i, j);
    assert!(i < arr.len());
    assert!(j < arr.len());
    let ptr = arr.as_mut_ptr();
    unsafe { (&mut *ptr.add(i), &mut *ptr.add(j)) }
}

pub(crate) struct GeometryData<C: MeshStorage>
{
    vert_data: C::VertStorage,
    edge_data: C::EdgeStorage,
    face_data: C::FaceStorage,
}

pub(crate) struct TopologyData
{
    topo_verts: Vec<TopoVert>,
    topo_edges: Vec<TopoEdge>,
    topo_half_edges: Vec<TopoHalfEdge>,
    topo_faces: Vec<TopoFace>,

    available_vert_ids: Vec<VertId>,
    available_edge_ids: Vec<EdgeId>,
    available_half_edge_ids: Vec<HalfEdgeId>,
    available_face_ids: Vec<FaceId>,
}

#[derive(Clone, Default, Debug)]
pub(crate) struct TopoVert
{
    pub(crate) id: VertId,
    pub(crate) arc: ArcId,
}

#[derive(Clone, Default, Debug)]
pub(crate) struct TopoEdge
{
    pub(crate) id: EdgeId,
    pub(crate) endpoints: [VertId; 2],
    pub(crate) half_edge: HalfEdgeId,
    pub(crate) endpoint_cycles: [EndpointList; 2],
}

impl TopoEdge
{
    pub(crate) fn cycle_at(&self, vid: VertId) -> Option<&EndpointList>
    {
        let index = self.endpoints.iter().position(|id| *id == vid);
        index.map(|i| &self.endpoint_cycles[i])
    }

    pub(crate) fn cycle_at_mut(&mut self, vid: VertId) -> Option<&mut EndpointList>
    {
        let index = self.endpoints.iter().position(|id| *id == vid);
        index.map(|i| &mut self.endpoint_cycles[i])
    }
}

#[derive(Clone, Default, Debug)]
pub(crate) struct TopoHalfEdge
{
    pub(crate) id: HalfEdgeId,

    pub(crate) source: VertId,
    pub(crate) orbit_next: HalfEdgeId,
    pub(crate) face_next: HalfEdgeId,
    pub(crate) face_prev: HalfEdgeId,
    /// Void in the case of a manifold mesh.
    pub(crate) edge: EdgeId,
    pub(crate) face: FaceId,
}

impl TopoHalfEdge
{
    pub(crate) fn attach_next_in_face(&mut self, other: &mut Self)
    {
        self.face_next = other.id;
        other.face_prev = self.id;
    }
}

#[derive(Clone, Default, Debug)]
pub(crate) struct TopoFace
{
    pub(crate) id: FaceId,
    pub(crate) half_edge: HalfEdgeId,
}

#[derive(Copy, Clone, Default, Debug)]
pub(crate) struct EndpointList
{
    next: EdgeId,
    prev: EdgeId,
}

impl EndpointList
{
    /// Join two circular doubly linked lists rooted at `a` and `b`.
    pub(crate) fn join_at(edges: &mut [TopoEdge], a: EdgeId, b: EdgeId, vid: VertId)
    {
        assert!(a != b);

        let (c1, c2) = get_two_mut(edges, a.to_index(), b.to_index());

        let a_prev = c1.cycle_at_mut(vid).unwrap().prev;
        let b_prev = c2.cycle_at_mut(vid).unwrap().prev;

        // Connect a's tail to b
        edges[a_prev.to_index()].cycle_at_mut(vid).unwrap().next = b;
        edges[b.to_index()].cycle_at_mut(vid).unwrap().prev = a_prev;

        // Connect b's tail to a
        edges[b_prev.to_index()].cycle_at_mut(vid).unwrap().next = a;
        edges[a.to_index()].cycle_at_mut(vid).unwrap().prev = b_prev;
    }

    pub(crate) fn remove_edge(edges: &mut [TopoEdge], edge: EdgeId, vid: VertId)
    {
        let endpoint_cycle = edges[edge.to_index()].cycle_at_mut(vid).unwrap();
        let prev_id = endpoint_cycle.prev;
        let next_id = endpoint_cycle.next;

        // If the node points to itself, it's the only node in the list.
        if prev_id == edge && next_id == edge
        {
            return;
        }

        // Connect the previous node to the next node.
        edges[prev_id.to_index()].cycle_at_mut(vid).unwrap().next = next_id;

        // Connect the next node to the previous node.
        edges[next_id.to_index()].cycle_at_mut(vid).unwrap().prev = prev_id;

        // Clear the removed node's connections.
        let removed_cycle = edges[edge.to_index()].cycle_at_mut(vid).unwrap();
        removed_cycle.prev = edge;
        removed_cycle.next = edge;
    }
}

#[derive(Clone, Debug)]
pub(crate) enum ArcId
{
    Edge(EdgeId),
    HalfEdge(HalfEdgeId),
}

impl Default for ArcId
{
    fn default() -> Self { Self::Edge(EdgeId::default()) }
}

impl ArcId
{
    pub(crate) fn is_void(&self) -> bool
    {
        match self
        {
            ArcId::Edge(id) => id.is_void(),
            ArcId::HalfEdge(id) => id.is_void(),
        }
    }

    pub(crate) fn to_edge_id(&self) -> EdgeId
    {
        match self
        {
            ArcId::Edge(id) => *id,
            ArcId::HalfEdge(id) =>
            {
                if id.is_void()
                {
                    return EdgeId::default();
                }

                panic!("Requested edge on a half edge arc.")
            }
        }
    }

    pub(crate) fn to_half_edge_id(&self) -> HalfEdgeId
    {
        match self
        {
            ArcId::HalfEdge(id) => *id,
            ArcId::Edge(id) =>
            {
                if id.is_void()
                {
                    return HalfEdgeId::default();
                }
                panic!("Requested half edge on an edge arc.")
            }
        }
    }
}

pub(crate) fn attach_face(
    topology_data: &mut TopologyData,
    hedges: &[HalfEdgeId],
    face: FaceId,
)
{
    for i in 0..hedges.len()
    {
        let he = hedges[i];
        let hn = hedges[(i + 1) % hedges.len()];
        topology_data.topo_half_edges[he.to_index()].face = face;
        topology_data.topo_half_edges[he.to_index()].face_next = hn;
        topology_data.topo_half_edges[hn.to_index()].face_prev = he;
    }

    topology_data.topo_faces[face.to_index()].half_edge = hedges[0];
}

/// Iterator over all the vertices of a mesh.
pub struct VertIterator<C: MeshStorage, const MANIFOLD: bool>
{
    mesh_data: *mut MeshData<C, MANIFOLD>,
    current_index: usize,
    last_index: usize,
}

impl<C: MeshStorage, const MANIFOLD: bool> VertIterator<C, MANIFOLD>
{
    pub(crate) fn new(mesh_data: *mut MeshData<C, MANIFOLD>) -> Self
    {
        let mut res = 0;
        while unsafe { (&(*mesh_data).topology_data.topo_verts)[res].id.is_void() }
        {
            res += 1;
        }

        Self {
            mesh_data,
            current_index: res,
            last_index: unsafe { (*mesh_data).topology_data.topo_verts.len() },
        }
    }
}

impl<C: MeshStorage, const MANIFOLD: bool> Iterator for VertIterator<C, MANIFOLD>
{
    type Item = VertHandle<C, MANIFOLD>;

    fn next(&mut self) -> Option<Self::Item>
    {
        let res = self.current_index;

        if res >= self.last_index
        {
            return None;
        }

        loop
        {
            self.current_index += 1;
            if self.current_index >= self.last_index
            {
                break;
            }

            if !unsafe {
                (&(*self.mesh_data).topology_data.topo_verts)[self.current_index]
                    .id
                    .is_void()
            }
            {
                break;
            }
        }

        Some(VertHandle {
            id: VertId::new(res),
            mesh_data: self.mesh_data,
        })
    }
}

/// Iterator over all the edges of a mesh.
pub struct EdgeIterator<C: MeshStorage, const MANIFOLD: bool>
{
    mesh_data: *mut MeshData<C, MANIFOLD>,
    current_index: usize,
    last_index: usize,
}

impl<C: MeshStorage, const MANIFOLD: bool> EdgeIterator<C, MANIFOLD>
{
    pub(crate) fn new(mesh_data: *mut MeshData<C, MANIFOLD>) -> Self
    {
        let mut res = 0;
        while if MANIFOLD
        {
            unsafe {
                (&(*mesh_data).topology_data.topo_half_edges)[res]
                    .id
                    .is_void()
            }
        }
        else
        {
            unsafe { (&(*mesh_data).topology_data.topo_edges)[res].id.is_void() }
        }
        {
            res += 1;
        }

        Self {
            mesh_data,
            current_index: res,
            last_index: if MANIFOLD
            {
                unsafe { (*mesh_data).topology_data.topo_half_edges.len() }
            }
            else
            {
                unsafe { (*mesh_data).topology_data.topo_edges.len() }
            },
        }
    }
}

impl<C: MeshStorage, const MANIFOLD: bool> Iterator for EdgeIterator<C, MANIFOLD>
{
    type Item = HalfEdgeHandle<C, MANIFOLD>;

    fn next(&mut self) -> Option<Self::Item>
    {
        let res = self.current_index;

        let get_half_edge_id = |index: usize| unsafe {
            if MANIFOLD
            {
                (&(*self.mesh_data).topology_data.topo_half_edges)[index].id
            }
            else
            {
                let edge_id = (&(*self.mesh_data).topology_data.topo_edges)[index].id;
                (*self.mesh_data).edge_ref(edge_id).half_edge
            }
        };

        let is_index_void = |index: usize| unsafe {
            if MANIFOLD
            {
                (&(*self.mesh_data).topology_data.topo_half_edges)[index]
                    .id
                    .is_void()
            }
            else
            {
                (&(*self.mesh_data).topology_data.topo_edges)[index]
                    .id
                    .is_void()
            }
        };

        if res >= self.last_index
        {
            return None;
        }

        loop
        {
            self.current_index += 1 + (MANIFOLD as usize);
            if self.current_index >= self.last_index || !is_index_void(self.current_index)
            {
                break;
            }
        }

        Some(HalfEdgeHandle {
            id: get_half_edge_id(res),
            mesh_data: self.mesh_data,
        })
    }
}

/// Iterator over all the faces of a mesh.
pub struct FaceIterator<C: MeshStorage, const MANIFOLD: bool>
{
    mesh_data: *mut MeshData<C, MANIFOLD>,
    current_index: usize,
    last_index: usize,
}

impl<C: MeshStorage, const MANIFOLD: bool> FaceIterator<C, MANIFOLD>
{
    pub(crate) fn new(mesh_data: *mut MeshData<C, MANIFOLD>) -> Self
    {
        let mut res = 0;
        while unsafe { (&(*mesh_data).topology_data.topo_faces)[res].id.is_void() }
        {
            res += 1;
        }

        Self {
            mesh_data,
            current_index: res,
            last_index: unsafe { (*mesh_data).topology_data.topo_faces.len() },
        }
    }
}

impl<C: MeshStorage, const MANIFOLD: bool> Iterator for FaceIterator<C, MANIFOLD>
{
    type Item = FaceHandle<C, MANIFOLD>;

    fn next(&mut self) -> Option<Self::Item>
    {
        let res = self.current_index;
        if res >= self.last_index
        {
            return None;
        }

        loop
        {
            self.current_index += 1;
            if self.current_index >= self.last_index
            {
                break;
            }

            if !unsafe {
                (&(*self.mesh_data).topology_data.topo_faces)[self.current_index]
                    .id
                    .is_void()
            }
            {
                break;
            }
        }

        Some(FaceHandle {
            id: FaceId::new(res),
            mesh_data: self.mesh_data,
        })
    }
}

// +| Tests |+ =======================================================
#[cfg(test)]
mod tests
{
    use nalgebra::*;
    use wavefront_loader::ObjData;

    use super::*;
    use crate::verification::verify_correctness;
    type Vec3 = Vector3<f32>;

    #[test]
    fn test_single_triangle()
    {
        let triangle = vec![
            Vec3::new(1., 0., 0.),
            Vec3::new(0., 1., 0.),
            Vec3::new(0., 0., 0.),
        ];

        let test = BasicPolyHedron::<_, false>::new(
            triangle,
            (),
            [0, 1, 2].chunks(3).map(|l| l.iter().copied()),
        );

        verify_correctness(&test).unwrap();
    }

    #[test]
    fn test_closed_mesh()
    {
        let ObjData {
            vertices,
            vertex_face_indices,
            ..
        } = ObjData::from_disk_file("../../../Assets/bunny_closed.obj");

        let test = BasicPolyHedron::<_, false>::new(
            vertices.clone(),
            (),
            vertex_face_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        let (vs, fs) = test.face_list();
        ObjData::export(&(&vs, &fs), "nonmanifold_test.obj");

        let test = BasicPolyHedron::<_, true>::new(
            vertices.clone(),
            (),
            vertex_face_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        let (vs, fs) = test.face_list();
        ObjData::export(&(&vs, &fs), "manifold_test.obj");
    }

    #[test]
    fn test_open_mesh()
    {
        let ObjData {
            vertices,
            vertex_face_indices,
            ..
        } = ObjData::from_disk_file("../../../Assets/bunny.obj");

        let test = BasicPolyHedron::<_, false>::new(
            vertices.clone(),
            (),
            vertex_face_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        let (vs, fs) = test.face_list();
        ObjData::export(&(&vs, &fs), "nonmanifold_open_test.obj");

        let test = BasicPolyHedron::<_, true>::new(
            vertices.clone(),
            (),
            vertex_face_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        let (vs, fs) = test.face_list();
        ObjData::export(&(&vs, &fs), "manifold_open_test.obj");
    }

    #[test]
    fn test_topology_operators()
    {
        let ObjData {
            vertices,
            vertex_face_indices,
            ..
        } = ObjData::from_disk_file("../../../Assets/bunny_closed.obj");

        let test = BasicPolyHedron::<_, false>::new(
            vertices.clone(),
            (),
            vertex_face_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        test.half_edge_handle(HalfEdgeId::new(0)).flip();
        verify_correctness(&test).unwrap();

        let (vs, fs) = test.face_list();
        ObjData::export(&(&vs, &fs), "flip_test.obj");

        let ObjData {
            vertices,
            vertex_face_indices,
            ..
        } = ObjData::from_disk_file("../../../Assets/bunny_closed.obj");

        let test = BasicPolyHedron::<_, false>::new(
            vertices.clone(),
            (),
            vertex_face_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        test.half_edge_handle(HalfEdgeId::new(0)).split();
        verify_correctness(&test).unwrap();

        let (vs, fs) = test.face_list();
        ObjData::export(&(&vs, &fs), "split_test.obj");

        let ObjData {
            vertices,
            vertex_face_indices,
            ..
        } = ObjData::from_disk_file("../../../Assets/bunny_closed.obj");

        let test = BasicPolyHedron::<_, false>::new(
            vertices.clone(),
            (),
            vertex_face_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        test.half_edge_handle(HalfEdgeId::new(0)).collapse();
        verify_correctness(&test).unwrap();

        let (vs, fs) = test.face_list();
        ObjData::export(&(&vs, &fs), "collapse_test.obj");
    }

    #[test]
    fn test_iterators()
    {
        use std::f64::consts::PI;
        let num_points = 50;
        let mut vertices = Vec::with_capacity(num_points + 1);
        let mut triangle_indices = Vec::with_capacity(num_points * 3);

        vertices.push(Vector3::<f64>::default());

        for i in 0..num_points
        {
            let angle = 2.0 * PI * (i as f64) / (num_points as f64);
            vertices.push(Vector3::new(angle.cos(), angle.sin(), 0.0));
        }

        for i in 0..num_points
        {
            let current_vertex = i + 1;
            let next_vertex = (i + 1) % num_points + 1; // Next circle vertex (wraps to 1)

            triangle_indices.push([0, current_vertex, next_vertex]);
        }

        let test = BasicPolyHedron::<_, false>::new(
            vertices.clone(),
            (),
            triangle_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        for (i, v) in test
            .vert_handle(VertId::new(0))
            .iter_star_half_edges()
            .enumerate()
        {
            assert_eq!(v.dest().id().to_index(), i + 1);
        }

        let test = BasicPolyHedron::<_, true>::new(
            vertices.clone(),
            (),
            triangle_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&test).unwrap();

        for (i, v) in test
            .vert_handle(VertId::new(0))
            .iter_star_half_edges()
            .enumerate()
        {
            assert_eq!(v.dest().id().to_index(), 50 - i);
        }
    }
}