package bvh

import "base:runtime"
import "core:container/queue"
import "core:fmt"
import "core:log"
import lg "core:math/linalg"
import "game:debug"
import rl "vendor:raylib"
import "vendor:raylib/rlgl"

_ :: fmt
_ :: log

// Assuming rl.BeginMode3D was called before this
debug_draw_bvh_bounds_mesh :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index: int) {
	old_width := rlgl.GetLineWidth()
	rlgl.SetLineWidth(4)
	defer rlgl.SetLineWidth(old_width)

	temp := runtime.default_temp_allocator_temp_begin()
	defer runtime.default_temp_allocator_temp_end(temp)

	Traversal :: struct {
		node_idx:    i32,
		should_draw: bool,
	}
	nodes_to_process: queue.Queue(Traversal)
	queue.init(&nodes_to_process, queue.DEFAULT_CAPACITY, context.temp_allocator)

	queue.push_back(&nodes_to_process, Traversal{0, node_index == 0})

	for queue.len(nodes_to_process) > 0 {
		traversal := queue.pop_front(&nodes_to_process)

		node_idx := traversal.node_idx
		should_draw := traversal.should_draw || node_index == int(node_idx)

		node := &bvh.nodes[node_idx]

		if should_draw {
			rl.DrawBoundingBox(
				rl.BoundingBox{node.aabb.min + pos, node.aabb.max + pos},
				debug.int_to_color(node_idx + 1),
			)
		}

		if !is_leaf_node(node^) {
			left_child := node.child_or_prim_start
			queue.push_back_elems(
				&nodes_to_process,
				Traversal{left_child, should_draw},
				Traversal{left_child + 1, should_draw},
			)
		} else if should_draw {
			for i in node.child_or_prim_start ..< node.child_or_prim_start + node.prim_len {
				tri := bvh.primitives[i]
				i1, i2, i3 :=
					mesh.indices[tri * 3], mesh.indices[tri * 3 + 1], mesh.indices[tri * 3 + 2]
				v1, v2, v3 := mesh.vertices[i1], mesh.vertices[i2], mesh.vertices[i3]

				centroid := (v1 + v2 + v3) * 0.33333333

				aabb: AABB
				aabb.min = Vec3 {
					min(v1.x, v2.x, v3.x),
					min(v1.y, v2.y, v3.y),
					min(v1.z, v2.z, v3.z),
				}
				aabb.max = Vec3 {
					max(v1.x, v2.x, v3.x),
					max(v1.y, v2.y, v3.y),
					max(v1.z, v2.z, v3.z),
				}

				size := lg.length(aabb.max - aabb.min)
				rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(tri) + 1))
				rl.DrawBoundingBox(
					rl.BoundingBox{aabb.min, aabb.max},
					debug.int_to_color(i32(tri) + 2),
				)

				if size < 1 {
					rl.DrawCubeWiresV(centroid, 0.05, debug.int_to_color(i32(tri) + 3))
				}
			}
		}
	}
}

debug_draw_bvh_bounds :: proc(bvh: ^BVH, primitive_bounds: []AABB, node_index: int) {
	old_width := rlgl.GetLineWidth()
	rlgl.SetLineWidth(4)
	defer rlgl.SetLineWidth(old_width)

	temp := runtime.default_temp_allocator_temp_begin()
	defer runtime.default_temp_allocator_temp_end(temp)

	Traversal :: struct {
		node_idx:    i32,
		should_draw: bool,
	}
	nodes_to_process: queue.Queue(Traversal)
	queue.init(&nodes_to_process, queue.DEFAULT_CAPACITY, context.temp_allocator)

	queue.push_back(&nodes_to_process, Traversal{0, node_index == 0})

	for queue.len(nodes_to_process) > 0 {
		traversal := queue.pop_front(&nodes_to_process)

		node_idx := traversal.node_idx
		should_draw := traversal.should_draw || node_index == int(node_idx)

		node := &bvh.nodes[node_idx]

		if should_draw {
			rl.DrawBoundingBox(
				rl.BoundingBox{node.aabb.min, node.aabb.max},
				debug.int_to_color(node_idx + 1),
			)
		}

		if !is_leaf_node(node^) {
			left_child := node.child_or_prim_start
			queue.push_back_elems(
				&nodes_to_process,
				Traversal{left_child, should_draw},
				Traversal{left_child + 1, should_draw},
			)
		} else if should_draw {
			for i in node.child_or_prim_start ..< node.child_or_prim_start + node.prim_len {
				prim := bvh.primitives[i]

				aabb := primitive_bounds[prim]

				rl.DrawBoundingBox(
					rl.BoundingBox{aabb.min, aabb.max},
					debug.int_to_color(i32(prim) + 2),
				)
			}
		}
	}
}

bvh_mesh_from_rl_mesh :: proc(mesh: rl.Mesh) -> Mesh {
	return Mesh {
		vertices = (cast([^]Vec3)mesh.vertices)[:mesh.vertexCount],
		indices = mesh.indices[:mesh.triangleCount * 3],
	}
}