//======================================================================================================
// Company Name:       MotionTechnologies.inc
// Copyright Year:     2025
// File:               MoATposeCalculator.cpp
//======================================================================================================

#include "MoATposeCalculator.h"

TMap<FName, FQuat> FMoATposeCalculator::CalculateTPoseDeltas(
	const USkeleton* TargetSkeleton,
	const USkeleton* TPoseReferenceSkeleton,
	const TArray<FMoABoneMappingRow>& BoneMappings)
{
	TMap<FName, FQuat> Result;

	if (!TargetSkeleton || !TPoseReferenceSkeleton)
	{
		UE_LOG(LogTemp, Warning, TEXT("FMoATposeCalculator: Invalid skeleton reference"));
		return Result;
	}

	const FReferenceSkeleton& TargetRefSkeleton = TargetSkeleton->GetReferenceSkeleton();
	const FReferenceSkeleton& TPoseRefSkeleton = TPoseReferenceSkeleton->GetReferenceSkeleton();

	FName LeftFootName = FindMappedBoneName(EMoABoneName::LeftAnkle, BoneMappings);
	FName RightFootName = FindMappedBoneName(EMoABoneName::RightAnkle, BoneMappings);

	int32 LeftFootIndex = TargetRefSkeleton.FindBoneIndex(LeftFootName);
	int32 RightFootIndex = TargetRefSkeleton.FindBoneIndex(RightFootName);
	FQuat OriginalLeftFootWorldRot = (LeftFootIndex != INDEX_NONE) ?
		CalculateWorldRotation(TargetRefSkeleton, LeftFootIndex) : FQuat::Identity;
	FQuat OriginalRightFootWorldRot = (RightFootIndex != INDEX_NONE) ?
		CalculateWorldRotation(TargetRefSkeleton, RightFootIndex) : FQuat::Identity;

	const TArray<EMoABoneName>& TargetBones = GetTargetBones();

	TMap<int32, FQuat> TPosedWorldRotations;

	for (EMoABoneName SourceBone : TargetBones)
	{
		FName TargetBoneName = FindMappedBoneName(SourceBone, BoneMappings);
		if (TargetBoneName.IsNone())
		{
			continue;
		}

		int32 TargetBoneIndex = TargetRefSkeleton.FindBoneIndex(TargetBoneName);
		if (TargetBoneIndex == INDEX_NONE)
		{
			continue;
		}

		FQuat TargetWorldRot = CalculateBoneTPoseRotation(
			TargetRefSkeleton,
			TPoseRefSkeleton,
			TargetBoneIndex,
			SourceBone,
			BoneMappings);

		TPosedWorldRotations.Add(TargetBoneIndex, TargetWorldRot);

		FQuat RefLocalRot = TargetRefSkeleton.GetRefBonePose()[TargetBoneIndex].GetRotation();

		int32 ParentIndex = TargetRefSkeleton.GetParentIndex(TargetBoneIndex);
		FQuat TargetLocalRot;
		if (ParentIndex != INDEX_NONE)
		{
			FQuat ParentWorldRot;
			if (const FQuat* TPosedParentRot = TPosedWorldRotations.Find(ParentIndex))
			{
				ParentWorldRot = *TPosedParentRot;
			}
			else
			{
				TArray<int32> AncestorChain;
				int32 CurrentIdx = ParentIndex;
				int32 TPosedAncestorIdx = INDEX_NONE;
				const FQuat* TPosedAncestorRot = nullptr;

				while (CurrentIdx != INDEX_NONE)
				{
					TPosedAncestorRot = TPosedWorldRotations.Find(CurrentIdx);
					if (TPosedAncestorRot)
					{
						TPosedAncestorIdx = CurrentIdx;
						break;
					}
					AncestorChain.Add(CurrentIdx);
					CurrentIdx = TargetRefSkeleton.GetParentIndex(CurrentIdx);
				}

				if (TPosedAncestorRot)
				{
					ParentWorldRot = *TPosedAncestorRot;
					for (int32 i = AncestorChain.Num() - 1; i >= 0; --i)
					{
						FQuat LocalRot = TargetRefSkeleton.GetRefBonePose()[AncestorChain[i]].GetRotation();
						ParentWorldRot = ParentWorldRot * LocalRot;
					}
				}
				else
				{
					ParentWorldRot = CalculateWorldRotation(TargetRefSkeleton, ParentIndex);
				}
			}
			TargetLocalRot = ParentWorldRot.Inverse() * TargetWorldRot;
		}
		else
		{
			TargetLocalRot = TargetWorldRot;
		}

		FQuat Delta = TargetLocalRot * RefLocalRot.Inverse();
		
		Result.Add(TargetBoneName, Delta);
	}

	auto CalculateFootDelta = [&](FName FootName, int32 FootIndex, const FQuat& OriginalWorldRot) -> FQuat
	{
		if (FootIndex == INDEX_NONE)
		{
			return FQuat::Identity;
		}

		int32 ParentIndex = TargetRefSkeleton.GetParentIndex(FootIndex);
		if (ParentIndex == INDEX_NONE)
		{
			return FQuat::Identity;
		}

		FQuat TPosedParentWorldRot;
		if (const FQuat* TPosedParentRot = TPosedWorldRotations.Find(ParentIndex))
		{
			TPosedParentWorldRot = *TPosedParentRot;
		}
		else
		{
			TPosedParentWorldRot = CalculateWorldRotation(TargetRefSkeleton, ParentIndex);
		}

		FQuat NewLocalRot = TPosedParentWorldRot.Inverse() * OriginalWorldRot;

		FQuat RefLocalRot = TargetRefSkeleton.GetRefBonePose()[FootIndex].GetRotation();

		return NewLocalRot * RefLocalRot.Inverse();
	};

	if (!LeftFootName.IsNone())
	{
		FQuat LeftFootDelta = CalculateFootDelta(LeftFootName, LeftFootIndex, OriginalLeftFootWorldRot);
		Result.Add(LeftFootName, LeftFootDelta);
	}
	if (!RightFootName.IsNone())
	{
		FQuat RightFootDelta = CalculateFootDelta(RightFootName, RightFootIndex, OriginalRightFootWorldRot);
		Result.Add(RightFootName, RightFootDelta);
	}

	return Result;
}

