extends Node
## Procedural leg stepping, body wobble, and tail simulation.
## Created at runtime by player.gd; call setup() before the first _process tick.

# ── Leg State Machine ─────────────────────────────────────────────────────────
enum LegState { GALLOP, LAUNCH, RAGDOLL, TUCK, LAND }

# Leg IK config — rest offsets are in bull_v10 local space
const LEG_CFG: Dictionary = {
	"FL": { "root": &"front_leg_1_L", "tip": &"front_leg_2_L_end",
			"rest": Vector3(-0.30, -0.25, -0.50) },
	"FR": { "root": &"front_leg_1_R", "tip": &"front_leg_2_R_end",
			"rest": Vector3( 0.30, -0.25, -0.50) },
	"BL": { "root": &"rear_leg_1_L",  "tip": &"rear_leg_3_L_end",
			"rest": Vector3(-0.30, -0.25,  0.60) },
	"BR": { "root": &"rear_leg_1_R",  "tip": &"rear_leg_3_R_end",
			"rest": Vector3( 0.30, -0.25,  0.60) },
}

# Gallop gait phase offset per leg (0.0–1.0 cycle).
const LEG_PHASES: Dictionary = { "BL": 0.0, "FL": 0.2, "BR": 0.5, "FR": 0.7 }

# Per-leg ragdoll phase offsets for variety
const RAGDOLL_PHASE: Dictionary = { "FL": 0.0, "FR": 1.3, "BL": 2.6, "BR": 3.9 }

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

# Per-leg state
var _ik:         Dictionary = {}  # id -> SkeletonIK3D
var _foot:       Dictionary = {}  # id -> current foot (world)
var _step_from:  Dictionary = {}  # id -> Vector3
var _step_to:    Dictionary = {}  # id -> Vector3
var _step_t:     Dictionary = {}  # id -> float  (1.0 = planted)

# 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

# Leg state machine
var _leg_state:    LegState = LegState.GALLOP
var _state_time:   float    = 0.0   # time spent in current state
var _was_on_floor: bool     = true
var _launch_feet:  Dictionary = {}  # frozen foot positions at launch

# Gait
var _gait_phase: 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 — we don't need it
	var phys_sim := _find_physical_bones(_skeleton)
	if phys_sim:
		phys_sim.active = false

	_setup_tail()

	for id: String in LEG_CFG:
		var cfg: Dictionary = LEG_CFG[id]

		var ik := SkeletonIK3D.new()
		ik.root_bone     = cfg["root"]
		ik.tip_bone      = cfg["tip"]
		ik.interpolation = 1.0
		ik.min_distance  = 0.01
		_skeleton.add_child(ik)
		ik.start()
		_ik[id] = ik

		var world_rest: Vector3 = _bull.to_global(cfg["rest"])
		_foot[id]      = world_rest
		_step_from[id] = world_rest
		_step_to[id]   = world_rest
		_step_t[id]    = 1.0


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

	# Initialise world positions first
	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)

	# Detect degenerate rest pose (FBX bones all at same position)
	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]
		# Backward-downward direction in world space from the skeleton
		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]

	# Segment lengths in world space — used by the Verlet length constraint
	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))

	# Rest directions in SKELETON-LOCAL space — must match point_dir_skel in _apply_tail
	# so that Quaternion(rest_dir, point_dir_skel) operates in a consistent frame.
	# _tail_world holds world positions; to_local converts them to skeleton space.
	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_leg_state(delta)
	_step_feet(delta)
	_apply_ik()
	_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"))

	_bull.rotation.x = _tilt_x + _aerial_pitch
	_bull.rotation.z = _tilt_z


# ── Leg State Machine ─────────────────────────────────────────────────────────

func _update_leg_state(delta: float) -> void:
	var on_floor: bool = _player.is_on_floor()
	_state_time += delta

	match _leg_state:
		LegState.GALLOP:
			if not on_floor and _was_on_floor:
				# Just left the ground — enter LAUNCH
				_leg_state = LegState.LAUNCH
				_state_time = 0.0
				# Freeze current foot positions
				for id: String in _foot:
					_launch_feet[id] = _foot[id]

		LegState.LAUNCH:
			if on_floor:
				_leg_state = LegState.GALLOP
				_state_time = 0.0
			elif _state_time > DP.f("launch_hold_time"):
				_leg_state = LegState.RAGDOLL
				_state_time = 0.0

		LegState.RAGDOLL:
			if on_floor:
				_leg_state = LegState.GALLOP
				_state_time = 0.0
			elif _player.velocity.y < DP.f("tuck_apex_threshold"):
				# Approaching or past apex — start tucking
				_leg_state = LegState.TUCK
				_state_time = 0.0

		LegState.TUCK:
			if on_floor:
				_leg_state = LegState.GALLOP
				_state_time = 0.0
			else:
				# Check for ground proximity — transition to LAND
				var space  := _player.get_world_3d().direct_space_state
				var start  := _player.global_position
				var end_pt := start + Vector3.DOWN * DP.f("jump_land_threshold")
				var params := PhysicsRayQueryParameters3D.create(start, end_pt)
				params.exclude = [_player.get_rid()]
				var hit := space.intersect_ray(params)
				if hit:
					_leg_state = LegState.LAND
					_state_time = 0.0

		LegState.LAND:
			if on_floor:
				_leg_state = LegState.GALLOP
				_state_time = 0.0

	_was_on_floor = on_floor


# ── Feet Stepping ─────────────────────────────────────────────────────────────

func _step_feet(delta: float) -> void:
	match _leg_state:
		LegState.GALLOP:
			_step_gallop(delta)
		LegState.LAUNCH:
			_step_launch(delta)
		LegState.RAGDOLL:
			_step_ragdoll(delta)
		LegState.TUCK:
			_step_tuck(delta)
		LegState.LAND:
			_step_land(delta)


