using System;
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using FlaxEngine;
using FlaxEngine.Assertions;
using Console = Game.Console;

namespace Game;

// Source: https://github.com/OskarSigvardsson/unity-quickhull/blob/master/Scripts/ConvexHullCalculator.cs

/// <summary>
///     An implementation of the quickhull algorithm for generating 3d convex
///     hulls.
///     The algorithm works like this: you start with an initial "seed" hull,
///     that is just a simple tetrahedron made up of four points in the point
///     cloud. This seed hull is then grown until it all the points in the
///     point cloud is inside of it, at which point it will be the convex hull
///     for the entire set.
///     All of the points in the point cloud is divided into two parts, the
///     "open set" and the "closed set". The open set consists of all the
///     points outside of the tetrahedron, and the closed set is all of the
///     points inside the tetrahedron. After each iteration of the algorithm,
///     the closed set gets bigger and the open set get smaller. When the open
///     set is empty, the algorithm is finished.
///     Each point in the open set is assigned to a face that it lies outside
///     of. To grow the hull, the point in the open set which is farthest from
///     it's face is chosen. All faces which are facing that point (I call
///     them "lit faces" in the code, because if you imagine the point as a
///     point light, it's the set of points which would be lit by that point
///     light) are removed, and a "horizon" of edges is found from where the
///     faces were removed. From this horizon, new faces are constructed in a
///     "cone" like fashion connecting the point to the edges.
///     To keep track of the faces, I use a struct for each face which
///     contains the three vertices of the face in CCW order, as well as the
///     three triangles which share an edge. I was considering doing a
///     half-edge structure to store the mesh, but it's not needed. Using a
///     struct for each face and neighbors simplify the algorithm and makes it
///     easy to export it as a mesh.
///     The most subtle part of the algorithm is finding the horizon. In order
///     to properly construct the cone so that all neighbors are kept
///     consistent, you can do a depth-first search from the first lit face.
///     If the depth-first search always proceeeds in a counter-clockwise
///     fashion, it guarantees that the horizon will be found in a
///     counter-clockwise order, which makes it easy to construct the cone of
///     new faces.
///     A note: the code uses a right-handed coordinate system, where the
///     cross-product uses the right-hand rule and the faces are in CCW order.
///     At the end of the algorithm, the hull is exported in a Unity-friendly
///     fashion, with a left-handed mesh.
/// </summary>
public class ConvexHullCalculator
{
    /// <summary>
    ///     Constant representing a point that has yet to be assigned to a
    ///     face. It's only used immediately after constructing the seed hull.
    /// </summary>
    private const int UNASSIGNED = -2;

    /// <summary>
    ///     Constant representing a point that is inside the convex hull, and
    ///     thus is behind all faces. In the openSet array, all points with
    ///     INSIDE are at the end of the array, with indexes larger
    ///     openSetTail.
    /// </summary>
    private const int INSIDE = -1;

    /// <summary>
    ///     Epsilon value. If the coordinates of the point space are
    ///     exceptionally close to each other, this value might need to be
    ///     adjusted.
    /// </summary>
    private const float EPSILON = 0.001f;

    /// <summary>
    ///     When adding a new face to the faces Dictionary, use this for the
    ///     key and then increment it.
    /// </summary>
    private int faceCount;

    /// <summary>
    ///     A dictionary storing the faces of the currently generated convex
    ///     hull. The key is the id of the face, used in the Face, PointFace
    ///     and HorizonEdge struct.
    ///     This is a Dictionary, because we need both random access to it,
    ///     the ability to loop through it, and ability to quickly delete
    ///     faces (in the ConstructCone method), and Dictionary is the obvious
    ///     candidate that can do all of those things.
    ///     I'm wondering if using a Dictionary is best idea, though. It might
    ///     be better to just have them in a <![CDATA[List<Face>]]> and mark a face as
    ///     deleted by adding a field to the Face struct. The downside is that
    ///     we would need an extra field in the Face struct, and when we're
    ///     looping through the points in openSet, we would have to loop
    ///     through all the Faces EVER created in the algorithm, and skip the
    ///     ones that have been marked as deleted. However, looping through a
    ///     list is fairly fast, and it might be worth it to avoid Dictionary
    ///     overhead.
    ///     TODO test converting to a <![CDATA[List<Face>]]> instead.
    /// </summary>
    private Dictionary<int, Face> faces;

    /// <summary>
    ///     The current horizon. Generated by the FindHorizon() DFS search,
    ///     and used in ConstructCone to construct new faces. The list of
    ///     edges are in CCW order.
    /// </summary>
    private List<HorizonEdge> horizon;