FQuat FMoATposeCalculator::CalculateWorldRotation(const FReferenceSkeleton& RefSkeleton, int32 BoneIndex, bool bIgnoreRootRotation)
{
	if (BoneIndex == INDEX_NONE)
	{
		return FQuat::Identity;
	}

	TArray<int32> BoneChain;
	int32 CurrentIndex = BoneIndex;
	while (CurrentIndex != INDEX_NONE)
	{
		BoneChain.Add(CurrentIndex);
		CurrentIndex = RefSkeleton.GetParentIndex(CurrentIndex);
	}

	FTransform WorldTransform = FTransform::Identity;
	for (int32 i = BoneChain.Num() - 1; i >= 0; --i)
	{
		FTransform LocalTransform = RefSkeleton.GetRefBonePose()[BoneChain[i]];

		if (bIgnoreRootRotation && i == BoneChain.Num() - 1)
		{
			LocalTransform.SetRotation(FQuat::Identity);
		}

		WorldTransform = LocalTransform * WorldTransform;
	}

	return WorldTransform.GetRotation();
}

FVector FMoATposeCalculator::CalculateWorldPosition(const FReferenceSkeleton& RefSkeleton, int32 BoneIndex, bool bIgnoreRootRotation)
{
	if (BoneIndex == INDEX_NONE)
	{
		return FVector::ZeroVector;
	}

	TArray<int32> BoneChain;
	int32 CurrentIndex = BoneIndex;
	while (CurrentIndex != INDEX_NONE)
	{
		BoneChain.Add(CurrentIndex);
		CurrentIndex = RefSkeleton.GetParentIndex(CurrentIndex);
	}

	FTransform WorldTransform = FTransform::Identity;
	for (int32 i = BoneChain.Num() - 1; i >= 0; --i)
	{
		FTransform LocalTransform = RefSkeleton.GetRefBonePose()[BoneChain[i]];

		if (bIgnoreRootRotation && i == BoneChain.Num() - 1)
		{
			LocalTransform.SetRotation(FQuat::Identity);
		}

		WorldTransform = LocalTransform * WorldTransform;
	}

	return WorldTransform.GetTranslation();
}

