extends Node
## Body wobble and tail simulation. Legs stay in rest pose (no IK, no ragdoll).
## Created at runtime by player.gd; call setup() before the first _process tick.

var _player:   CharacterBody3D
var _bull:     Node3D
var _skeleton: Skeleton3D

# Body wobble
var _prev_vel:     Vector3 = Vector3.ZERO
var _tilt_x:       float   = 0.0
var _tilt_z:       float   = 0.0
var _aerial_pitch: float   = 0.0

var charge_pitch_target: float = 0.0
var _charge_pitch_curr:  float = 0.0

# Tail — Verlet chain simulation
const TAIL_BONES: Array = [
	&"tail_1", &"tail_2", &"tail_3", &"tail_4",
	&"tail_5", &"tail_6",
	&"tail_tip_1", &"tail_tip_2", &"tail_tip_2_end",
]
var _tail_idx:     Array[int]     = []
var _tail_world:   Array[Vector3] = []
var _tail_prev:    Array[Vector3] = []
var _tail_lengths: Array[float]   = []
var _tail_rest_dir: Array[Vector3] = []


func setup(player: CharacterBody3D, bull: Node3D) -> void:
	_player   = player
	_bull     = bull
	_skeleton = _find_skeleton(bull)
	if not _skeleton:
		push_error("BullLegs: Skeleton3D not found under bull_v10")
		return

	# Disable any physical bone simulator
	var phys_sim := _find_physical_bones(_skeleton)
	if phys_sim:
		phys_sim.active = false

	_setup_tail()


func _setup_tail() -> void:
	for bone_name: StringName in TAIL_BONES:
		var idx: int = _skeleton.find_bone(bone_name)
		if idx == -1:
			push_warning("BullLegs: tail bone '%s' not found" % bone_name)
			continue
		_tail_idx.append(idx)

	if _tail_idx.size() < 2:
		push_error("BullLegs: not enough tail bones found")
		return

	for idx: int in _tail_idx:
		var world: Vector3 = _skeleton.to_global(_skeleton.get_bone_global_rest(idx).origin)
		_tail_world.append(world)
		_tail_prev.append(world)

	var max_sep: float = 0.0
	for i in range(_tail_world.size() - 1):
		max_sep = maxf(max_sep, _tail_world[i].distance_to(_tail_world[i + 1]))
	if max_sep < 0.01:
		push_warning("BullLegs: tail bones have zero separation in rest pose, distributing along spine")
		var root: Vector3 = _tail_world[0]
		var back: Vector3 = (_skeleton.global_transform.basis.z + Vector3.DOWN * 0.3).normalized()
		for i in range(_tail_world.size()):
			_tail_world[i] = root + back * (i * 0.09)
			_tail_prev[i]  = _tail_world[i]

	const MIN_SEG: float = 0.06
	for i in range(_tail_world.size() - 1):
		var seg_len: float = _tail_world[i].distance_to(_tail_world[i + 1])
		_tail_lengths.append(maxf(seg_len, MIN_SEG))

	for i in range(_tail_world.size() - 1):
		var p1_skel: Vector3 = _skeleton.to_local(_tail_world[i])
		var p2_skel: Vector3 = _skeleton.to_local(_tail_world[i + 1])
		var seg_skel: Vector3 = p2_skel - p1_skel
		_tail_rest_dir.append(seg_skel.normalized() if seg_skel.length() > 0.001 else Vector3(0, 0, 1))
	_tail_rest_dir.append(_tail_rest_dir[-1])


func _process(delta: float) -> void:
	if not _skeleton:
		return
	_wobble_body(delta)
	_update_tail(delta)
	_apply_tail()


# ── Body Wobble ───────────────────────────────────────────────────────────────

func _wobble_body(delta: float) -> void:
	var vel:   Vector3 = _player.velocity
	var accel: Vector3 = (vel - _prev_vel) / maxf(delta, 0.001)
	_prev_vel = vel

	var la: Vector3 = _bull.global_transform.basis.inverse() * accel

	var tx := clampf(-la.z * DP.f("body_tilt_fwd"),  -0.35, 0.35)
	var tz := clampf(-la.x * DP.f("body_tilt_side"), -0.25, 0.25)

	_tilt_x = lerpf(_tilt_x, tx, delta * 10.0)
	_tilt_z = lerpf(_tilt_z, tz, delta * 10.0)

	var flat_spd: float = Vector2(vel.x, vel.z).length()
	var target_pitch: float = 0.0
	if not _player.is_on_floor() and flat_spd > 0.5:
		target_pitch = clampf(-atan2(vel.y, flat_spd) * DP.f("aerial_pitch_scale"), -1.4, 1.4)
	_aerial_pitch = lerpf(_aerial_pitch, target_pitch, delta * DP.f("aerial_pitch_speed"))

	_charge_pitch_curr = lerpf(_charge_pitch_curr, charge_pitch_target, delta * DP.f("charge_head_speed"))
	_bull.rotation.x = _tilt_x + _aerial_pitch + _charge_pitch_curr
	_bull.rotation.z = _tilt_z


