Simplex :: struct {
    points: [4] Vector3;
    size: s32;
}

Edge :: struct {
    i1: s64;
    i2: s64;
}

Collision_Point :: struct {
    normal: Vector3;
    penetration_depth: float;
    has_collision: bool;
}

get_face_normals :: (polytope: [] Vector3, faces: [] s64) -> [..] Vector4, s64 {
    normals : [..] Vector4;
    normals.allocator = temp;

    min_triangle : s64 =  0;
    min_distance := FLOAT32_MAX;

    i := 0;
    while i < faces.count {
        defer i += 3;

        a := polytope[faces[i+0]];
        b := polytope[faces[i+1]];
        c := polytope[faces[i+2]];

        normal := normalize(cross(b - a, c - a));
        distance := dot(normal, a);

        if distance < 0 {
            normal *= -1.0;
            distance *= -1.0;
        }

        data : Vector4;
        data.x = normal.x;
        data.y = normal.y;
        data.z = normal.z;
        data.w = distance;
        array_add(*normals, data);

        if distance < min_distance {
            min_triangle = i / 3;
            min_distance = distance;
        }
    }

    return normals, min_triangle;
}

add_if_unique_edge :: (edges: *[..] Edge, faces: [] s64, a: s64, b: s64) {
    i := 0;
    found := false;
    while i < edges.count {
        defer i += 1;

        edge := edges.*[i];
        if edge.i1 == faces[b] && edge.i2 == faces[a] {
           array_unordered_remove_by_index(edges, i);
           found = true;
           break;
        }
    }

    if !found {
        array_add(edges, .{faces[a], faces[b]});
    }
}

nearly_equal :: (a: float, b: float) -> bool {
    return abs(a - b) < 0.00001;
}

polytope : [..] Vector3;

epa :: (simplex: Simplex, collider_a: Collider, collider_b: Collider) -> Collision_Point {
    polytope.count = 0;

    array_reserve(*polytope, simplex.size);
    for simplex.points {
        array_add(*polytope, it);
    }

    faces : [..] s64;
    faces.allocator = temp;
    array_resize(*faces, 12);

    faces[0] = 0;
    faces[1] = 1;
    faces[2] = 2;
    faces[3] = 0;
    faces[4] = 3;
    faces[5] = 1;
    faces[6] = 0;
    faces[7] = 2;
    faces[8] = 3;
    faces[9] = 1;
    faces[10] = 3;
    faces[11] = 2;

    normals, min_face := get_face_normals(polytope, faces);

    min_normal : Vector3;
    min_distance := FLOAT32_MAX;

    while nearly_equal(min_distance, FLOAT32_MAX) {
        min_normal = to_v3(normals[min_face]);
        min_distance = normals[min_face].w;

        s := support(collider_a, collider_b, min_normal);
        s_distance := dot(min_normal, s);

        if abs(s_distance - min_distance) > 0.01 {
            min_distance = FLOAT32_MAX;

            unique_edges : [..] Edge;
            unique_edges.allocator = temp;

            i := 0;
            while i < normals.count {
                defer i += 1;
                if dot(to_v3(normals[i]), s) > dot(to_v3(normals[i]), polytope[faces[i*3]]) { //same_direction(to_v3(normals[i]), s) {
                    f := i * 3;
                    add_if_unique_edge(*unique_edges, faces, f, f + 1);
                    add_if_unique_edge(*unique_edges, faces, f + 1, f + 2);
                    add_if_unique_edge(*unique_edges, faces, f + 2, f);

                    faces[f+2] = faces[faces.count-1];
                    faces[f+1] = faces[faces.count-2];
                    faces[f+0] = faces[faces.count-3];

                    faces.count -= 3;

                    normals[i] = normals[normals.count-1];
                    normals.count -= 1;
                    i -= 1;
                }
            }

            new_faces : [..] s64;
            new_faces.allocator = temp;
            for e: unique_edges {
                array_add(*new_faces, e.i1);
                array_add(*new_faces, e.i2);
                array_add(*new_faces, polytope.count);
            }

            array_add(*polytope, s);

            new_normals, new_min_face := get_face_normals(polytope, new_faces);

            old_min_distance := FLOAT32_MAX;

            for n: normals {
                if n.w < old_min_distance {
                    old_min_distance = n.w;
                    min_face = it_index;
                }
            }

            if new_normals[new_min_face].w < old_min_distance {
                min_face = new_min_face + normals.count;
            }


            for f: new_faces {
                array_add(*faces, f);
            }

            for n: new_normals {
                array_add(*normals, n);
            }
        }
    }

    point : Collision_Point;
    point.normal = min_normal;
    point.penetration_depth = min_distance + 0.001;
    point.has_collision = true;

    return point;
}