FVector FMoATposeCalculator::GetBoneDirection(
	const FReferenceSkeleton& RefSkeleton,
	int32 BoneIndex,
	EMoABoneName SourceBone,
	const TArray<FMoABoneMappingRow>& BoneMappings,
	bool bIgnoreRootRotation)
{
	if (BoneIndex == INDEX_NONE)
	{
		return FVector::ForwardVector;
	}

	FVector BoneWorldPos = CalculateWorldPosition(RefSkeleton, BoneIndex, bIgnoreRootRotation);

	EMoABoneName ChildBoneEnum = GetChildBone(SourceBone);
	if (ChildBoneEnum != EMoABoneName::Count)
	{
		int32 ChildIndex = INDEX_NONE;

		FName MoAChildName = FMoABoneNameUtils::EnumToFName(ChildBoneEnum);
		ChildIndex = RefSkeleton.FindBoneIndex(MoAChildName);

		if (ChildIndex == INDEX_NONE)
		{
			FName MappedChildName = FindMappedBoneName(ChildBoneEnum, BoneMappings);
			ChildIndex = RefSkeleton.FindBoneIndex(MappedChildName);
		}

		if (ChildIndex != INDEX_NONE)
		{
			FVector ChildWorldPos = CalculateWorldPosition(RefSkeleton, ChildIndex, bIgnoreRootRotation);
			FVector Dir = ChildWorldPos - BoneWorldPos;
			if (Dir.SizeSquared() > KINDA_SMALL_NUMBER)
			{
				return Dir.GetSafeNormal();
			}
		}
	}

	EMoABoneName ParentBoneEnum = GetParentBone(SourceBone);
	if (ParentBoneEnum != EMoABoneName::Count)
	{
		int32 ParentIndex = INDEX_NONE;

		FName MoAParentName = FMoABoneNameUtils::EnumToFName(ParentBoneEnum);
		ParentIndex = RefSkeleton.FindBoneIndex(MoAParentName);

		if (ParentIndex == INDEX_NONE)
		{
			FName MappedParentName = FindMappedBoneName(ParentBoneEnum, BoneMappings);
			ParentIndex = RefSkeleton.FindBoneIndex(MappedParentName);
		}

		if (ParentIndex != INDEX_NONE)
		{
			FVector ParentWorldPos = CalculateWorldPosition(RefSkeleton, ParentIndex, bIgnoreRootRotation);
			FVector Dir = BoneWorldPos - ParentWorldPos;
			if (Dir.SizeSquared() > KINDA_SMALL_NUMBER)
			{
				return Dir.GetSafeNormal();
			}
		}
	}

	FQuat WorldRot = CalculateWorldRotation(RefSkeleton, BoneIndex, bIgnoreRootRotation);
	return WorldRot.GetAxisX();
}

FVector FMoATposeCalculator::GetBoneUp(EMoABoneName SourceBone, const FVector& Direction)
{
	static const FVector RightDir = FVector(0.0f, 1.0f, 0.0f);

	FVector Hint = GetUpHint(SourceBone);

	FVector Up = Hint - FVector::DotProduct(Hint, Direction) * Direction;

	if (Up.SizeSquared() < KINDA_SMALL_NUMBER)
	{
		Up = FVector::CrossProduct(Direction, RightDir);
		if (Up.SizeSquared() < KINDA_SMALL_NUMBER)
		{
			Up = FVector::CrossProduct(Direction, FVector::UpVector);
		}
	}

	return Up.GetSafeNormal();
}