    /// <summary>
    ///     If SplitVerts is false, this Dictionary is used to keep track of
    ///     which points we've added to the final mesh.
    /// </summary>
    private Dictionary<int, int> hullVerts;

    /// <summary>
    ///     Set of faces which are "lit" by the current point in the set. This
    ///     is used in the FindHorizon() DFS search to keep track of which
    ///     faces we've already visited, and in the ReassignPoints() method to
    ///     know which points need to be reassigned.
    /// </summary>
    private HashSet<int> litFaces;

    /// <summary>
    ///     The set of points to be processed. "openSet" is a misleading name,
    ///     because it's both the open set (points which are still outside the
    ///     convex hull) and the closed set (points that are inside the convex
    ///     hull). The first part of the array (with <![CDATA[indexes <= openSetTail]]>)
    ///     is the openSet, the last part of the array (with <![CDATA[indexes >
    ///   openSetTail]]>) are the closed set, with
    ///     Face set to INSIDE. The
    ///     closed set is largely irrelevant to the algorithm, the open set is
    ///     what matters.
    ///     Storing the entire open set in one big list has a downside: when
    ///     we're reassigning points after ConstructCone, we only need to
    ///     reassign points that belong to the faces that have been removed,
    ///     but storing it in one array, we have to loop through the entire
    ///     list, and checking litFaces to determine which we can skip and
    ///     which need to be reassigned.
    ///     The alternative here is to give each face in Face array it's own
    ///     openSet. I don't like that solution, because then you have to
    ///     juggle so many more heap-allocated <![CDATA[List<T>'s]]>, we'd have to use
    ///     object pools and such. It would do a lot more allocation, and it
    ///     would have worse locality. I should maybe test that solution, but
    ///     it probably wont be faster enough (if at all) to justify the extra
    ///     allocations.
    /// </summary>
    private List<PointFace> openSet;

    /// <summary>
    ///     The "tail" of the openSet, the last index of a vertex that has
    ///     been assigned to a face.
    /// </summary>
    private int openSetTail = -1;

    /// <summary>
    ///     Generate a convex hull from points in points array, and store the
    ///     mesh in Unity-friendly format in verts and tris. If splitVerts is
    ///     true, the the verts will be split, if false, the same vert will be
    ///     used for more than one triangle.
    /// </summary>
    public void GenerateHull(
        List<Float3> points,
        bool splitVerts,
        ref List<Float3> verts,
        ref List<int> tris,
        ref List<Float3> normals)
    {
        if (points.Count < 4)
            throw new ArgumentException("Need at least 4 points to generate a convex hull");

        Initialize(points, splitVerts);

        GenerateInitialHull(points);

        while (openSetTail >= 0)
            GrowHull(points);

        ExportMesh(points, splitVerts, ref verts, ref tris, ref normals);
        //VerifyMesh(points, ref verts, ref tris);
    }

    /// <summary>
    ///     Make sure all the buffers and variables needed for the algorithm
    ///     are initialized.
    /// </summary>
    private void Initialize(List<Float3> points, bool splitVerts)
    {
        faceCount = 0;
        openSetTail = -1;

        if (faces == null)
        {
            faces = new Dictionary<int, Face>();
            litFaces = new HashSet<int>();
            horizon = new List<HorizonEdge>();
            openSet = new List<PointFace>(points.Count);
        }
        else
        {
            faces.Clear();
            litFaces.Clear();
            horizon.Clear();
            openSet.Clear();

            if (openSet.Capacity < points.Count) // i wonder if this is a good idea... if you call
                // GenerateHull over and over with slightly increasing
                // points counts, it's going to reallocate every time. Maybe
                // i should just use .Add(), and let the List<T> manage the
                // capacity, increasing it geometrically every time we need
                // to reallocate.

                // maybe do
                //   openSet.Capacity = Mathf.NextPowerOfTwo(points.Count)
                // instead?

                openSet.Capacity = points.Count;
        }

        if (!splitVerts)
        {
            if (hullVerts == null)
                hullVerts = new Dictionary<int, int>();
            else
                hullVerts.Clear();
        }
    }

