extends RigidBody3D
class_name Unit

@export var movement_force: float = 100
@export var max_speed: float = 100

var target_velocity: Vector3 = Vector3()

func _ready() -> void:
	$NavigationAgent3D.connect("velocity_computed", avoidance_velocity_computed)
	
func avoidance_velocity_computed(velocity: Vector3) -> void:
	if velocity != target_velocity:
		DebugDraw3D.draw_line(global_position, global_position + velocity, Color.ORANGE)
		#print("Avoidance issued!")
		target_velocity = velocity

func _process(delta: float) -> void:
	if $NavigationAgent3D.is_target_reached() or $NavigationAgent3D.target_position.is_zero_approx():
		$NavigationAgent3D.target_position = NavigationServer3D.map_get_random_point(NavigationServer3D.get_maps()[0], 1, true)
	else:
		#DebugDraw3D.draw_sphere($NavigationAgent3D.target_position, 0.5, Color.RED)
		var next_point: Vector3 = $NavigationAgent3D.get_next_path_position()
		DebugDraw3D.draw_sphere(next_point, 0.1, Color.YELLOW)
		var direction: Vector3 = (next_point - global_position).normalized()
		#DebugDraw3D.draw_line(global_position, global_position + linear_velocity, Color.BLUE)
		var target_velocity: Vector3 = direction * max_speed
		$NavigationAgent3D.velocity = target_velocity
		DebugDraw3D.draw_line(global_position, global_position + target_velocity, Color.MAGENTA)


func _physics_process(delta: float) -> void:
	if $ShapeCast3D.is_colliding():
		var force_direction: Vector3 = (target_velocity-linear_velocity)
		var normal: Vector3 = $ShapeCast3D.get_collision_normal(0)
		DebugDraw3D.draw_line(global_position, global_position + normal, Color.DODGER_BLUE)
		var force: Vector3 = (force_direction * movement_force).slide(normal)
		DebugDraw3D.draw_line(global_position, global_position + force, Color.GREEN)
		apply_central_force(force)