extends TestCase

## T-WIN-006：33 点 → VRM 骨骼重定向
##
## 纯逻辑部分总是运行；真实模型部分在找不到 .vrm 时跳过。
## 其中最关键的断言是**功能性**的：给定一组关键点，
## 重定向后骨骼在世界空间的实际指向必须与关键点方向一致。

const KeypointFrame = preload("res://src/net/keypoint_frame.gd")
const VrmLoader = preload("res://src/model/vrm_loader.gd")
const SkeletonRetarget = preload("res://src/anim/skeleton_retarget.gd")

const ASPECT := 4.0 / 3.0
const CANDIDATE_PATHS := [
	"res://_scratch/sample.vrm",
	"user://models/sample.vrm",
]

var _scene: Node = null


func _find_model_path() -> String:
	for path in CANDIDATE_PATHS:
		if FileAccess.file_exists(path):
			return path
	for arg in OS.get_cmdline_user_args():
		if arg.begins_with("--vrm="):
			var path := arg.substr(6)
			if FileAccess.file_exists(path):
				return path
	return ""


func _tree() -> SceneTree:
	return Engine.get_main_loop() as SceneTree


## 加载真实模型并挂入场景树（返回 VrmModel，失败返回 null）
func _load_model() -> VrmLoader.VrmModel:
	var path := _find_model_path()
	if path == "":
		return null
	var model: VrmLoader.VrmModel = VrmLoader.load_from_file(path)
	if not model.available or model.skeleton == null:
		model.dispose()
		return null
	_scene = model.root
	_tree().root.add_child(_scene)
	return model


func _unload_model(model: VrmLoader.VrmModel) -> void:
	if _scene != null and is_instance_valid(_scene):
		_tree().root.remove_child(_scene)
		_scene.free()
	_scene = null
	if model != null:
		model.dispose()


## 构造一帧：左臂水平向侧面伸出，其余保持站姿
func _frame_with_left_arm_out() -> KeypointFrame.PoseFrame:
	var frame := KeypointFrame.PoseFrame.new()
	frame.points.resize(33)
	frame.confidence.resize(33)
	frame.points.fill(Vector3(0.5, 0.5, 0.0))
	frame.confidence.fill(1.0)

	frame.points[KeypointFrame.NOSE] = Vector3(0.50, 0.18, 0.0)
	frame.points[KeypointFrame.LEFT_SHOULDER] = Vector3(0.60, 0.30, 0.0)
	frame.points[KeypointFrame.RIGHT_SHOULDER] = Vector3(0.40, 0.30, 0.0)
	frame.points[KeypointFrame.LEFT_ELBOW] = Vector3(0.72, 0.30, 0.0)
	frame.points[KeypointFrame.LEFT_WRIST] = Vector3(0.84, 0.30, 0.0)
	frame.points[KeypointFrame.LEFT_INDEX] = Vector3(0.88, 0.30, 0.0)
	frame.points[KeypointFrame.RIGHT_ELBOW] = Vector3(0.30, 0.42, 0.0)
	frame.points[KeypointFrame.RIGHT_WRIST] = Vector3(0.22, 0.55, 0.0)
	frame.points[KeypointFrame.RIGHT_INDEX] = Vector3(0.20, 0.58, 0.0)
	frame.points[KeypointFrame.LEFT_HIP] = Vector3(0.55, 0.62, 0.0)
	frame.points[KeypointFrame.RIGHT_HIP] = Vector3(0.45, 0.62, 0.0)
	frame.points[KeypointFrame.LEFT_KNEE] = Vector3(0.56, 0.78, 0.0)
	frame.points[KeypointFrame.RIGHT_KNEE] = Vector3(0.44, 0.78, 0.0)
	frame.points[KeypointFrame.LEFT_ANKLE] = Vector3(0.57, 0.94, 0.0)
	frame.points[KeypointFrame.RIGHT_ANKLE] = Vector3(0.43, 0.94, 0.0)
	frame.points[KeypointFrame.LEFT_FOOT_INDEX] = Vector3(0.57, 0.98, 0.0)
	frame.points[KeypointFrame.RIGHT_FOOT_INDEX] = Vector3(0.43, 0.98, 0.0)
	return frame


# ---------------- 纯逻辑 ----------------

func test_to_godot_axis_conversion() -> void:
	var center := SkeletonRetarget.to_godot(Vector3(0.5, 0.5, 0.0), ASPECT)
	almost(center.x, 0.5 * ASPECT, 1e-6, "x 按宽高比缩放")
	almost(center.y, -0.5, 1e-6, "y 取反（屏幕向下 → 世界向下）")

	# 屏幕上方（y 小）应得到更大的世界 Y
	var higher := SkeletonRetarget.to_godot(Vector3(0.5, 0.2, 0.0), ASPECT)
	var lower := SkeletonRetarget.to_godot(Vector3(0.5, 0.8, 0.0), ASPECT)
	ok(higher.y > lower.y, "屏幕上方映射到世界更高处")

	# MediaPipe z 负值朝相机；Godot +Z 朝相机
	var toward_camera := SkeletonRetarget.to_godot(Vector3(0.5, 0.5, -0.2), ASPECT)
	ok(toward_camera.z > 0.0, "MediaPipe 负 z（朝相机）映射到 Godot +Z", "实际 %.4f" % toward_camera.z)