    /// <summary>
    ///     Create initial seed hull.
    /// </summary>
    private void GenerateInitialHull(List<Float3> points)
    {
        // Find points suitable for use as the seed hull. Some varieties of
        // this algorithm pick extreme points here, but I'm not convinced
        // you gain all that much from that. Currently what it does is just
        // find the first four points that are not coplanar.
        int b0, b1, b2, b3;
        FindInitialHullIndices(points, out b0, out b1, out b2, out b3);

        Float3 v0 = points[b0];
        Float3 v1 = points[b1];
        Float3 v2 = points[b2];
        Float3 v3 = points[b3];

        bool above = Dot(v3 - v1, Cross(v1 - v0, v2 - v0)) > 0.0f;

        // Create the faces of the seed hull. You need to draw a diagram
        // here, otherwise it's impossible to know what's going on :)

        // Basically: there are two different possible start-tetrahedrons,
        // depending on whether the fourth point is above or below the base
        // triangle. If you draw a tetrahedron with these coordinates (in a
        // right-handed coordinate-system):

        //   b0 = (0,0,0)
        //   b1 = (1,0,0)
        //   b2 = (0,1,0)
        //   b3 = (0,0,1)

        // you can see the first case (set b3 = (0,0,-1) for the second
        // case). The faces are added with the proper references to the
        // faces opposite each vertex

        faceCount = 0;
        if (above)
        {
            faces[faceCount++] = new Face(b0, b2, b1, 3, 1, 2, Normal(points[b0], points[b2], points[b1]));
            faces[faceCount++] = new Face(b0, b1, b3, 3, 2, 0, Normal(points[b0], points[b1], points[b3]));
            faces[faceCount++] = new Face(b0, b3, b2, 3, 0, 1, Normal(points[b0], points[b3], points[b2]));
            faces[faceCount++] = new Face(b1, b2, b3, 2, 1, 0, Normal(points[b1], points[b2], points[b3]));
        }
        else
        {
            faces[faceCount++] = new Face(b0, b1, b2, 3, 2, 1, Normal(points[b0], points[b1], points[b2]));
            faces[faceCount++] = new Face(b0, b3, b1, 3, 0, 2, Normal(points[b0], points[b3], points[b1]));
            faces[faceCount++] = new Face(b0, b2, b3, 3, 1, 0, Normal(points[b0], points[b2], points[b3]));
            faces[faceCount++] = new Face(b1, b3, b2, 2, 0, 1, Normal(points[b1], points[b3], points[b2]));
        }

        VerifyFaces(points);

        // Create the openSet. Add all points except the points of the seed
        // hull.
        for (int i = 0; i < points.Count; i++)
        {
            if (i == b0 || i == b1 || i == b2 || i == b3)
                continue;

            openSet.Add(new PointFace(i, UNASSIGNED, 0.0f));
        }

        // Add the seed hull verts to the tail of the list.
        openSet.Add(new PointFace(b0, INSIDE, float.NaN));
        openSet.Add(new PointFace(b1, INSIDE, float.NaN));
        openSet.Add(new PointFace(b2, INSIDE, float.NaN));
        openSet.Add(new PointFace(b3, INSIDE, float.NaN));

        // Set the openSetTail value. Last item in the array is
        // openSet.Count - 1, but four of the points (the verts of the seed
        // hull) are part of the closed set, so move openSetTail to just
        // before those.
        openSetTail = openSet.Count - 5;

        Assert.IsTrue(openSet.Count == points.Count);

        // Assign all points of the open set. This does basically the same
        // thing as ReassignPoints()
        for (int i = 0; i <= openSetTail; i++)
        {
            Assert.IsTrue(openSet[i].Face == UNASSIGNED);
            Assert.IsTrue(openSet[openSetTail].Face == UNASSIGNED);
            Assert.IsTrue(openSet[openSetTail + 1].Face == INSIDE);

            bool assigned = false;
            PointFace fp = openSet[i];

            Assert.IsTrue(faces.Count == 4);
            Assert.IsTrue(faces.Count == faceCount);
            for (int j = 0; j < 4; j++)
            {
                Assert.IsTrue(faces.ContainsKey(j));

                Face face = faces[j];

                float dist = PointFaceDistance(points[fp.Point], points[face.Vertex0], face);

                if (dist > 0)
                {
                    fp.Face = j;
                    fp.Distance = dist;
                    openSet[i] = fp;

                    assigned = true;
                    break;
                }
            }

            if (!assigned)
            {
                // Point is inside
                fp.Face = INSIDE;
                fp.Distance = float.NaN;

                // Point is inside seed hull: swap point with tail, and move
                // openSetTail back. We also have to decrement i, because
                // there's a new item at openSet[i], and we need to process
                // it next iteration
                openSet[i] = openSet[openSetTail];
                openSet[openSetTail] = fp;

                openSetTail -= 1;
                i -= 1;
            }
        }

        VerifyOpenSet(points);
    }

