/* 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
{
	[System.Serializable]
	public class HandCollisionModel
	{
		public BoxCollider palm;
		public BoxCollider thumb;
		public BoxCollider index;
		public BoxCollider middle;
		public BoxCollider ring;
		public BoxCollider pinky;

		public IEnumerable<BoxCollider> Colliders
		{
			get
			{
				if (palm) yield return palm;
				if (thumb) yield return thumb;
				if (index) yield return index;
				if (middle) yield return middle;
				if (ring) yield return ring;
				if (pinky) yield return pinky;
			}
		}
	}

	public class HandCollisionAvoidance2 : MonoBehaviour
	{
		[Header("Left Arm")]
		public Transform leftUpperArm;
		public Transform leftLowerArm;
		public Transform leftHand;

		[Header("Right Arm")]
		public Transform rightUpperArm;
		public Transform rightLowerArm;
		public Transform rightHand;

		[Header("Targets")]
		public Transform leftHandTarget;
		public Transform rightHandTarget;
		public Transform leftElbowHint;
		public Transform rightElbowHint;

		[Header("Body Colliders")]
		public List<Collider> bodyColliders;

		[Header("Hand Colliders")]
		public HandCollisionModel leftHandColliders;
		public HandCollisionModel rightHandColliders;

		[Header("Settings")]
		public bool useHandColliders = true;
		public bool findClosesPoint = false;
		public float maxCorrectionDistance = 0.08f;

		[Header("Elbow Preservation")]
		public bool preserveElbowPose = true;
		public float elbowPreservation = 0.8f;

		[Header("Smoothing")]
		public bool smoothTarget = true;
		public float smoothTime = 0.1f; // seconds

		[Header("IK Weight Blending")]
		public bool blendIKWeight = true;
		public float maxPenetrationForFullWeight = 0.1f; // meters

		private Vector3 leftTargetVelocity;
		private Vector3 rightTargetVelocity;

		void LateUpdate()
		{
			if (useHandColliders)
			{
				ProcessHandWithColliders(leftUpperArm, leftLowerArm, leftHand, leftHandColliders, leftHandTarget, leftElbowHint, ref leftTargetVelocity);
				ProcessHandWithColliders(rightUpperArm, rightLowerArm, rightHand, rightHandColliders, rightHandTarget, rightElbowHint, ref rightTargetVelocity);
			}
			else
			{
				ProcessHand(leftUpperArm, leftLowerArm, leftHand, leftHandTarget, leftElbowHint);
				ProcessHand(rightUpperArm, rightLowerArm, rightHand, rightHandTarget, rightElbowHint);
			}
		}

		void ProcessHand(Transform upperArm, Transform lowerArm, Transform hand, Transform target, Transform hint)
		{
			target.position = hand.position;
			target.rotation = hand.rotation;

			hint.position = lowerArm.position;
			hint.rotation = lowerArm.rotation;

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

			SolveTwoBoneIK(upperArm, lowerArm, hand, target, hint, 1, 1, 1);
		}

		void ProcessHandWithColliders(Transform upperArm, Transform lowerArm, Transform hand, HandCollisionModel handColliders, Transform target, Transform hint, ref Vector3 velocity)
		{
			// Set initial target to hand (as before)
			target.position = hand.position;
			target.rotation = hand.rotation;

			hint.position = lowerArm.position;
			hint.rotation = lowerArm.rotation;

			Vector3 originalHandPosition = hand.position;

			// Compute the raw correction delta (does not modify target)
			Vector3 correctionDelta = ResolveHandPenetration(handColliders);
			Vector3 desiredPosition = originalHandPosition + correctionDelta;

			// Preserve elbow pose using the raw desired position
			if (preserveElbowPose)
			{
				Vector3 handOffset = desiredPosition - originalHandPosition;
				PreserveElbowPose(upperArm, lowerArm, hand, desiredPosition, hint, handOffset);
			}

			// Apply smoothing to the target position
			if (smoothTarget)
			{
				target.position = Vector3.SmoothDamp(target.position, desiredPosition, ref velocity, smoothTime);
			}
			else
			{
				target.position = desiredPosition;
			}

			// --- IK weight blending ---
			float ikWeight = 1f;
			if (blendIKWeight)
			{
				float penetrationMagnitude = correctionDelta.magnitude;
				ikWeight = Mathf.Clamp01(penetrationMagnitude / maxPenetrationForFullWeight);
			}

			// Solve IK with the (possibly smoothed) target and blended weight
			SolveTwoBoneIK(upperArm, lowerArm, hand, target, hint, ikWeight, ikWeight, ikWeight);
		}

		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);
		}

		Vector3 ResolveHandPenetration(HandCollisionModel hand)
		{
			Vector3 targetPosDelta = Vector3.zero;
			const int MaxIterations = 4;

			for (int iteration = 0; iteration < MaxIterations; iteration++)
			{
				Vector3 bestDirection = Vector3.zero;
				float bestDistance = 0f;

				foreach (var handCollider in hand.Colliders)
				{
					if (handCollider == null || !handCollider.enabled)
						continue;

					foreach (var body in bodyColliders)
					{
						if (body == null || !body.enabled)
							continue;

						Vector3 colliderPos = handCollider.transform.position + targetPosDelta;
						Quaternion colliderRot = handCollider.transform.rotation;

						if (Physics.ComputePenetration(handCollider, colliderPos, colliderRot, body, body.transform.position, body.transform.rotation,
							out Vector3 direction, out float distance))
						{
							if (distance < 0.002f)
								continue;

							if (findClosesPoint)
							{
								Vector3 surfacePoint = body.ClosestPoint(colliderPos);
								if ((surfacePoint - colliderPos).sqrMagnitude > 0.000001f)
								{
									Vector3 normal = (colliderPos - surfacePoint).normalized;
									direction = Vector3.Slerp(direction, normal, 0.7f);
									distance = Vector3.Dot(surfacePoint - colliderPos, direction);
								}
							}

							if (distance > bestDistance)
							{
								bestDistance = distance;
								bestDirection = direction;
							}
						}
					}
				}

				if (bestDistance == 0f)
					break;

				Vector3 correction = bestDirection * bestDistance;
				correction = Vector3.ClampMagnitude(correction, maxCorrectionDistance);
				targetPosDelta += correction;
			}

			return Vector3.ClampMagnitude(targetPosDelta, maxCorrectionDistance);
		}

		void PreserveElbowPose(Transform upperArm, Transform lowerArm, Transform hand, Vector3 newHandPosition, Transform hint, Vector3 handOffset)
		{
			if (hint == null)
				return;

			Vector3 shoulder = upperArm.position;
			Vector3 oldHandDir = hand.position - shoulder;
			Vector3 newHandDir = newHandPosition - shoulder;

			// Avoid degenerate cases
			float oldHandMag = oldHandDir.magnitude;
			float newHandMag = newHandDir.magnitude;
			if (oldHandMag < 0.0001f || newHandMag < 0.0001f)
				return;

			oldHandDir /= oldHandMag;
			newHandDir /= newHandMag;

			Vector3 oldElbowVec = lowerArm.position - shoulder;
			float elbowLength = oldElbowVec.magnitude;
			if (elbowLength < 0.0001f)
				return;

			// Compute the rotation that aligns the old hand direction with the new one
			Quaternion deltaRotation = Quaternion.FromToRotation(oldHandDir, newHandDir);

			// Apply the same rotation to the old elbow vector, preserving its length
			Vector3 newElbowVec = deltaRotation * oldElbowVec;
			Vector3 newElbowPosition = shoulder + newElbowVec;

			// Blend between the original elbow position and the new one
			hint.position = Vector3.Lerp(lowerArm.position, newElbowPosition, elbowPreservation);
			hint.rotation = lowerArm.rotation; // rotation is ignored by the IK solver
		}
	}
}