find_furthest_point :: (collider: Collider, direction: Vector3) -> Vector3 {
    max_point : Vector3;
    max_distance := -FLOAT32_MAX;

    if collider.type == {
        case .MESH; {
            for collider.mesh.vertices {
                distance := dot(it, direction);
                if distance > max_distance {
                    max_distance = distance;
                    max_point = it;
                }
            }
        }
    }

    return max_point;
}

push_front :: (simplex: *Simplex, p: Vector3) {
	simplex.points[3] = simplex.points[2];
	simplex.points[2] = simplex.points[1];
	simplex.points[1] = simplex.points[0];
	simplex.points[0] = p;

	simplex.size = min(simplex.size + 1, 4);
}

tetrahedron :: (simplex: *Simplex, direction: *Vector3) -> bool {
	a := simplex.points[0];
	b := simplex.points[1];
	c := simplex.points[2];
	d := simplex.points[3];

	ab := b - a;
	ac := c - a;
	ad := d - a;
	ao :=   - a;
 
	abc := cross(ab, ac);
	acd := cross(ac, ad);
	adb := cross(ad, ab);
 
	if same_direction(abc, ao) {
		simplex.points[0] = a;
		simplex.points[1] = b;
		simplex.points[2] = c;
		simplex.size = 3;
		return triangle(simplex, direction);
	}
		
	if same_direction(acd, ao) {
		simplex.points[0] = a;
		simplex.points[1] = c;
		simplex.points[2] = d;
		simplex.size = 3;
		return triangle(simplex,  direction);
	}
 
	if same_direction(adb, ao) {
		simplex.points[0] = a;
		simplex.points[1] = d;
		simplex.points[2] = b;
		simplex.size = 3;
		return triangle(simplex, direction);
	}

	return true;
}

triangle :: (simplex: *Simplex, direction: *Vector3) -> bool {
	a := simplex.points[0];
	b := simplex.points[1];
	c := simplex.points[2];

	ab := b - a;
	ac := c - a;
	ao :=   - a;

	abc := cross(ab, ac);

	if same_direction(cross(abc, ac), ao) {
		if same_direction(ac, ao) {
			simplex.points[0] = a;
			simplex.points[1] = c;
			simplex.size = 2;
			direction.* = cross(cross(ac, ao), ac);
		} else {
			simplex.points[0] = a;
			simplex.points[1] = b;
			simplex.size = 2;
			return line(simplex, direction);
		}
	} else {
		if same_direction(cross(ab, abc), ao) {
			simplex.points[0] = a;
			simplex.points[1] = b;
			simplex.size = 2;
			return line(simplex, direction);
		} else {
			if same_direction(abc, ao) {
				direction.* = abc;
			} else {
				simplex.points[0] = a;
				simplex.points[1] = c;
				simplex.points[2] = b;
				simplex.size = 3;
				direction.* = -abc;
			}
		}
	}

	return false;
}

same_direction :: (direction: Vector3, ao: Vector3) -> bool {
	return dot(direction, ao) > 0;
}

line :: (simplex: *Simplex, direction: *Vector3) -> bool {
	a := simplex.points[0];
	b := simplex.points[1];

	ab := b - a;
	ao :=   - a;
 
	if same_direction(ab, ao) {
		direction.* = cross(cross(ab, ao), ab);
	} else {
		simplex.points[0] = a;
		simplex.size = 1;
		direction.* = ao;
	}

	return false;
}

next_simplex :: (simplex: *Simplex, direction: *Vector3) -> bool {
	if simplex.size == {
		case 2; return line(simplex, direction);
		case 3; return triangle(simplex, direction);
		case 4; return tetrahedron(simplex, direction);
	}

	return false;
}

support  :: (a: Collider, b: Collider, direction: Vector3) -> Vector3 {
    return find_furthest_point(a, direction) - find_furthest_point(b, -direction);
}

gjk :: (a: Collider, b: Collider) -> Collision_Point {
    sup := support(a, b, .{1,0,0});

    points : Simplex;
	push_front(*points, sup);

    direction := -sup;
	
	while true {
		sup = support(a, b, direction);

		if dot(sup, direction) <= 0 {
			return .{};
		}

		push_front(*points, sup);

		if next_simplex(*points, *direction) {
            collision_point := epa(points, a, b);
			return collision_point;
		}
	}

	return .{};
}