func test_extract_positions_adds_virtual_joints() -> void:
	var positions := SkeletonRetarget.extract_positions(_frame_with_left_arm_out(), ASPECT)
	eq(positions.size(), 35, "输出 33 点 + neck + pelvis 共 35 项")

	var expected_neck: Vector3 = (positions[KeypointFrame.LEFT_SHOULDER] + positions[KeypointFrame.RIGHT_SHOULDER]) * 0.5
	var expected_pelvis: Vector3 = (positions[KeypointFrame.LEFT_HIP] + positions[KeypointFrame.RIGHT_HIP]) * 0.5
	almost((positions[SkeletonRetarget.VIRTUAL_NECK] - expected_neck).length(), 0.0, 1e-6, "neck 为双肩中点")
	almost((positions[SkeletonRetarget.VIRTUAL_PELVIS] - expected_pelvis).length(), 0.0, 1e-6, "pelvis 为双髋中点")


func test_mirrored_frame_is_normalized() -> void:
	var frame := _frame_with_left_arm_out()
	var plain := SkeletonRetarget.extract_positions(frame, ASPECT)

	frame.flags = KeypointFrame.FLAG_MIRRORED
	var mirrored := SkeletonRetarget.extract_positions(frame, ASPECT)

	ok(
		absf(mirrored[KeypointFrame.LEFT_WRIST].x - plain[KeypointFrame.LEFT_WRIST].x) > 1e-3,
		"镜像帧的 x 被翻转"
	)


func test_average_confidence() -> void:
	var frame := _frame_with_left_arm_out()
	almost(SkeletonRetarget.average_confidence(frame), 1.0, 1e-6, "全 1 置信度均值 1.0")
	frame.confidence.fill(0.5)
	almost(SkeletonRetarget.average_confidence(frame), 0.5, 1e-5, "全 0.5 置信度均值 0.5")


func test_unbound_retarget_is_safe() -> void:
	var retarget := SkeletonRetarget.new()
	is_false(retarget.is_bound(), "未绑定时 is_bound 为 false")
	eq(retarget.apply(_frame_with_left_arm_out()), 0, "未绑定时 apply 返回 0 且不报错")


func test_bone_chains_cover_all_drive_order_entries() -> void:
	for canonical in SkeletonRetarget.DRIVE_ORDER:
		is_true(SkeletonRetarget.BONE_CHAINS.has(canonical), "%s 有对应骨链定义" % canonical)
	for canonical in SkeletonRetarget.BONE_CHAINS:
		var chain: Array = SkeletonRetarget.BONE_CHAINS[canonical]
		eq(chain.size(), 2, "%s 骨链为二元组" % canonical)
		for point in chain:
			ok(int(point) >= 0 and int(point) <= SkeletonRetarget.VIRTUAL_PELVIS, "%s 的端点索引合法" % canonical)


# ---------------- 真实模型：功能性验证 ----------------

