extends Control
class_name DebugDraw3D


class Vector:
	var object  # The node to follow
	var property  # The property to draw
	var scale  # Scale factor
	var width  # Line width
	var color  # Draw color

	func _init(_object, _property, _scale, _width, _color) -> void:
		object = _object
		property = _property
		scale = _scale
		width = _width
		color = _color

	func draw(node: Control, camera: Camera3D) -> void:
		var start = camera.unproject_position(object.global_transform.origin)
		var endv = object.global_transform.origin + object.get(property) * scale
		var end = camera.unproject_position(endv)
		if start != end:
			node.draw_line(start, end, color, width)
			if camera.is_position_in_frustum(endv):
				node.draw_triangle(end, start.direction_to(end), width * 2, color)


var vectors = []  # Array to hold all registered values.

func draw_triangle(pos: Vector2, dir: Vector2, length: float, color: Color) -> void:
	var a = pos + dir.rotated(-PI / 2) * length
	var b = pos + dir * length
	var c = pos + dir.rotated(PI / 2) * length
	var points = PackedVector2Array([a, b, c])
	draw_polygon(points, PackedColorArray([color]))


func _process(_delta: float) -> void:
	queue_redraw()


func _draw() -> void:
	var camera = get_viewport().get_camera_3d()
	for vector in vectors:
		vector.draw(self, camera)


func add_vector(object, property, vscale, width, color) -> void:
	vectors.append(Vector.new(object, property, vscale, width, color))