FVector FMoATposeCalculator::GetUpHint(EMoABoneName SourceBone)
{
	static const FVector ForwardDir = FVector(1.0f, 0.0f, 0.0f);

	switch (SourceBone)
	{
		case EMoABoneName::LeftShoulder:
		case EMoABoneName::LeftElbow:
		case EMoABoneName::LeftWrist:
		case EMoABoneName::RightShoulder:
		case EMoABoneName::RightElbow:
		case EMoABoneName::RightWrist:
			return FVector::DownVector;

		case EMoABoneName::LeftHip:
		case EMoABoneName::LeftKnee:
		case EMoABoneName::RightHip:
		case EMoABoneName::RightKnee:
			return ForwardDir;

		case EMoABoneName::LeftCollar:
		case EMoABoneName::RightCollar:
			return FVector::UpVector;

		default:
			return FVector::DownVector;
	}
}

EMoABoneName FMoATposeCalculator::GetParentBone(EMoABoneName SourceBone)
{
	switch (SourceBone)
	{
		// Arms
		case EMoABoneName::LeftCollar: return EMoABoneName::Chest2;
		case EMoABoneName::LeftShoulder: return EMoABoneName::LeftCollar;
		case EMoABoneName::LeftElbow: return EMoABoneName::LeftShoulder;
		case EMoABoneName::LeftWrist: return EMoABoneName::LeftElbow;
		case EMoABoneName::RightCollar: return EMoABoneName::Chest2;
		case EMoABoneName::RightShoulder: return EMoABoneName::RightCollar;
		case EMoABoneName::RightElbow: return EMoABoneName::RightShoulder;
		case EMoABoneName::RightWrist: return EMoABoneName::RightElbow;

		// Legs
		case EMoABoneName::LeftHip: return EMoABoneName::Hips;
		case EMoABoneName::LeftKnee: return EMoABoneName::LeftHip;
		case EMoABoneName::LeftAnkle: return EMoABoneName::LeftKnee;
		case EMoABoneName::LeftToe: return EMoABoneName::LeftAnkle;
		case EMoABoneName::RightHip: return EMoABoneName::Hips;
		case EMoABoneName::RightKnee: return EMoABoneName::RightHip;
		case EMoABoneName::RightAnkle: return EMoABoneName::RightKnee;
		case EMoABoneName::RightToe: return EMoABoneName::RightAnkle;

		// Left Fingers - all first segments parent to wrist
		case EMoABoneName::LeftFinger0:
		case EMoABoneName::LeftFinger1:
		case EMoABoneName::LeftFinger2:
		case EMoABoneName::LeftFinger3:
		case EMoABoneName::LeftFinger4:
			return EMoABoneName::LeftWrist;
		case EMoABoneName::LeftFinger01: return EMoABoneName::LeftFinger0;
		case EMoABoneName::LeftFinger02: return EMoABoneName::LeftFinger01;
		case EMoABoneName::LeftFinger11: return EMoABoneName::LeftFinger1;
		case EMoABoneName::LeftFinger12: return EMoABoneName::LeftFinger11;
		case EMoABoneName::LeftFinger21: return EMoABoneName::LeftFinger2;
		case EMoABoneName::LeftFinger22: return EMoABoneName::LeftFinger21;
		case EMoABoneName::LeftFinger31: return EMoABoneName::LeftFinger3;
		case EMoABoneName::LeftFinger32: return EMoABoneName::LeftFinger31;
		case EMoABoneName::LeftFinger41: return EMoABoneName::LeftFinger4;
		case EMoABoneName::LeftFinger42: return EMoABoneName::LeftFinger41;

		// Right Fingers
		case EMoABoneName::RightFinger0:
		case EMoABoneName::RightFinger1:
		case EMoABoneName::RightFinger2:
		case EMoABoneName::RightFinger3:
		case EMoABoneName::RightFinger4:
			return EMoABoneName::RightWrist;
		case EMoABoneName::RightFinger01: return EMoABoneName::RightFinger0;
		case EMoABoneName::RightFinger02: return EMoABoneName::RightFinger01;
		case EMoABoneName::RightFinger11: return EMoABoneName::RightFinger1;
		case EMoABoneName::RightFinger12: return EMoABoneName::RightFinger11;
		case EMoABoneName::RightFinger21: return EMoABoneName::RightFinger2;
		case EMoABoneName::RightFinger22: return EMoABoneName::RightFinger21;
		case EMoABoneName::RightFinger31: return EMoABoneName::RightFinger3;
		case EMoABoneName::RightFinger32: return EMoABoneName::RightFinger31;
		case EMoABoneName::RightFinger41: return EMoABoneName::RightFinger4;
		case EMoABoneName::RightFinger42: return EMoABoneName::RightFinger41;

		default: return EMoABoneName::Count;
	}
}