    /// <summary>
    ///     Find four points in the point cloud that are not coplanar for the
    ///     seed hull
    /// </summary>
    private void FindInitialHullIndices(List<Float3> points, out int b0, out int b1, out int b2, out int b3)
    {
        int count = points.Count;

        for (int i0 = 0; i0 < count - 3; i0++)
        for (int i1 = i0 + 1; i1 < count - 2; i1++)
        {
            Float3 p0 = points[i0];
            Float3 p1 = points[i1];

            if (AreCoincident(p0, p1))
                continue;

            for (int i2 = i1 + 1; i2 < count - 1; i2++)
            {
                Float3 p2 = points[i2];

                if (AreCollinear(p0, p1, p2))
                    continue;

                for (int i3 = i2 + 1; i3 < count - 0; i3++)
                {
                    Float3 p3 = points[i3];

                    if (AreCoplanar(p0, p1, p2, p3))
                        continue;

                    b0 = i0;
                    b1 = i1;
                    b2 = i2;
                    b3 = i3;
                    return;
                }
            }
        }

        throw new ArgumentException("Can't generate hull, points are coplanar");
    }

    /// <summary>
    ///     Grow the hull. This method takes the current hull, and expands it
    ///     to encompass the point in openSet with the point furthest away
    ///     from its face.
    /// </summary>
    private void GrowHull(List<Float3> points)
    {
        Assert.IsTrue(openSetTail >= 0);
        Assert.IsTrue(openSet[0].Face != INSIDE);

        // Find farthest point and first lit face.
        int farthestPoint = 0;
        float dist = openSet[0].Distance;

        for (int i = 1; i <= openSetTail; i++)
            if (openSet[i].Distance > dist)
            {
                farthestPoint = i;
                dist = openSet[i].Distance;
            }

        // Use lit face to find horizon and the rest of the lit
        // faces.
        FindHorizon(
            points,
            points[openSet[farthestPoint].Point],
            openSet[farthestPoint].Face,
            faces[openSet[farthestPoint].Face]);

        VerifyHorizon();

        // Construct new cone from horizon
        ConstructCone(points, openSet[farthestPoint].Point);

        VerifyFaces(points);

        // Reassign points
        ReassignPoints(points);
    }

    /// <summary>
    ///     Start the search for the horizon.
    ///     The search is a DFS search that searches neighboring triangles in
    ///     a counter-clockwise fashion. When it find a neighbor which is not
    ///     lit, that edge will be a line on the horizon. If the search always
    ///     proceeds counter-clockwise, the edges of the horizon will be found
    ///     in counter-clockwise order.
    ///     The heart of the search can be found in the recursive
    ///     SearchHorizon() method, but the the first iteration of the search
    ///     is special, because it has to visit three neighbors (all the
    ///     neighbors of the initial triangle), while the rest of the search
    ///     only has to visit two (because one of them has already been
    ///     visited, the one you came from).
    /// </summary>
    private void FindHorizon(List<Float3> points, Float3 point, int fi, Face face)
    {
        // TODO should I use epsilon in the PointFaceDistance comparisons?

        litFaces.Clear();
        horizon.Clear();

        litFaces.Add(fi);

        Assert.IsTrue(PointFaceDistance(point, points[face.Vertex0], face) > 0.0f);

        // For the rest of the recursive search calls, we first check if the
        // triangle has already been visited and is part of litFaces.
        // However, in this first call we can skip that because we know it
        // can't possibly have been visited yet, since the only thing in
        // litFaces is the current triangle.
        {
            Face oppositeFace = faces[face.Opposite0];

            float dist = PointFaceDistance(
                point,
                points[oppositeFace.Vertex0],
                oppositeFace);

            if (dist <= 0.0f)
                horizon.Add(new HorizonEdge
                {
                    Face = face.Opposite0,
                    Edge0 = face.Vertex1,
                    Edge1 = face.Vertex2
                });
            else
                SearchHorizon(points, point, fi, face.Opposite0, oppositeFace);
        }

        if (!litFaces.Contains(face.Opposite1))
        {
            Face oppositeFace = faces[face.Opposite1];

            float dist = PointFaceDistance(
                point,
                points[oppositeFace.Vertex0],
                oppositeFace);

            if (dist <= 0.0f)
                horizon.Add(new HorizonEdge
                {
                    Face = face.Opposite1,
                    Edge0 = face.Vertex2,
                    Edge1 = face.Vertex0
                });
            else
                SearchHorizon(points, point, fi, face.Opposite1, oppositeFace);
        }

        if (!litFaces.Contains(face.Opposite2))
        {
            Face oppositeFace = faces[face.Opposite2];

            float dist = PointFaceDistance(
                point,
                points[oppositeFace.Vertex0],
                oppositeFace);

            if (dist <= 0.0f)
                horizon.Add(new HorizonEdge
                {
                    Face = face.Opposite2,
                    Edge0 = face.Vertex0,
                    Edge1 = face.Vertex1
                });
            else
                SearchHorizon(points, point, fi, face.Opposite2, oppositeFace);
        }
    }

