/* 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 UnityEngine;
using System.Collections.Generic;
using UnityEngine.Animations;

namespace AvatarSDKMove
{
	public class HandCollisionAvoidance : MonoBehaviour
	{
		[Header("References")]
		public Animator animator;
		public Transform leftHandTarget;
		public Transform rightHandTarget;
		public Transform leftElbowHint;
		public Transform rightElbowHint;

		[Header("Body Colliders")]
		public List<Collider> bodyColliders;
		public void Solve()
		{
			ProcessHand(HumanBodyBones.LeftHand, HumanBodyBones.LeftLowerArm, HumanBodyBones.LeftUpperArm, leftHandTarget, leftElbowHint);
			ProcessHand(HumanBodyBones.RightHand, HumanBodyBones.RightLowerArm, HumanBodyBones.RightUpperArm, rightHandTarget, rightElbowHint);
		}

		void ProcessHand(HumanBodyBones handBone, HumanBodyBones elbowBone, HumanBodyBones upperArmBone, Transform target, Transform hint)
		{
			if (target == null || animator == null)
				return;

			// 1. Get animated bone transforms
			Transform handTr = animator.GetBoneTransform(handBone);
			Transform elbowTr = animator.GetBoneTransform(elbowBone);
			Transform upperArmTr = animator.GetBoneTransform(upperArmBone);
			if (handTr == null || elbowTr == null)
				return;

			target.position = handTr.position;
			target.rotation = handTr.rotation;

			hint.position = elbowTr.position;
			hint.rotation = elbowTr.rotation;

			foreach (Collider col in bodyColliders)
			{
				if (col == null || !col.enabled) continue;
				ColliderUtility.ClampToSurface(col, target);
				ColliderUtility.ClampToSurface(col, hint);
			}

			SolveTwoBoneIK(upperArmTr, elbowTr, handTr, target, hint, 1, 1, 1);
		}

		const float k_SqrEpsilon = 1e-8f;
		private void SolveTwoBoneIK(
			Transform root,
			Transform mid,
			Transform tip,
			Transform target,
			Transform hint,
			float posWeight,
			float rotWeight,
			float hintWeight)
		{
			Vector3 aPosition = root.position;
			Vector3 bPosition = mid.position;
			Vector3 cPosition = tip.position;

			Vector3 targetPos = target.position;
			Quaternion targetRot = target.rotation;

			Vector3 tPosition = Vector3.Lerp(cPosition, targetPos, posWeight);
			Quaternion tRotation = Quaternion.Lerp(tip.rotation, targetRot, rotWeight);

			bool hasHint = hint != null && hintWeight > 0f;

			Vector3 ab = bPosition - aPosition;
			Vector3 bc = cPosition - bPosition;
			Vector3 ac = cPosition - aPosition;
			Vector3 at = tPosition - aPosition;

			float abLen = ab.magnitude;
			float bcLen = bc.magnitude;
			float acLen = ac.magnitude;
			float atLen = at.magnitude;

			float oldAbcAngle = TriangleAngle(acLen, abLen, bcLen);
			float newAbcAngle = TriangleAngle(atLen, abLen, bcLen);

			Vector3 axis = Vector3.Cross(ab, bc);
			if (axis.sqrMagnitude < k_SqrEpsilon)
			{
				axis = hasHint ? Vector3.Cross(hint.position - aPosition, bc) : Vector3.zero;

				if (axis.sqrMagnitude < k_SqrEpsilon)
					axis = Vector3.Cross(at, bc);

				if (axis.sqrMagnitude < k_SqrEpsilon)
					axis = Vector3.up;
			}
			axis = Vector3.Normalize(axis);

			float a = 0.5f * (oldAbcAngle - newAbcAngle);
			float sin = Mathf.Sin(a);
			float cos = Mathf.Cos(a);
			Quaternion deltaR = new Quaternion(axis.x * sin, axis.y * sin, axis.z * sin, cos);
			mid.rotation = deltaR * mid.rotation;

			cPosition = tip.position;
			ac = cPosition - aPosition;
			root.rotation = QuaternionExt.FromToRotation(ac, at) * root.rotation;

			if (hasHint)
			{
				float acSqrMag = ac.sqrMagnitude;
				if (acSqrMag > 0f)
				{
					bPosition = mid.position;
					cPosition = tip.position;
					ab = bPosition - aPosition;
					ac = cPosition - aPosition;

					Vector3 acNorm = ac / Mathf.Sqrt(acSqrMag);
					Vector3 ah = hint.position - aPosition;
					Vector3 abProj = ab - acNorm * Vector3.Dot(ab, acNorm);
					Vector3 ahProj = ah - acNorm * Vector3.Dot(ah, acNorm);

					float maxReach = abLen + bcLen;
					if (abProj.sqrMagnitude > (maxReach * maxReach * 0.001f) && ahProj.sqrMagnitude > 0f)
					{
						Quaternion hintR = QuaternionExt.FromToRotation(abProj, ahProj);
						hintR.x *= hintWeight;
						hintR.y *= hintWeight;
						hintR.z *= hintWeight;
						hintR = QuaternionExt.NormalizeSafe(hintR);
						root.rotation = hintR * root.rotation;
					}
				}
			}

			tip.rotation = tRotation;
		}

		float TriangleAngle(float aLen, float aLen1, float aLen2)
		{
			float c = Mathf.Clamp((aLen1 * aLen1 + aLen2 * aLen2 - aLen * aLen) / (aLen1 * aLen2) / 2.0f, -1.0f, 1.0f);
			return Mathf.Acos(c);
		}
	}
}