EMoABoneName FMoATposeCalculator::GetChildBone(EMoABoneName SourceBone)
{
	switch (SourceBone)
	{
		// Arms
		case EMoABoneName::LeftCollar: return EMoABoneName::LeftShoulder;
		case EMoABoneName::LeftShoulder: return EMoABoneName::LeftElbow;
		case EMoABoneName::LeftElbow: return EMoABoneName::LeftWrist;
		case EMoABoneName::LeftWrist: return EMoABoneName::LeftFinger2; // Middle finger as hand direction
		case EMoABoneName::RightCollar: return EMoABoneName::RightShoulder;
		case EMoABoneName::RightShoulder: return EMoABoneName::RightElbow;
		case EMoABoneName::RightElbow: return EMoABoneName::RightWrist;
		case EMoABoneName::RightWrist: return EMoABoneName::RightFinger2;

		// Legs
		case EMoABoneName::LeftHip: return EMoABoneName::LeftKnee;
		case EMoABoneName::LeftKnee: return EMoABoneName::LeftAnkle;
		case EMoABoneName::RightHip: return EMoABoneName::RightKnee;
		case EMoABoneName::RightKnee: return EMoABoneName::RightAnkle;

		// Left Fingers
		case EMoABoneName::LeftFinger0: return EMoABoneName::LeftFinger01;
		case EMoABoneName::LeftFinger01: return EMoABoneName::LeftFinger02;
		case EMoABoneName::LeftFinger1: return EMoABoneName::LeftFinger11;
		case EMoABoneName::LeftFinger11: return EMoABoneName::LeftFinger12;
		case EMoABoneName::LeftFinger2: return EMoABoneName::LeftFinger21;
		case EMoABoneName::LeftFinger21: return EMoABoneName::LeftFinger22;
		case EMoABoneName::LeftFinger3: return EMoABoneName::LeftFinger31;
		case EMoABoneName::LeftFinger31: return EMoABoneName::LeftFinger32;
		case EMoABoneName::LeftFinger4: return EMoABoneName::LeftFinger41;
		case EMoABoneName::LeftFinger41: return EMoABoneName::LeftFinger42;

		// Right Fingers
		case EMoABoneName::RightFinger0: return EMoABoneName::RightFinger01;
		case EMoABoneName::RightFinger01: return EMoABoneName::RightFinger02;
		case EMoABoneName::RightFinger1: return EMoABoneName::RightFinger11;
		case EMoABoneName::RightFinger11: return EMoABoneName::RightFinger12;
		case EMoABoneName::RightFinger2: return EMoABoneName::RightFinger21;
		case EMoABoneName::RightFinger21: return EMoABoneName::RightFinger22;
		case EMoABoneName::RightFinger3: return EMoABoneName::RightFinger31;
		case EMoABoneName::RightFinger31: return EMoABoneName::RightFinger32;
		case EMoABoneName::RightFinger4: return EMoABoneName::RightFinger41;
		case EMoABoneName::RightFinger41: return EMoABoneName::RightFinger42;

		default: return EMoABoneName::Count;
	}
}