    /// <summary>
    ///     Recursively search to find the horizon or lit set.
    /// </summary>
    private void SearchHorizon(List<Float3> points, Float3 point, int prevFaceIndex, int faceCount, Face face)
    {
        Assert.IsTrue(prevFaceIndex >= 0);
        Assert.IsTrue(litFaces.Contains(prevFaceIndex));
        Assert.IsTrue(!litFaces.Contains(faceCount));
        Assert.IsTrue(faces[faceCount].Equals(face));

        litFaces.Add(faceCount);

        // Use prevFaceIndex to determine what the next face to search will
        // be, and what edges we need to cross to get there. It's important
        // that the search proceeds in counter-clockwise order from the
        // previous face.
        int nextFaceIndex0;
        int nextFaceIndex1;
        int edge0;
        int edge1;
        int edge2;

        if (prevFaceIndex == face.Opposite0)
        {
            nextFaceIndex0 = face.Opposite1;
            nextFaceIndex1 = face.Opposite2;

            edge0 = face.Vertex2;
            edge1 = face.Vertex0;
            edge2 = face.Vertex1;
        }
        else if (prevFaceIndex == face.Opposite1)
        {
            nextFaceIndex0 = face.Opposite2;
            nextFaceIndex1 = face.Opposite0;

            edge0 = face.Vertex0;
            edge1 = face.Vertex1;
            edge2 = face.Vertex2;
        }
        else
        {
            Assert.IsTrue(prevFaceIndex == face.Opposite2);

            nextFaceIndex0 = face.Opposite0;
            nextFaceIndex1 = face.Opposite1;

            edge0 = face.Vertex1;
            edge1 = face.Vertex2;
            edge2 = face.Vertex0;
        }

        if (!litFaces.Contains(nextFaceIndex0))
        {
            Face oppositeFace = faces[nextFaceIndex0];

            float dist = PointFaceDistance(
                point,
                points[oppositeFace.Vertex0],
                oppositeFace);

            if (dist <= 0.0f)
                horizon.Add(new HorizonEdge
                {
                    Face = nextFaceIndex0,
                    Edge0 = edge0,
                    Edge1 = edge1
                });
            else
                SearchHorizon(points, point, faceCount, nextFaceIndex0, oppositeFace);
        }

        if (!litFaces.Contains(nextFaceIndex1))
        {
            Face oppositeFace = faces[nextFaceIndex1];

            float dist = PointFaceDistance(
                point,
                points[oppositeFace.Vertex0],
                oppositeFace);

            if (dist <= 0.0f)
                horizon.Add(new HorizonEdge
                {
                    Face = nextFaceIndex1,
                    Edge0 = edge1,
                    Edge1 = edge2
                });
            else
                SearchHorizon(points, point, faceCount, nextFaceIndex1, oppositeFace);
        }
    }

    /// <summary>
    ///     Remove all lit faces and construct new faces from the horizon in a
    ///     "cone-like" fashion.
    ///     This is a relatively straight-forward procedure, given that the
    ///     horizon is handed to it in already sorted counter-clockwise. The
    ///     neighbors of the new faces are easy to find: they're the previous
    ///     and next faces to be constructed in the cone, as well as the face
    ///     on the other side of the horizon. We also have to update the face
    ///     on the other side of the horizon to reflect it's new neighbor from
    ///     the cone.
    /// </summary>
    private void ConstructCone(List<Float3> points, int farthestPoint)
    {
        foreach (int fi in litFaces)
        {
            Assert.IsTrue(faces.ContainsKey(fi));
            faces.Remove(fi);
        }

        int firstNewFace = faceCount;

        // Check for coplanar faces
        /*var firstNormal = new Float3(0, 0, 0);
        int sameNormals = 1;
        for (int i = 0; i < horizon.Count; i++)
        {
            var v0 = farthestPoint;
            var v1 = horizon[i].Edge0;
            var v2 = horizon[i].Edge1;
            var norm = Normal(points[v0], points[v1], points[v2]);
            if (firstNormal.IsZero)
            {
                firstNormal = norm;
                continue;
            }

            const float tol = 0.1f;
            if ((firstNormal - norm).Length < tol || (firstNormal - norm).Length > 2.0f-tol)
            {
                sameNormals++;
            }
        }

        if (sameNormals == horizon.Count)
            sameNormals = sameNormals;*/

        for (int i = 0; i < horizon.Count; i++)
        {
            // Vertices of the new face, the farthest point as well as the
            // edge on the horizon. Horizon edge is CCW, so the triangle
            // should be as well.
            int v0 = farthestPoint;
            int v1 = horizon[i].Edge0;
            int v2 = horizon[i].Edge1;

            // Opposite faces of the triangle. First, the edge on the other
            // side of the horizon, then the next/prev faces on the new cone
            int o0 = horizon[i].Face;
            int o1 = i == horizon.Count - 1 ? firstNewFace : firstNewFace + i + 1;
            int o2 = i == 0 ? firstNewFace + horizon.Count - 1 : firstNewFace + i - 1;

            int fi = faceCount++;

            faces[fi] = new Face(
                v0, v1, v2,
                o0, o1, o2,
                Normal(points[v0], points[v1], points[v2]));

            Face horizonFace = faces[horizon[i].Face];

            if (horizonFace.Vertex0 == v1)
            {
                Assert.IsTrue(v2 == horizonFace.Vertex2);
                horizonFace.Opposite1 = fi;
            }
            else if (horizonFace.Vertex1 == v1)
            {
                Assert.IsTrue(v2 == horizonFace.Vertex0);
                horizonFace.Opposite2 = fi;
            }
            else
            {
                Assert.IsTrue(v1 == horizonFace.Vertex2);
                Assert.IsTrue(v2 == horizonFace.Vertex1);
                horizonFace.Opposite0 = fi;
            }

            faces[horizon[i].Face] = horizonFace;
        }
    }