func test_retarget_actually_drives_real_skeleton() -> void:
	var model := await _load_model()
	if model == null:
		skip("真实模型重定向", "未找到 .vrm（放置于 _scratch/sample.vrm 或传 --vrm=<路径>）")
		return

	var skeleton := model.skeleton
	skeleton.reset_bone_poses()
	await _tree().process_frame

	var retarget := SkeletonRetarget.new(skeleton, model.bones)
	retarget.capture_rest()
	is_true(retarget.is_bound(), "重定向器绑定成功")
	ok(retarget.driven_bone_count() >= 16, "至少驱动 16 根骨骼", "实际 %d" % retarget.driven_bone_count())

	var frame := _frame_with_left_arm_out()
	var applied := retarget.apply(frame, ASPECT)
	ok(applied >= 16, "apply 返回被驱动的骨骼数", "实际 %d" % applied)
	await _tree().process_frame

	# 期望方向：关键点给出的左上臂方向
	var positions := SkeletonRetarget.extract_positions(frame, ASPECT)
	var expected_upper: Vector3 = (
		positions[KeypointFrame.LEFT_ELBOW] - positions[KeypointFrame.LEFT_SHOULDER]
	).normalized()
	var expected_lower: Vector3 = (
		positions[KeypointFrame.LEFT_WRIST] - positions[KeypointFrame.LEFT_ELBOW]
	).normalized()

	var upper_idx := model.bone_index("leftUpperArm")
	var lower_idx := model.bone_index("leftLowerArm")
	var hand_idx := model.bone_index("leftHand")

	var upper_origin: Vector3 = skeleton.get_bone_global_pose(upper_idx).origin
	var lower_origin: Vector3 = skeleton.get_bone_global_pose(lower_idx).origin
	var hand_origin: Vector3 = skeleton.get_bone_global_pose(hand_idx).origin

	var actual_upper: Vector3 = (lower_origin - upper_origin).normalized()
	var actual_lower: Vector3 = (hand_origin - lower_origin).normalized()

	var dot_upper := actual_upper.dot(expected_upper)
	var dot_lower := actual_lower.dot(expected_lower)

	print("      → 左上臂方向点积 %.4f，左前臂方向点积 %.4f" % [dot_upper, dot_lower])
	ok(dot_upper > 0.95, "左上臂实际指向与关键点方向一致", "点积 %.4f" % dot_upper)
	ok(dot_lower > 0.95, "左前臂实际指向与关键点方向一致", "点积 %.4f" % dot_lower)

	# 左臂应指向角色左侧：MediaPipe 的 left_wrist.x > left_shoulder.x
	ok(
		positions[KeypointFrame.LEFT_WRIST].x > positions[KeypointFrame.LEFT_SHOULDER].x,
		"测试用例本身左侧方向正确"
	)

	# 判别性验证：换一组关键点，骨骼必须随之改变。
	# 注意不能拿「静止姿态」做对照 —— VRM 样例本身就是 T-pose，
	# 其静止左上臂方向与「手臂平举」恰好一致，那样的断言会假失败。
	var rest_basis: Basis = skeleton.get_bone_global_pose(upper_idx).basis

	var raised := _frame_with_left_arm_out()
	raised.points[KeypointFrame.LEFT_ELBOW] = Vector3(0.62, 0.16, 0.0)
	raised.points[KeypointFrame.LEFT_WRIST] = Vector3(0.64, 0.02, 0.0)
	retarget.apply(raised, ASPECT)
	await _tree().process_frame

	var raised_positions := SkeletonRetarget.extract_positions(raised, ASPECT)
	var expected_raised: Vector3 = (
		raised_positions[KeypointFrame.LEFT_ELBOW] - raised_positions[KeypointFrame.LEFT_SHOULDER]
	).normalized()
	var raised_upper: Vector3 = (
		skeleton.get_bone_global_pose(lower_idx).origin - skeleton.get_bone_global_pose(upper_idx).origin
	).normalized()

	ok(raised_upper.dot(expected_raised) > 0.95, "换成举手臂后，左上臂仍与关键点方向一致", "点积 %.4f" % raised_upper.dot(expected_raised))
	ok(
		actual_upper.dot(raised_upper) < 0.99,
		"两组不同关键点产生不同的骨骼朝向（重定向确实在起效）",
		"平举与上举方向点积 %.4f" % actual_upper.dot(raised_upper)
	)

	# reset 必须回到绑定时捕获的静止朝向
	retarget.reset()
	await _tree().process_frame
	var after_reset: Basis = skeleton.get_bone_global_pose(upper_idx).basis
	var basis_delta := 0.0
	for i in 3:
		basis_delta = maxf(basis_delta, (after_reset[i] - rest_basis[i]).length())
	ok(basis_delta < 1e-4, "reset 后骨骼朝向回到静止值", "最大基向量偏差 %.6f" % basis_delta)

	await _unload_model_and_free(model)


func _unload_model_and_free(model: VrmLoader.VrmModel) -> void:
	_unload_model(model)


func test_torso_rotation_propagates_to_head() -> void:
	var model := await _load_model()
	if model == null:
		skip("躯干传播", "未找到 .vrm")
		return

	var skeleton := model.skeleton
	skeleton.reset_bone_poses()
	await _tree().process_frame

	var retarget := SkeletonRetarget.new(skeleton, model.bones)
	retarget.capture_rest()

	var head_idx := model.bone_index("head")
	var hips_idx := model.bone_index("hips")
	# 记录驱动前头部的相对位置
	var hips_origin: Vector3 = skeleton.get_bone_global_pose(hips_idx).origin
	var head_before: Vector3 = skeleton.get_bone_global_pose(head_idx).origin - hips_origin

	# 构造一个向左侧弯腰的躯干：肩线左低右高
	var frame := _frame_with_left_arm_out()
	frame.points[KeypointFrame.LEFT_SHOULDER] = Vector3(0.62, 0.40, 0.0)
	frame.points[KeypointFrame.RIGHT_SHOULDER] = Vector3(0.38, 0.26, 0.0)
	retarget.apply(frame, ASPECT)
	await _tree().process_frame

	var head_after: Vector3 = skeleton.get_bone_global_pose(head_idx).origin - hips_origin
	var moved: float = head_before.distance_to(head_after)
	ok(moved > 0.005, "躯干旋转使头部位置发生位移（FK 传播生效）", "位移 %.5f" % moved)

	retarget.reset()
	await _tree().process_frame
	await _unload_model_and_free(model)