FName FMoATposeCalculator::FindMappedBoneName(EMoABoneName SourceBone, const TArray<FMoABoneMappingRow>& BoneMappings)
{
	for (const FMoABoneMappingRow& Mapping : BoneMappings)
	{
		if (Mapping.SourceBone == SourceBone)
		{
			return Mapping.TargetBone;
		}
	}
	return NAME_None;
}

FQuat FMoATposeCalculator::CalculateBoneTPoseRotation(
	const FReferenceSkeleton& TargetRefSkeleton,
	const FReferenceSkeleton& TPoseRefSkeleton,
	int32 TargetBoneIndex,
	EMoABoneName SourceBone,
	const TArray<FMoABoneMappingRow>& BoneMappings)
{
	if (TargetBoneIndex == INDEX_NONE)
	{
		return FQuat::Identity;
	}

	if (ShouldSkipTPoseCalculation(SourceBone))
	{
		return CalculateWorldRotation(TargetRefSkeleton, TargetBoneIndex);
	}

	FName TargetBoneName = TargetRefSkeleton.GetBoneName(TargetBoneIndex);
	int32 TPoseBoneIndex = TPoseRefSkeleton.FindBoneIndex(TargetBoneName);

	if (TPoseBoneIndex == INDEX_NONE)
	{
		FName SourceBoneFName = FMoABoneNameUtils::EnumToFName(SourceBone);
		TPoseBoneIndex = TPoseRefSkeleton.FindBoneIndex(SourceBoneFName);
	}

	if (TPoseBoneIndex == INDEX_NONE)
	{
		return CalculateWorldRotation(TargetRefSkeleton, TargetBoneIndex);
	}

	constexpr bool bIgnoreRootRotation = false;

	FVector CurrentDir = GetBoneDirection(TargetRefSkeleton, TargetBoneIndex, SourceBone, BoneMappings, bIgnoreRootRotation);
	FVector RefDir = GetBoneDirection(TPoseRefSkeleton, TPoseBoneIndex, SourceBone, BoneMappings, bIgnoreRootRotation);

	FQuat DirDelta = FQuat::FindBetweenVectors(CurrentDir, RefDir);

	FQuat CurrentWorldRot = CalculateWorldRotation(TargetRefSkeleton, TargetBoneIndex);

	return DirDelta * CurrentWorldRot;
}

bool FMoATposeCalculator::ShouldSkipTPoseCalculation(EMoABoneName SourceBone)
{
	// Toes should keep original rotation
	return SourceBone == EMoABoneName::LeftToe || SourceBone == EMoABoneName::RightToe;
}