    /// <summary>
    ///     Reassign points based on the new faces added by ConstructCone().
    ///     Only points that were previous assigned to a removed face need to
    ///     be updated, so check litFaces while looping through the open set.
    ///     There is a potential optimization here: there's no reason to loop
    ///     through the entire openSet here. If each face had it's own
    ///     openSet, we could just loop through the openSets in the removed
    ///     faces. That would make the loop here shorter.
    ///     However, to do that, we would have to juggle A LOT more <![CDATA[List<T>'s]]>,
    ///     and we would need an object pool to manage them all without
    ///     generating a whole bunch of garbage. I don't think it's worth
    ///     doing that to make this loop shorter, a straight for-loop through
    ///     a list is pretty darn fast. Still, it might be worth trying
    /// </summary>
    private void ReassignPoints(List<Float3> points)
    {
        for (int i = 0; i <= openSetTail; i++)
        {
            PointFace fp = openSet[i];

            if (litFaces.Contains(fp.Face))
            {
                bool assigned = false;
                Float3 point = points[fp.Point];

                foreach (var kvp in faces)
                {
                    int fi = kvp.Key;
                    Face face = kvp.Value;

                    float dist = PointFaceDistance(
                        point,
                        points[face.Vertex0],
                        face);

                    if (dist > EPSILON)
                    {
                        assigned = true;

                        fp.Face = fi;
                        fp.Distance = dist;

                        openSet[i] = fp;
                        break;
                    }
                }

                if (!assigned)
                {
                    // If point hasn't been assigned, then it's inside the
                    // convex hull. Swap it with openSetTail, and decrement
                    // openSetTail. We also have to decrement i, because
                    // there's now a new thing in openSet[i], so we need i
                    // to remain the same the next iteration of the loop.
                    fp.Face = INSIDE;
                    fp.Distance = float.NaN;

                    openSet[i] = openSet[openSetTail];
                    openSet[openSetTail] = fp;

                    i--;
                    openSetTail--;
                }
            }
        }
    }

    /// <summary>
    ///     Final step in algorithm, export the faces of the convex hull in a
    ///     mesh-friendly format.
    ///     TODO normals calculation for non-split vertices. Right now it just
    ///     leaves the normal array empty.
    /// </summary>
    private void ExportMesh(
        List<Float3> points,
        bool splitVerts,
        ref List<Float3> verts,
        ref List<int> tris,
        ref List<Float3> normals)
    {
        if (verts == null)
            verts = new List<Float3>();
        else
            verts.Clear();

        if (tris == null)
            tris = new List<int>();
        else
            tris.Clear();

        if (normals == null)
            normals = new List<Float3>();
        else
            normals.Clear();

        foreach (Face face in faces.Values)
        {
            int vi0, vi1, vi2;

            if (splitVerts)
            {
                vi0 = verts.Count;
                verts.Add(points[face.Vertex0]);
                vi1 = verts.Count;
                verts.Add(points[face.Vertex1]);
                vi2 = verts.Count;
                verts.Add(points[face.Vertex2]);

                normals.Add(face.Normal);
                normals.Add(face.Normal);
                normals.Add(face.Normal);
            }
            else
            {
                if (!hullVerts.TryGetValue(face.Vertex0, out vi0))
                {
                    vi0 = verts.Count;
                    hullVerts[face.Vertex0] = vi0;
                    verts.Add(points[face.Vertex0]);
                }

                if (!hullVerts.TryGetValue(face.Vertex1, out vi1))
                {
                    vi1 = verts.Count;
                    hullVerts[face.Vertex1] = vi1;
                    verts.Add(points[face.Vertex1]);
                }

                if (!hullVerts.TryGetValue(face.Vertex2, out vi2))
                {
                    vi2 = verts.Count;
                    hullVerts[face.Vertex2] = vi2;
                    verts.Add(points[face.Vertex2]);
                }
            }

            tris.Add(vi0);
            tris.Add(vi1);
            tris.Add(vi2);
        }
    }