# ── Tail ──────────────────────────────────────────────────────────────────────

func _update_tail(delta: float) -> void:
	if _tail_world.is_empty():
		return

	var gravity := Vector3(0.0, -DP.f("tail_gravity"), 0.0)
	var damping := 1.0 - clampf(DP.f("tail_damping"), 0.0, 0.99)

	var anchor: Vector3 = _skeleton.to_global(_skeleton.get_bone_global_rest(_tail_idx[0]).origin)
	_tail_world[0] = anchor
	_tail_prev[0]  = anchor

	var flat_speed: float = Vector2(_player.velocity.x, _player.velocity.z).length()
	var wag_strength: float = lerpf(DP.f("tail_wag_idle"), 0.0, clampf(flat_speed / 3.0, 0.0, 1.0))
	var time: float = Time.get_ticks_msec() / 1000.0

	for i in range(1, _tail_world.size()):
		var vel: Vector3 = (_tail_world[i] - _tail_prev[i]) * damping
		_tail_prev[i]  = _tail_world[i]
		var wag := _bull.global_transform.basis.x \
				* sin(time * DP.f("tail_wag_freq") + i * 0.5) * wag_strength
		_tail_world[i] = _tail_world[i] + vel + gravity * (delta * delta) + wag * delta

	for i in range(1, _tail_world.size()):
		var dir: Vector3 = _tail_world[i] - _tail_world[i - 1]
		var len: float   = dir.length()
		var target_len: float = _tail_lengths[i - 1]
		if len > 0.0001:
			_tail_world[i] = _tail_world[i - 1] + dir * (target_len / len)
		else:
			_tail_world[i] = _tail_world[i - 1] + Vector3(0.0, -target_len, 0.0)

	var body_radius: float = DP.f("tail_body_radius")
	for i in range(1, _tail_world.size()):
		var p := _bull.to_local(_tail_world[i])
		var closest := Vector3(0.0, 0.0, clampf(p.z, -0.3, 0.5))
		var diff := p - closest
		var dist := diff.length()
		if dist < body_radius and dist > 0.001:
			p = closest + diff.normalized() * body_radius
			_tail_world[i] = _bull.to_global(p)


func _apply_tail() -> void:
	if _tail_world.is_empty():
		return

	for i in range(_tail_world.size()):
		var idx: int = _tail_idx[i]
		var pos_skel: Vector3 = _skeleton.to_local(_tail_world[i])

		var point_dir_skel: Vector3
		if i < _tail_world.size() - 1:
			point_dir_skel = (_skeleton.to_local(_tail_world[i + 1]) - pos_skel).normalized()
		else:
			var prev_skel: Vector3 = _skeleton.to_local(_tail_world[i - 1])
			point_dir_skel = (pos_skel - prev_skel).normalized()

		var rest_dir: Vector3 = _tail_rest_dir[i]
		var rest_t: Transform3D = _skeleton.get_bone_global_rest(idx)
		var new_basis: Basis
		if rest_dir.length_squared() > 0.0001 and point_dir_skel.length_squared() > 0.0001:
			new_basis = Basis(Quaternion(rest_dir, point_dir_skel)) * rest_t.basis
		else:
			new_basis = rest_t.basis

		_skeleton.set_bone_global_pose_override(idx, Transform3D(new_basis, pos_skel), 1.0, false)


# ── Utility ───────────────────────────────────────────────────────────────────

func _find_skeleton(node: Node) -> Skeleton3D:
	if node is Skeleton3D:
		return node as Skeleton3D
	for child in node.get_children():
		var r := _find_skeleton(child)
		if r:
			return r
	return null


func _find_physical_bones(node: Node) -> PhysicalBoneSimulator3D:
	for child in node.get_children():
		if child is PhysicalBoneSimulator3D:
			return child as PhysicalBoneSimulator3D
	return null