const TArray<EMoABoneName>& FMoATposeCalculator::GetTargetBones()
{
	static TArray<EMoABoneName> TargetBones = {
		EMoABoneName::LeftHip, EMoABoneName::LeftKnee, EMoABoneName::LeftToe,
		EMoABoneName::RightHip, EMoABoneName::RightKnee, EMoABoneName::RightToe,

		EMoABoneName::LeftCollar, EMoABoneName::LeftShoulder, EMoABoneName::LeftElbow, EMoABoneName::LeftWrist,
		EMoABoneName::RightCollar, EMoABoneName::RightShoulder, EMoABoneName::RightElbow, EMoABoneName::RightWrist,

		EMoABoneName::LeftFinger0, EMoABoneName::LeftFinger01, EMoABoneName::LeftFinger02,
		EMoABoneName::LeftFinger1, EMoABoneName::LeftFinger11, EMoABoneName::LeftFinger12,
		EMoABoneName::LeftFinger2, EMoABoneName::LeftFinger21, EMoABoneName::LeftFinger22,
		EMoABoneName::LeftFinger3, EMoABoneName::LeftFinger31, EMoABoneName::LeftFinger32,
		EMoABoneName::LeftFinger4, EMoABoneName::LeftFinger41, EMoABoneName::LeftFinger42,

		EMoABoneName::RightFinger0, EMoABoneName::RightFinger01, EMoABoneName::RightFinger02,
		EMoABoneName::RightFinger1, EMoABoneName::RightFinger11, EMoABoneName::RightFinger12,
		EMoABoneName::RightFinger2, EMoABoneName::RightFinger21, EMoABoneName::RightFinger22,
		EMoABoneName::RightFinger3, EMoABoneName::RightFinger31, EMoABoneName::RightFinger32,
		EMoABoneName::RightFinger4, EMoABoneName::RightFinger41, EMoABoneName::RightFinger42,
	};

	return TargetBones;
}

float FMoATposeCalculator::CalculateHipToFootBottomDistance(
	const USkeleton* Skeleton,
	const TArray<FMoABoneMappingRow>& BoneMappings)
{
	if (!Skeleton)
	{
		return 0.0f;
	}

	const FReferenceSkeleton& RefSkeleton = Skeleton->GetReferenceSkeleton();

	// Find mapped bone names - Use ANKLE as reference (matches MoA leg length calculation)
	FName HipBoneName = FindMappedBoneName(EMoABoneName::Hips, BoneMappings);
	FName LeftAnkleBoneName = FindMappedBoneName(EMoABoneName::LeftAnkle, BoneMappings);
	FName RightAnkleBoneName = FindMappedBoneName(EMoABoneName::RightAnkle, BoneMappings);

	// Get bone indices
	int32 HipIdx = RefSkeleton.FindBoneIndex(HipBoneName);
	int32 LeftAnkleIdx = RefSkeleton.FindBoneIndex(LeftAnkleBoneName);
	int32 RightAnkleIdx = RefSkeleton.FindBoneIndex(RightAnkleBoneName);

	if (HipIdx == INDEX_NONE)
	{
		UE_LOG(LogTemp, Warning, TEXT("[MoA TposeCalc] Hip bone not found for height calculation"));
		return 0.0f;
	}

	// Calculate world positions
	FVector HipPos = CalculateWorldPosition(RefSkeleton, HipIdx);

	// Get lowest ankle position (use ANKLE to match MoA leg length)
	float LowestAnkleZ = HipPos.Z; // Default if no ankle found
	bool bAnkleFound = false;

	if (LeftAnkleIdx != INDEX_NONE)
	{
		FVector LeftAnklePos = CalculateWorldPosition(RefSkeleton, LeftAnkleIdx);
		LowestAnkleZ = FMath::Min(LowestAnkleZ, LeftAnklePos.Z);
		bAnkleFound = true;
	}
	if (RightAnkleIdx != INDEX_NONE)
	{
		FVector RightAnklePos = CalculateWorldPosition(RefSkeleton, RightAnkleIdx);
		LowestAnkleZ = FMath::Min(LowestAnkleZ, RightAnklePos.Z);
		bAnkleFound = true;
	}

	if (!bAnkleFound)
	{
		UE_LOG(LogTemp, Warning, TEXT("[MoA TposeCalc] No ankle bones found for height calculation"));
		return 0.0f;
	}

	float Distance = HipPos.Z - LowestAnkleZ;

	UE_LOG(LogTemp, Log, TEXT("[MoA TposeCalc] Hip-to-Ankle distance: %.2f cm (Hip.Z=%.2f, Ankle.Z=%.2f)"),
		Distance, HipPos.Z, LowestAnkleZ);

	return Distance;
}