    /// <summary>
    ///     Signed distance from face to point (a positive number means that
    ///     the point is above the face)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private float PointFaceDistance(Float3 point, Float3 pointOnFace, Face face)
    {
        return Dot(face.Normal, point - pointOnFace);
    }

    /// <summary>
    ///     Calculate normal for triangle
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private Float3 Normal(Float3 v0, Float3 v1, Float3 v2)
    {
        return Cross(v1 - v0, v2 - v0).Normalized;
    }

    /// <summary>
    ///     Dot product, for convenience.
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static float Dot(Float3 a, Float3 b)
    {
        return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
    }

    /// <summary>
    ///     Float3.Cross i left-handed, the algorithm is right-handed. Also,
    ///     i wanna test to see if using aggressive inlining makes any
    ///     difference here.
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static Float3 Cross(Float3 a, Float3 b)
    {
        return new Float3(
            a.Y * b.Z - a.Z * b.Y,
            a.Z * b.X - a.X * b.Z,
            a.X * b.Y - a.Y * b.X);
    }

    /// <summary>
    ///     Check if two points are coincident
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private bool AreCoincident(Float3 a, Float3 b)
    {
        return (a - b).Length <= EPSILON;
    }

    /// <summary>
    ///     Check if three points are collinear
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private bool AreCollinear(Float3 a, Float3 b, Float3 c)
    {
        return Cross(c - a, c - b).Length <= EPSILON;
    }

    /// <summary>
    ///     Check if four points are coplanar
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private bool AreCoplanar(Float3 a, Float3 b, Float3 c, Float3 d)
    {
        Float3 n1 = Cross(c - a, c - b);
        Float3 n2 = Cross(d - a, d - b);

        float m1 = n1.Length;
        float m2 = n2.Length;

        return m1 <= EPSILON
                || m2 <= EPSILON
                || AreCollinear(Float3.Zero,
                    1.0f / m1 * n1,
                    1.0f / m2 * n2);
    }

    /// <summary>
    ///     Method used for debugging, verifies that the openSet is in a
    ///     sensible state. Conditionally compiled if DEBUG_QUICKHULL if
    ///     defined.
    /// </summary>
    private void VerifyOpenSet(List<Float3> points)
    {
        for (int i = 0; i < openSet.Count; i++)
            if (i > openSetTail)
            {
                Assert.IsTrue(openSet[i].Face == INSIDE);
            }
            else
            {
                Assert.IsTrue(openSet[i].Face != INSIDE);
                Assert.IsTrue(openSet[i].Face != UNASSIGNED);

                Assert.IsTrue(PointFaceDistance(
                    points[openSet[i].Point],
                    points[faces[openSet[i].Face].Vertex0],
                    faces[openSet[i].Face]) > 0.0f);
            }
    }

    /// <summary>
    ///     Method used for debugging, verifies that the horizon is in a
    ///     sensible state. Conditionally compiled if DEBUG_QUICKHULL if
    ///     defined.
    /// </summary>
    private void VerifyHorizon()
    {
        for (int i = 0; i < horizon.Count; i++)
        {
            int prev = i == 0 ? horizon.Count - 1 : i - 1;

            Assert.IsTrue(horizon[prev].Edge1 == horizon[i].Edge0);
            Assert.IsTrue(HasEdge(faces[horizon[i].Face], horizon[i].Edge1, horizon[i].Edge0));
        }
    }

