/* Copyright (C) Itseez3D, Inc. - All Rights Reserved
* You may not use this file except in compliance with an authorized license
* Unauthorized copying of this file, via any medium is strictly prohibited
* Proprietary and confidential
* UNLESS REQUIRED BY APPLICABLE LAW OR AGREED BY ITSEEZ3D, INC. IN WRITING, SOFTWARE DISTRIBUTED UNDER THE LICENSE IS DISTRIBUTED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OR
* CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
* See the License for the specific language governing permissions and limitations under the License.
* Written by Itseez3D, Inc. <support@avatarsdk.com>, June 2026
*/

using System.Collections.Generic;
using UnityEngine;

namespace AvatarSDKMove
{
	public class HumanoidRetargeter : MonoBehaviour
	{
		public Avatar sourceAvatar;
		public GameObject sourceModel;

		public GameObject targetModel;

		public HandCollisionAvoidance handCollisionAvoidance;

		public TargetPositionConstraint targetPositionConstraint;

		public List<HumanBodyBones> bonesToRetarget = new List<HumanBodyBones>();

		private Animator targetAnimator;

		private Vector3 srcInitModelPos;
		private Quaternion srcInitModelRot;
		private Vector3 srcInitHipsPos;

		private Vector3 targetInitModelPos;
		private Quaternion targetInitModelRot;
		private float hipsRatio = 1.0f;

		private HumanDescriptionBoneCache sourceBoneCache;
		private HumanDescriptionBoneCache targetBoneCache;

		private bool isSourceInitialized = false;
		private GameObject initializedTargetModel;

		void Awake()
		{
			if (targetModel != null)
				Initialize();
		}

		public void Initialize()
		{
			targetAnimator = targetModel == null ? null : targetModel.GetComponent<Animator>();
			if (targetAnimator == null || !targetAnimator.avatar.isHuman)
			{
				Debug.LogError("HumanoidRetargeter requires a humanoid Animator on the target GameObject.");
				enabled = false;
				return;
			}

			if (!isSourceInitialized)
			{
				sourceBoneCache = new HumanDescriptionBoneCache(sourceModel.transform, sourceAvatar.humanDescription);

				srcInitModelPos = sourceModel.transform.position;
				srcInitModelRot = sourceModel.transform.rotation;
				srcInitHipsPos = sourceBoneCache.Bones[HumanBodyBones.Hips].Transform.position;

				isSourceInitialized = true;
			}

			targetBoneCache = new HumanDescriptionBoneCache(targetModel.transform, targetAnimator.avatar.humanDescription);

			if (initializedTargetModel != targetModel)
			{
				targetInitModelPos = targetModel.transform.position;
				targetInitModelRot = targetModel.transform.rotation;

				initializedTargetModel = targetModel;
			}

			if (targetPositionConstraint != null)
				targetPositionConstraint.target = targetModel.transform;

			float srcHipsHeight = srcInitHipsPos.y - srcInitModelPos.y;
			hipsRatio = Mathf.Approximately(srcHipsHeight, 0.0f)
				? 1.0f
				: targetBoneCache.Bones[HumanBodyBones.Hips].Transform.localPosition.y / srcHipsHeight;

			enabled = true;
		}

		void LateUpdate()
		{
			if (targetAnimator == null)
				return;

			foreach (var humanBone in bonesToRetarget)
			{
				if (sourceBoneCache.TryGet(humanBone, out var srcBoneInfo) &&
					targetBoneCache.TryGet(humanBone, out var targetBoneInfo))
				{
					Quaternion worldDelta = srcBoneInfo.ParentSkeletonWorldRotation * srcBoneInfo.Transform.localRotation * Quaternion.Inverse(srcBoneInfo.SkeletonWorldRotation);
					Quaternion targetWorldRot = worldDelta * targetBoneInfo.SkeletonWorldRotation;
					Quaternion targetLocalRot = Quaternion.Inverse(targetBoneInfo.ParentSkeletonWorldRotation) * targetWorldRot;

					targetBoneInfo.Transform.localRotation = targetLocalRot;
				}
			}

			Transform sourceHips = sourceBoneCache.Bones[HumanBodyBones.Hips].Transform;

			Vector3 srcDeltaHipsPos = sourceHips.position - srcInitHipsPos;

			Vector3 localDelta = Quaternion.Inverse(srcInitModelRot) * srcDeltaHipsPos;
			Vector3 targetDeltaPos = targetInitModelRot * (localDelta * hipsRatio);

			targetModel.transform.position = targetInitModelPos + targetDeltaPos;
			targetModel.transform.rotation = targetInitModelRot;

			if (handCollisionAvoidance != null)
				handCollisionAvoidance.Solve();

			if (targetPositionConstraint != null)
				targetPositionConstraint.UpdateTargetPosition();
		}
	}
}
