import vmath
import std/random
import std/sets
import std/sequtils

type Face* = object
    points*: array[3, Vec3]
    plane*: Vec4

func plane_from_norm_point(normal, point: Vec3): Vec4 =
    let n = normal.normalize
    let p = point
    vec4(n.x, n.y, n.z, -(n.x*p.x+n.y*p.y+n.z*p.z))

func face_from_triangle(a,b,c: Vec3): Face =
    let n = cross(b-a, c-a)
    Face(points: [a,b,c], plane: plane_from_norm_point(n, a))

template distance_to_plane(point: Vec3, plane: Vec4): float32 =
    dot(plane, vec4(point, 1.0))

proc add_point_to_hull(point: Vec3, hull: var seq[Face]) =
    var horizon_edges: seq[(Vec3, Vec3)] = @[]
    var new_faces: seq[Face] = @[]

    for face in hull:
        if distance_to_plane(point, face.plane) > 0:
            # Store the edges of the horizon
            for i in 0 .. 2:
                let edge = (face.points[i], face.points[(i+1) mod 3])
                let other = (edge[1], edge[0])
                let other_pos = horizon_edges.find other
                if other_pos >= 0:
                    horizon_edges.del other_pos
                else:
                    horizon_edges.add edge
        else:
            new_faces.add face

    for (a, b) in horizon_edges:
        new_faces.add face_from_triangle(a, b, point)

    hull = new_faces

proc quickhull*(points: seq[Vec3]): seq[Face] =
    if points.len <= 4:
        return
    var hull: seq[Face]

    var points = points
    shuffle(points)
    
    let p0 = points[0]
    let p1 = points[1]
    let p2 = points[2]
    let p3 = points[3]

    let f = face_from_triangle(p0, p1, p2)
    # Ensure all faces point outwards
    if distance_to_plane(p3, f.plane) < 0:
        hull.add f
        hull.add face_from_triangle(p0, p2, p3)
        hull.add face_from_triangle(p1, p3, p2)
        hull.add face_from_triangle(p0, p3, p1)
    else:
        hull.add face_from_triangle(p0, p2, p1)
        hull.add face_from_triangle(p0, p3, p2)
        hull.add face_from_triangle(p1, p2, p3)
        hull.add face_from_triangle(p0, p1, p3)

    for i,point in points:
        if i<4: continue
        add_point_to_hull(point, hull)
    
    return hull

proc quickhull_points*(points: seq[Vec3]): seq[Vec3] =
    let hull = quickhull(points)
    if hull.len == 0:
        return points
    var s: HashSet[Vec3]
    for f in hull:
        for v in f.points:
            s.incl v
    return s.toSeq