    /// <summary>
    ///     Method used for debugging, verifies that the faces array is in a
    ///     sensible state. Conditionally compiled if DEBUG_QUICKHULL if
    ///     defined.
    /// </summary>
    private void VerifyFaces(List<Float3> points)
    {
        foreach (var kvp in faces)
        {
            int fi = kvp.Key;
            Face face = kvp.Value;

            Assert.IsTrue(faces.ContainsKey(face.Opposite0));
            Assert.IsTrue(faces.ContainsKey(face.Opposite1));
            Assert.IsTrue(faces.ContainsKey(face.Opposite2));

            Assert.IsTrue(face.Opposite0 != fi);
            Assert.IsTrue(face.Opposite1 != fi);
            Assert.IsTrue(face.Opposite2 != fi);

            Assert.IsTrue(face.Vertex0 != face.Vertex1);
            Assert.IsTrue(face.Vertex0 != face.Vertex2);
            Assert.IsTrue(face.Vertex1 != face.Vertex2);

            Assert.IsTrue(HasEdge(faces[face.Opposite0], face.Vertex2, face.Vertex1));
            Assert.IsTrue(HasEdge(faces[face.Opposite1], face.Vertex0, face.Vertex2));
            Assert.IsTrue(HasEdge(faces[face.Opposite2], face.Vertex1, face.Vertex0));

            Assert.IsTrue((face.Normal - Normal(
                points[face.Vertex0],
                points[face.Vertex1],
                points[face.Vertex2])).Length < EPSILON);
        }
    }

    /// <summary>
    ///     Method used for debugging, verifies that the final mesh is
    ///     actually a convex hull of all the points. Conditionally compiled
    ///     if DEBUG_QUICKHULL if defined.
    /// </summary>
    private void VerifyMesh(List<Float3> points, ref List<Float3> verts, ref List<int> tris)
    {
        Assert.IsTrue(tris.Count % 3 == 0);

        for (int i = 0; i < points.Count; i++)
        for (int j = 0; j < tris.Count; j += 3)
        {
            Float3 t0 = verts[tris[j]];
            Float3 t1 = verts[tris[j + 1]];
            Float3 t2 = verts[tris[j + 2]];

            float dot = Dot(points[i] - t0, Float3.Cross(t1 - t0, t2 - t0));
            //Assert.IsTrue(dot <= EPSILON, $"not convex hull: {dot} > {EPSILON}");
            if (!(dot <= EPSILON))
                Console.PrintError($"not convex hull: {dot} > {EPSILON}");
        }
    }

    /// <summary>
    ///     Does face f have a face with vertexes e0 and e1? Used only for
    ///     debugging.
    /// </summary>
    private bool HasEdge(Face f, int e0, int e1)
    {
        return (f.Vertex0 == e0 && f.Vertex1 == e1)
                || (f.Vertex1 == e0 && f.Vertex2 == e1)
                || (f.Vertex2 == e0 && f.Vertex0 == e1);
    }

    /// <summary>
    ///     Struct representing a single face.
    ///     Vertex0, Vertex1 and Vertex2 are the vertices in CCW order. They
    ///     acutal points are stored in the points array, these are just
    ///     indexes into that array.
    ///     Opposite0, Opposite1 and Opposite2 are the keys to the faces which
    ///     share an edge with this face. Opposite0 is the face opposite
    ///     Vertex0 (so it has an edge with Vertex2 and Vertex1), etc.
    ///     Normal is (unsurprisingly) the normal of the triangle.
    /// </summary>
    private struct Face
    {
        public readonly int Vertex0;
        public readonly int Vertex1;
        public readonly int Vertex2;

        public int Opposite0;
        public int Opposite1;
        public int Opposite2;

        public readonly Float3 Normal;

        public Face(int v0, int v1, int v2, int o0, int o1, int o2, Float3 normal)
        {
            Vertex0 = v0;
            Vertex1 = v1;
            Vertex2 = v2;
            Opposite0 = o0;
            Opposite1 = o1;
            Opposite2 = o2;
            Normal = normal;
        }

        public bool Equals(Face other)
        {
            return Vertex0 == other.Vertex0
                    && Vertex1 == other.Vertex1
                    && Vertex2 == other.Vertex2
                    && Opposite0 == other.Opposite0
                    && Opposite1 == other.Opposite1
                    && Opposite2 == other.Opposite2
                    && Normal == other.Normal;
        }
    }

    /// <summary>
    ///     Struct representing a mapping between a point and a face. These
    ///     are used in the openSet array.
    ///     Point is the index of the point in the points array, Face is the
    ///     key of the face in the Key dictionary, Distance is the distance
    ///     from the face to the point.
    /// </summary>
    private struct PointFace
    {
        public readonly int Point;
        public int Face;
        public float Distance;

        public PointFace(int p, int f, float d)
        {
            Point = p;
            Face = f;
            Distance = d;
        }
    }

    /// <summary>
    ///     Struct representing a single edge in the horizon.
    ///     Edge0 and Edge1 are the vertexes of edge in CCW order, Face is the
    ///     face on the other side of the horizon.
    ///     TODO Edge1 isn't actually needed, you can just index the next item
    ///     in the horizon array.
    /// </summary>
    private struct HorizonEdge
    {
        public int Face;
        public int Edge0;
        public int Edge1;
    }
}