func _step_gallop(delta: float) -> void:
	var threshold: float = DP.f("leg_step_threshold")
	var duration:  float = DP.f("leg_step_duration")
	var height:    float = DP.f("leg_step_height")
	var overshoot: float = DP.f("leg_step_overshoot")
	var flat_spd:  float = Vector2(_player.velocity.x, _player.velocity.z).length()

	var prev_phase: float = _gait_phase
	_gait_phase = fmod(_gait_phase + flat_spd * DP.f("gait_freq") * delta, 1.0)

	# Advance in-progress steps
	for id: String in _step_t:
		if _step_t[id] >= 1.0:
			continue
		_step_t[id] = minf(_step_t[id] + delta / duration, 1.0)
		var t: float   = _step_t[id]
		var e: float   = t * t * (3.0 - 2.0 * t)
		var arc: float = height * sin(t * PI)
		_foot[id] = _step_from[id].lerp(_step_to[id], e) + Vector3(0.0, arc, 0.0)

	# Trigger steps when gait phase crosses each leg's offset
	for id: String in LEG_PHASES:
		if _step_t[id] < 1.0:
			continue
		if not _phase_crossed(prev_phase, _gait_phase, LEG_PHASES[id]):
			continue
		var ideal: Vector3 = _ideal_foot(id)
		if _foot[id].distance_to(ideal) < threshold:
			continue
		_step_from[id] = _foot[id]
		_step_to[id]   = ideal + _player.velocity * duration * overshoot
		_step_t[id]    = 0.0


func _step_launch(_delta: float) -> void:
	# Hold feet at their frozen launch positions
	for id: String in _foot:
		if _launch_feet.has(id):
			_foot[id] = _launch_feet[id]


func _step_ragdoll(_delta: float) -> void:
	# Procedural floppy dangle — NOT real physics ragdoll
	var time: float = _state_time
	var amp: float = DP.f("ragdoll_wobble_amp")
	# Amplitude grows over time (legs spread more the longer airborne)
	var grow: float = clampf(time * 0.8, 0.0, 1.5)

	for id: String in _foot:
		var rest: Vector3 = LEG_CFG[id]["rest"]
		var phase: float = RAGDOLL_PHASE[id]
		var dangle_offset := Vector3(
			sin(time * 3.0 + phase) * amp * grow,          # side sway
			-0.4 - sin(time * 2.0 + phase) * 0.1 * grow,  # gravity pull down
			cos(time * 2.5 + phase) * amp * grow            # forward/back wobble
		)
		_foot[id] = _bull.to_global(rest + dangle_offset)


func _step_tuck(delta: float) -> void:
	# Smoothly pull legs into tucked position under the body
	var speed: float = DP.f("jump_tuck_speed")
	for id: String in _foot:
		var tucked: Vector3 = _get_tucked_foot(id)
		_foot[id] = _foot[id].lerp(tucked, clampf(delta * speed, 0.0, 1.0))


func _step_land(delta: float) -> void:
	# Extend legs from tucked pose toward ideal ground positions, resume gallop cycle
	var speed: float = DP.f("jump_tuck_speed") * 1.5  # slightly faster extend
	for id: String in _foot:
		var ideal: Vector3 = _ideal_foot(id)
		_foot[id] = _foot[id].lerp(ideal, clampf(delta * speed, 0.0, 1.0))


# ── Helpers ───────────────────────────────────────────────────────────────────

func _phase_crossed(prev: float, curr: float, target: float) -> bool:
	if curr >= prev:
		return target >= prev and target < curr
	else:
		return target >= prev or target < curr


func _ideal_foot(id: String) -> Vector3:
	var world_rest: Vector3 = _bull.to_global(LEG_CFG[id]["rest"])

	# Raycast for ground
	var space  := _player.get_world_3d().direct_space_state
	var params := PhysicsRayQueryParameters3D.create(
		world_rest + Vector3(0.0, 0.6, 0.0),
		world_rest + Vector3(0.0, -1.4, 0.0),
	)
	params.exclude = [_player.get_rid()]
	var hit := space.intersect_ray(params)
	return hit["position"] if hit else world_rest


func _get_tucked_foot(id: String) -> Vector3:
	var rest: Vector3 = LEG_CFG[id]["rest"]
	var tucked_local := rest + Vector3(0.0, DP.f("jump_tuck_up"), DP.f("jump_tuck_fwd"))
	return _bull.to_global(tucked_local)


func _apply_ik() -> void:
	var skel_inv: Transform3D = _skeleton.global_transform.affine_inverse()
	for id: String in _ik:
		var ik: SkeletonIK3D = _ik[id]
		ik.target = Transform3D(Basis.IDENTITY, skel_inv * _foot[id])


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

	# Re-anchor root joint
	var anchor: Vector3 = _skeleton.to_global(_skeleton.get_bone_global_rest(_tail_idx[0]).origin)
	_tail_world[0] = anchor
	_tail_prev[0]  = anchor

	# Idle wag force
	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

	# Verlet integrate
	for i in range(1, _tail_world.size()):
		var vel: Vector3 = (_tail_world[i] - _tail_prev[i]) * damping
		_tail_prev[i]  = _tail_world[i]
		# Add wag force (sinusoidal side-to-side)
		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

	# Length constraint
	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)

	# Body collision pass — prevent tail from clipping through torso
	# Use bull's local space; body is approximated as a capsule along Z axis
	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])
		# Capsule centered at roughly (0, 0, 0.1) in bull local space, half-length 0.4 along Z
		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