###
### geometry utility code
### some of this might not work yet, please dont use it
###
extends Object

class_name GDBGeoUtility

class Grid3D:
	var size : Vector3
	var data : Array
	
	func _init(size_ : Vector3, init_val = null):
		size = size_
		data = []
		data.resize(size.x * size.y * size.z)
		for i in range(data.size()):
			data[i] = init_val
	
	func _idx(x, y, z):
		return x + y * size.x + z * size.x * size.y
	
	func get_at(x, y, z):
		return data[_idx(x, y, z)]
	
	func set_at(x, y, z, val):
		data[_idx(x, y, z)] = val

################
# public stuff #
################
func _init() -> void:
	assert(0, "This class should not be instantiated.")

static func full_aabb(root : Spatial):
	var aabb = AABB()
	for vi in GDBUtility.find_nodes_by_type(root, VisualInstance):
		var local_aabb = vi.get_aabb()
		local_aabb = vi.global_transform.xform(local_aabb)
		aabb = aabb.merge(local_aabb)
	return aabb

static func gen_aabb(points : Array, point_transform = Transform()) -> AABB:
	var aabb = AABB()
	for point in points:
		aabb = aabb.expand(point_transform.xform(point))
	return aabb

static func collsion_aabb(collision_object : CollisionObject) -> AABB:
	var aabb := AABB()
	for owner_id in collision_object.get_shape_owners():
		var trans = collision_object.shape_owner_get_transform(owner_id)
		for shape_id in range(collision_object.shape_owner_get_shape_count(owner_id)):
			var shape = collision_object.shape_owner_get_shape(owner_id, shape_id)
			var new_aabb = shape_aabb(shape)
			if new_aabb.size:
				aabb = aabb.merge(trans.xform(new_aabb))
	return aabb

static func shape_aabb(shape : Shape) -> AABB:
	if shape is BoxShape:
		return AABB(-shape.extents, 2.0 * shape.extents)
	elif shape is CapsuleShape:
		return AABB(Vector3(-shape.radius, -shape.radius - 0.5 * shape.height, -shape.radius), \
				Vector3(2.0 * shape.radius, 2.0 * shape.radius + shape.height, 2.0 * shape.radius))
	elif shape is CylinderShape:
		return AABB(Vector3(-shape.radius, -0.5 * shape.height, -shape.radius), \
				Vector3(2.0 * shape.radius, shape.height, 2.0 * shape.radius))
	elif shape is PlaneShape:
		return AABB()
	elif shape is SphereShape:
		return AABB(-Vector3(shape.radius, shape.radius, shape.radius), 2.0 * Vector3(shape.radius, shape.radius, shape.radius))
	else:
		# TODO: polygon shapes
		return AABB()

static func orphan_global_transform(node : Node):
	var transform : Transform
	if node is Spatial:
		transform = node.transform
	var parent = node.get_parent()
	if parent != null:
		transform = orphan_global_transform(parent) * transform
	return transform

static func voxelize_surf(mesh : ArrayMesh, surf : int, particle_size = -1.0) -> PoolVector3Array:
	var arrays = mesh.surface_get_arrays(surf)
	var points = arrays[Mesh.ARRAY_VERTEX]
	return PoolVector3Array(points)

static func voxelize(mesh : ArrayMesh, particle_size = -1.0) -> PoolVector3Array:
	var points = PoolVector3Array()
	for surf in range(mesh.get_surface_count()):
		points.append_array(voxelize_surf(mesh, surf, particle_size))
	return points

static func transform_to(node : Node, root : Node):
	var transform = Transform()
	var node_ = node
	while node_ != root && node_ != null:
		if node_ is Spatial:
			transform = transform * node_.transform
		node_ = node_.get_parent()
	return transform

static func mesh_to_grid(mesh : ArrayMesh, grid_size : float, point_transform = Transform()):
	var aabb = AABB()
	for i in range(mesh.get_surface_count()):
		var points = mesh.surface_get_arrays(i)[Mesh.ARRAY_VERTEX]
		aabb = aabb.merge(gen_aabb(points, point_transform))
	var grid_x = ceil(aabb.size.x / grid_size)
	var grid_y = ceil(aabb.size.y / grid_size)
	var grid_z = ceil(aabb.size.z / grid_size)
	var grid = Grid3D.new(Vector3(grid_x, grid_y, grid_z), 0)
	var grid_origin = aabb.position
	
	for i in range(mesh.get_surface_count()):
		__insert_surf(mesh, i, grid_origin, grid_size, grid, point_transform)
	
	var result = []
	for x in range(grid.size.x):
		for y in range(grid.size.y):
			var inside = false
			for z in range(grid.size.z):
				var cell_value = grid.get_at(x, y, z) % 2
				if cell_value == 1:
					inside = !inside
				if inside || cell_value > 0:
					result.append(Vector3(x, y, z))
	return result

# http://www.boris-belousov.net/2016/12/01/quat-dist/
static func basis_angle(basis0 : Basis, basis1 : Basis):
	var basis_diff = basis0 * basis1.transposed()
	var tr : float = basis_diff.x.x + basis_diff.y.y + basis_diff.z.z
	return acos((tr - 1) / 2)

static func angle_normalize(angle : float) -> float:
	while angle < -PI:
		angle += 2.0 * PI
	while angle > PI:
		angle -= 2.0 * PI
	return angle

static func angle_diff(angle0 : float, angle1 : float) -> float:
	return angle_normalize(angle0 - angle1)

#################
# private stuff #
#################
static func __insert_tri(a : Vector3, b : Vector3, c : Vector3, grid_origin : Vector3, grid_size : float, grid : Grid3D, point_transform : Transform):
	var ray_dir = Vector3(0, 0, 1)
	for x in range(grid.size.x):
		for y in range(grid.size.y):
			var ray_origin = grid_origin + grid_size * Vector3(x, y, 0)
			var inters = Geometry.ray_intersects_triangle(ray_origin, ray_dir, point_transform.xform(a), point_transform.xform(b), point_transform.xform(c))
			if inters != null:
				var z = floor(0.99 * (inters.z - grid_origin.z) / grid_size)
				grid.set_at(x, y, z, grid.get_at(x, y, z) + 1)

static func __insert_surf(mesh : ArrayMesh, surf : int, grid_origin : Vector3, grid_size : float, grid : Grid3D, point_transform : Transform):
	var arrays = mesh.surface_get_arrays(surf)
	if arrays.size() >= Mesh.ARRAY_INDEX - 1 && arrays[Mesh.ARRAY_INDEX].size() > 0:
		# index mode
		var indices = arrays[Mesh.ARRAY_INDEX]
		var vertices = arrays[Mesh.ARRAY_VERTEX]
		for i in range(0, indices.size() - 2, 3):
			__insert_tri(vertices[indices[i]], vertices[indices[i + 1]], vertices[indices[i + 2]], grid_origin, grid_size, grid, point_transform)
	else:
		# normal mode
		var vertices = arrays[Mesh.ARRAY_VERTEX]
		for i in range(0, vertices.size() - 2, 3):
			__insert_tri(vertices[i], vertices[i + 1], vertices[i + 2], grid_origin, grid_size, grid, point_transform)