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

# When true, another system (the roll's ball-tumble) owns the bull's full
# orientation — skip writing body wobble so we don't fight it.
var external_rotation: bool = false

# 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] = []
var _tail_parent_idx: int            = -1

var tail_ragdoll:   bool = false
var _prev_on_floor: bool = true

var _dbg_on:    bool                    = false
var _dbg_nodes: Array[MeshInstance3D]   = []


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")
		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])

	_tail_parent_idx = _skeleton.get_bone_parent(_tail_idx[0])


func _process(delta: float) -> void:
	if not _skeleton:
		return
	_wobble_body(delta)
	_update_tail(delta)
	_apply_tail()
	if DP.b("show_tail") != _dbg_on:
		_dbg_toggle()
	if _dbg_on:
		_dbg_update()


# ── 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"))
	if external_rotation:
		return
	_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
	if _tail_parent_idx >= 0:
		var parent_t := _skeleton.get_bone_global_pose(_tail_parent_idx)
		anchor = _skeleton.to_global((parent_t * _skeleton.get_bone_rest(_tail_idx[0])).origin)
	else:
		anchor = _skeleton.to_global(_skeleton.get_bone_global_rest(_tail_idx[0]).origin)
	_tail_world[0] = anchor
	_tail_prev[0]  = anchor

	var on_floor := _player.is_on_floor()
	if not on_floor and _prev_on_floor:
		# Bull just left the ground — push all tail particles upward with it
		for i: int in range(1, _tail_world.size()):
			_tail_prev[i] = _tail_world[i] - _player.velocity * delta
	_prev_on_floor = on_floor

	var gravity_mult: float
	if tail_ragdoll:
		gravity_mult = 2.5
	elif not on_floor:
		gravity_mult = 0.15
	else:
		gravity_mult = 1.0
	gravity *= gravity_mult

	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 wag_freq: float = DP.f("tail_wag_freq")
	var time: float = Time.get_ticks_msec() / 1000.0
	var basis_x: Vector3 = _bull.global_transform.basis.x

	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 := Vector3.ZERO if tail_ragdoll \
			else basis_x * sin(time * 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

	var n := _tail_world.size()
	var local_pos: Array[Vector3] = []
	local_pos.resize(n)
	for i in n:
		local_pos[i] = _skeleton.to_local(_tail_world[i])

	for i in n:
		var idx: int = _tail_idx[i]
		var pos_skel: Vector3 = local_pos[i]

		var point_dir_skel: Vector3
		if i < n - 1:
			point_dir_skel = (local_pos[i + 1] - pos_skel).normalized()
		else:
			point_dir_skel = (pos_skel - local_pos[i - 1]).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)


# ── Debug Visualization ───────────────────────────────────────────────────────

func _dbg_toggle() -> void:
	if _dbg_on:
		for n: MeshInstance3D in _dbg_nodes:
			if is_instance_valid(n):
				n.queue_free()
		_dbg_nodes.clear()
		_dbg_on = false
	else:
		_dbg_build()
		_dbg_on = true


func _dbg_build() -> void:
	var root: Node = _player.get_parent()
	var n: int = _tail_world.size()
	for i: int in n:
		var mat := StandardMaterial3D.new()
		mat.shading_mode  = BaseMaterial3D.SHADING_MODE_UNSHADED
		mat.render_priority = 1
		mat.albedo_color  = Color.RED if i == 0 \
			else Color(1.0 - float(i) / n, 0.4 + 0.6 * float(i) / n, 0.0)
		var sm := SphereMesh.new()
		sm.radius          = 0.10 if i == 0 else 0.06
		sm.height          = sm.radius * 2.0
		sm.radial_segments = 6
		sm.rings           = 3
		sm.material        = mat
		var mi := MeshInstance3D.new()
		mi.mesh = sm
		root.add_child(mi)
		_dbg_nodes.append(mi)


func _dbg_update() -> void:
	for i: int in _dbg_nodes.size():
		_dbg_nodes[i].global_position = _tail_world[i]


# ── 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