extends RigidBody3D
class_name Unit

@export_group("Defence")
@export var max_hp: float = 100

@export_group("Movement")
@export var movement_force: float = 100
@export var rotation_torque: float = 3
@export var max_speed: float = 100

@export_group("Navigation")
@export var minimum_progress_rate: float = 1.0
@export var stuck_time: float = 1.0

@onready var hp: float = max_hp
var target_velocity: Vector3 = Vector3()
var avoidance_velocity: Vector3 = Vector3()
var avoidance_timeout: float = 0.0
var last_distance_to_target: float = 0.0
var stuck_timer: float = 0.0
var move_target: Vector3 = Vector3(16, 1, 13)
var move_radius: float = 5.0

@onready var shapecast_3d: ShapeCast3D = $ShapeCast3D
@onready var nav_agent_3d: NavigationAgent3D = $NavigationAgent3D
@onready var label_3d: Label3D = $Label3D

func _ready() -> void:
	nav_agent_3d.connect("velocity_computed", avoidance_velocity_computed)
	pass
	
func avoidance_velocity_computed(velocity: Vector3) -> void:
	if velocity != target_velocity:
		avoidance_velocity = velocity
		avoidance_timeout = 0.5

func _process(delta: float) -> void:
	label_3d.text = "HP: %d" % hp
	
	if nav_agent_3d.is_target_reached() \
		or nav_agent_3d.target_position.is_zero_approx() \
		or !nav_agent_3d.is_target_reachable():
		#target_velocity = Vector3()
		nav_agent_3d.target_position = move_target + Vector3(randfn(0, move_radius), 0, randfn(0, move_radius))
		#nav_agent_3d.target_position = NavigationServer3D.map_get_random_point(NavigationServer3D.get_maps()[0], 1, true)
		last_distance_to_target = nav_agent_3d.distance_to_target()
	else:
		if shapecast_3d.is_colliding():
			var distance_to_target: float = nav_agent_3d.distance_to_target()
			var progress_rate: float = (last_distance_to_target - distance_to_target) / delta
			last_distance_to_target = distance_to_target
			if progress_rate < minimum_progress_rate:
				stuck_timer += delta
				if stuck_timer >= stuck_time:
					unstuck()
			else:
				label_3d.modulate = Color.WHITE
				stuck_timer = 0
		if global_position.y <= -10:
			unstuck()
		#DebugDraw3D.draw_sphere(nav_agent_3d.target_position, 0.5, Color.RED)
		var next_point: Vector3 = nav_agent_3d.get_next_path_position()
		#DebugDraw3D.draw_sphere(next_point, 0.1, Color.YELLOW)
		var direction: Vector3 = (next_point - global_position).normalized()
		#basis = Basis.looking_at(direction)
		#DebugDraw3D.draw_line(global_position, global_position + linear_velocity, Color.BLUE)
		target_velocity = direction * max_speed
		nav_agent_3d.velocity = target_velocity
		#DebugDraw3D.draw_line(global_position, global_positiaon + target_velocity, Color.MAGENTA)
		#DebugDraw3D.draw_text(global_position + Vector3(0,1,0), "%f" % nav_agent_3d.distance_to_target())

func unstuck() -> void:
	# teleport to next path point
	linear_velocity = Vector3()
	global_position = nav_agent_3d.get_next_path_position()
	stuck_timer = 0
	
func hurt(damage: float) -> void:
	hp -= damage
	#print("%s hit for %f damage, HP=%f" % [name, damage, hp])
	if hp <= 0:
		die()
		
func die() -> void:
	queue_free()

func _physics_process(delta: float) -> void:
	if shapecast_3d.is_colliding():
		var actual_target_velocity: Vector3 = target_velocity
		if avoidance_timeout > 0:
			avoidance_timeout -= delta
			actual_target_velocity = actual_target_velocity.slerp(avoidance_velocity, 0.25)
			#DebugDraw3D.draw_line(global_position, global_position + actual_target_velocity, Color.ORANGE)
		var force_direction: Vector3 = (actual_target_velocity-linear_velocity)
		var normal: Vector3 = shapecast_3d.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)
		apply_torque(Vector3(0,force_direction.normalized().signed_angle_to(-global_basis.z, Vector3.DOWN) * rotation_torque,0))
		#DebugDraw3D.draw_line(global_position, global_position - global_basis.z, Color.BLUE)
		#DebugDraw3D.draw_line(global_position, global_position + force_direction, Color.RED)