using System;
using UnityEngine;

namespace RootMotion.FinalIK
{
	[HelpURL("http://www.root-motion.com/finalikdox/html/page9.html")]
	[AddComponentMenu("Scripts/RootMotion.FinalIK/Grounder/Grounder Quadruped")]
	public class GrounderQuadruped : Grounder
	{
		public struct Foot
		{
			public IKSolver solver;

			public Transform transform;

			public Quaternion rotation;

			public Grounding.Leg leg;

			public Foot(IKSolver solver, Transform transform)
			{
				this.solver = solver;
				this.transform = transform;
				leg = null;
				rotation = transform.rotation;
			}
		}

		[Tooltip("The Grounding solver for the forelegs.")]
		public Grounding forelegSolver = new Grounding();

		[Tooltip("The weight of rotating the character root to the ground angle (range: 0 - 1).")]
		[Range(0f, 1f)]
		public float rootRotationWeight = 0.5f;

		[Tooltip("The maximum angle of rotating the quadruped downwards (going downhill, range: -90 - 0).")]
		[Range(-90f, 0f)]
		public float minRootRotation = -25f;

		[Tooltip("The maximum angle of rotating the quadruped upwards (going uphill, range: 0 - 90).")]
		[Range(0f, 90f)]
		public float maxRootRotation = 45f;

		[Tooltip("The speed of interpolating the character root rotation (range: 0 - inf).")]
		public float rootRotationSpeed = 5f;

		[Tooltip("The maximum IK offset for the legs (range: 0 - inf).")]
		public float maxLegOffset = 0.5f;

		[Tooltip("The maximum IK offset for the forelegs (range: 0 - inf).")]
		public float maxForeLegOffset = 0.5f;

		[Tooltip("The weight of maintaining the head's rotation as it was before solving the Grounding (range: 0 - 1).")]
		[Range(0f, 1f)]
		public float maintainHeadRotationWeight = 0.5f;

		[Tooltip("The root Transform of the character, with the rigidbody and the collider.")]
		public Transform characterRoot;

		[Tooltip("The pelvis transform. Common ancestor of both legs and the spine.")]
		public Transform pelvis;

		[Tooltip("The last bone in the spine that is the common parent for both forelegs.")]
		public Transform lastSpineBone;

		[Tooltip("The head (optional, if you intend to maintain its rotation).")]
		public Transform head;

		public IK[] legs;

		public IK[] forelegs;

		[HideInInspector]
		public Vector3 gravity = Vector3.down;

		private Foot[] feet = new Foot[0];

		private Vector3 animatedPelvisLocalPosition;

		private Quaternion animatedPelvisLocalRotation;

		private Quaternion animatedHeadLocalRotation;

		private Vector3 solvedPelvisLocalPosition;

		private Quaternion solvedPelvisLocalRotation;

		private Quaternion solvedHeadLocalRotation;

		private int solvedFeet;

		private bool solved;

		private float angle;

		private Transform forefeetRoot;

		private Quaternion headRotation;

		private float lastWeight;

		private Rigidbody characterRootRigidbody;

		[ContextMenu("User Manual")]
		protected override void OpenUserManual()
		{
			Application.OpenURL("http://www.root-motion.com/finalikdox/html/page9.html");
		}

		[ContextMenu("Scrpt Reference")]
		protected override void OpenScriptReference()
		{
			Application.OpenURL("http://www.root-motion.com/finalikdox/html/class_root_motion_1_1_final_i_k_1_1_grounder_quadruped.html");
		}

		public override void ResetPosition()
		{
			for (int i = 0; i < legs.Length; i++)
			{
				legs[i].GetIKSolver().IKPosition = feet[i].transform.position;
				if (legs[i] is LimbIK)
				{
					(legs[i] as LimbIK).solver.IKRotation = solver.legs[i].transform.rotation;
				}
			}
			solver.Reset();
			forelegSolver.Reset();
		}

		private bool IsReadyToInitiate()
		{
			if (pelvis == null)
			{
				return false;
			}
			if (lastSpineBone == null)
			{
				return false;
			}
			if (legs.Length == 0)
			{
				return false;
			}
			if (forelegs.Length == 0)
			{
				return false;
			}
			if (characterRoot == null)
			{
				return false;
			}
			if (!IsReadyToInitiateLegs(legs))
			{
				return false;
			}
			if (!IsReadyToInitiateLegs(forelegs))
			{
				return false;
			}
			return true;
		}

		private bool IsReadyToInitiateLegs(IK[] ikComponents)
		{
			foreach (IK iK in ikComponents)
			{
				if (iK == null)
				{
					return false;
				}
				if (iK is FullBodyBipedIK)
				{
					LogWarning("GrounderIK does not support FullBodyBipedIK, use CCDIK, FABRIK, LimbIK or TrigonometricIK instead. If you want to use FullBodyBipedIK, use the GrounderFBBIK component.");
					return false;
				}
				if (iK is FABRIKRoot)
				{
					LogWarning("GrounderIK does not support FABRIKRoot, use CCDIK, FABRIK, LimbIK or TrigonometricIK instead.");
					return false;
				}
				if (iK is AimIK)
				{
					LogWarning("GrounderIK does not support AimIK, use CCDIK, FABRIK, LimbIK or TrigonometricIK instead.");
					return false;
				}
			}
			return true;
		}

		private void OnDisable()
		{
			if (!base.initiated)
			{
				return;
			}
			for (int i = 0; i < feet.Length; i++)
			{
				if (feet[i].solver != null)
				{
					feet[i].solver.IKPositionWeight = 0f;
				}
			}
		}

		private void Update()
		{
			weight = Mathf.Clamp(weight, 0f, 1f);
			if (!(weight <= 0f))
			{
				solved = false;
				if (!base.initiated && IsReadyToInitiate())
				{
					Initiate();
				}
			}
		}

		private void Initiate()
		{
			feet = new Foot[legs.Length + forelegs.Length];
			Transform[] array = InitiateFeet(legs, ref feet, 0);
			Transform[] array2 = InitiateFeet(forelegs, ref feet, legs.Length);
			animatedPelvisLocalPosition = pelvis.localPosition;
			animatedPelvisLocalRotation = pelvis.localRotation;
			if (head != null)
			{
				animatedHeadLocalRotation = head.localRotation;
			}
			forefeetRoot = new GameObject().transform;
			forefeetRoot.parent = base.transform;
			forefeetRoot.name = "Forefeet Root";
			solver.Initiate(base.transform, array);
			forelegSolver.Initiate(forefeetRoot, array2);
			for (int i = 0; i < array.Length; i++)
			{
				feet[i].leg = solver.legs[i];
			}
			for (int j = 0; j < array2.Length; j++)
			{
				feet[j + legs.Length].leg = forelegSolver.legs[j];
			}
			characterRootRigidbody = characterRoot.GetComponent<Rigidbody>();
			base.initiated = true;
		}

		private Transform[] InitiateFeet(IK[] ikComponents, ref Foot[] f, int indexOffset)
		{
			Transform[] array = new Transform[ikComponents.Length];
			for (int i = 0; i < ikComponents.Length; i++)
			{
				IKSolver.Point[] points = ikComponents[i].GetIKSolver().GetPoints();
				f[i + indexOffset] = new Foot(ikComponents[i].GetIKSolver(), points[^1].transform);
				array[i] = f[i + indexOffset].transform;
				IKSolver iKSolver = f[i + indexOffset].solver;
				iKSolver.OnPreUpdate = (IKSolver.UpdateDelegate)Delegate.Combine(iKSolver.OnPreUpdate, new IKSolver.UpdateDelegate(OnSolverUpdate));
				IKSolver iKSolver2 = f[i + indexOffset].solver;
				iKSolver2.OnPostUpdate = (IKSolver.UpdateDelegate)Delegate.Combine(iKSolver2.OnPostUpdate, new IKSolver.UpdateDelegate(OnPostSolverUpdate));
			}
			return array;
		}

		private void LateUpdate()
		{
			if (!(weight <= 0f))
			{
				rootRotationWeight = Mathf.Clamp(rootRotationWeight, 0f, 1f);
				minRootRotation = Mathf.Clamp(minRootRotation, -90f, maxRootRotation);
				maxRootRotation = Mathf.Clamp(maxRootRotation, minRootRotation, 90f);
				rootRotationSpeed = Mathf.Clamp(rootRotationSpeed, 0f, rootRotationSpeed);
				maxLegOffset = Mathf.Clamp(maxLegOffset, 0f, maxLegOffset);
				maxForeLegOffset = Mathf.Clamp(maxForeLegOffset, 0f, maxForeLegOffset);
				maintainHeadRotationWeight = Mathf.Clamp(maintainHeadRotationWeight, 0f, 1f);
				RootRotation();
			}
		}

		private void RootRotation()
		{
			if (!(rootRotationWeight <= 0f) && !(rootRotationSpeed <= 0f))
			{
				solver.rotateSolver = true;
				forelegSolver.rotateSolver = true;
				Vector3 tangent = characterRoot.forward;
				Vector3 normal = -gravity;
				Vector3.OrthoNormalize(ref normal, ref tangent);
				Quaternion quaternion = Quaternion.LookRotation(tangent, -gravity);
				Vector3 vector = forelegSolver.rootHit.point - solver.rootHit.point;
				Vector3 vector2 = Quaternion.Inverse(quaternion) * vector;
				float num = Mathf.Atan2(vector2.y, vector2.z) * 57.29578f;
				num = Mathf.Clamp(num * rootRotationWeight, minRootRotation, maxRootRotation);
				angle = Mathf.Lerp(angle, num, Time.deltaTime * rootRotationSpeed);
				if (characterRootRigidbody == null)
				{
					characterRoot.rotation = Quaternion.Slerp(characterRoot.rotation, Quaternion.AngleAxis(0f - angle, characterRoot.right) * quaternion, weight);
				}
				else
				{
					characterRootRigidbody.MoveRotation(Quaternion.Slerp(characterRoot.rotation, Quaternion.AngleAxis(0f - angle, characterRoot.right) * quaternion, weight));
				}
			}
		}

		private void OnSolverUpdate()
		{
			if (!base.enabled)
			{
				return;
			}
			if (weight <= 0f)
			{
				if (lastWeight <= 0f)
				{
					return;
				}
				OnDisable();
			}
			lastWeight = weight;
			if (solved)
			{
				return;
			}
			if (OnPreGrounder != null)
			{
				OnPreGrounder();
			}
			if (pelvis.localPosition != solvedPelvisLocalPosition)
			{
				animatedPelvisLocalPosition = pelvis.localPosition;
			}
			else
			{
				pelvis.localPosition = animatedPelvisLocalPosition;
			}
			if (pelvis.localRotation != solvedPelvisLocalRotation)
			{
				animatedPelvisLocalRotation = pelvis.localRotation;
			}
			else
			{
				pelvis.localRotation = animatedPelvisLocalRotation;
			}
			if (head != null)
			{
				if (head.localRotation != solvedHeadLocalRotation)
				{
					animatedHeadLocalRotation = head.localRotation;
				}
				else
				{
					head.localRotation = animatedHeadLocalRotation;
				}
			}
			for (int i = 0; i < feet.Length; i++)
			{
				feet[i].rotation = feet[i].transform.rotation;
			}
			if (head != null)
			{
				headRotation = head.rotation;
			}
			UpdateForefeetRoot();
			solver.Update();
			forelegSolver.Update();
			pelvis.position += solver.pelvis.IKOffset * weight;
			Vector3 fromDirection = lastSpineBone.position - pelvis.position;
			Vector3 toDirection = lastSpineBone.position + forelegSolver.root.up * Mathf.Clamp(forelegSolver.pelvis.heightOffset, float.NegativeInfinity, 0f) - solver.root.up * solver.pelvis.heightOffset - pelvis.position;
			Quaternion b = Quaternion.FromToRotation(fromDirection, toDirection);
			pelvis.rotation = Quaternion.Slerp(Quaternion.identity, b, weight) * pelvis.rotation;
			for (int j = 0; j < feet.Length; j++)
			{
				SetFootIK(feet[j], (j < 2) ? maxLegOffset : maxForeLegOffset);
			}
			solved = true;
			solvedFeet = 0;
			if (OnPostGrounder != null)
			{
				OnPostGrounder();
			}
		}

		private void UpdateForefeetRoot()
		{
			Vector3 zero = Vector3.zero;
			for (int i = 0; i < forelegSolver.legs.Length; i++)
			{
				zero += forelegSolver.legs[i].transform.position;
			}
			zero /= (float)forelegs.Length;
			Vector3 vector = zero - base.transform.position;
			Vector3 normal = base.transform.up;
			Vector3 tangent = vector;
			Vector3.OrthoNormalize(ref normal, ref tangent);
			forefeetRoot.position = base.transform.position + tangent.normalized * vector.magnitude;
		}

		private void SetFootIK(Foot foot, float maxOffset)
		{
			Vector3 vector = foot.leg.IKPosition - foot.transform.position;
			foot.solver.IKPosition = foot.transform.position + Vector3.ClampMagnitude(vector, maxOffset);
			foot.solver.IKPositionWeight = weight;
		}

		private void OnPostSolverUpdate()
		{
			if (weight <= 0f || !base.enabled)
			{
				return;
			}
			solvedFeet++;
			if (solvedFeet >= feet.Length)
			{
				for (int i = 0; i < feet.Length; i++)
				{
					feet[i].transform.rotation = Quaternion.Slerp(Quaternion.identity, feet[i].leg.rotationOffset, weight) * feet[i].rotation;
				}
				if (head != null)
				{
					head.rotation = Quaternion.Lerp(head.rotation, headRotation, maintainHeadRotationWeight * weight);
				}
				solvedPelvisLocalPosition = pelvis.localPosition;
				solvedPelvisLocalRotation = pelvis.localRotation;
				if (head != null)
				{
					solvedHeadLocalRotation = head.localRotation;
				}
				if (OnPostIK != null)
				{
					OnPostIK();
				}
			}
		}

		private void OnDestroy()
		{
			if (base.initiated)
			{
				DestroyLegs(legs);
				DestroyLegs(forelegs);
			}
		}

		private void DestroyLegs(IK[] ikComponents)
		{
			foreach (IK iK in ikComponents)
			{
				if (iK != null)
				{
					IKSolver iKSolver = iK.GetIKSolver();
					iKSolver.OnPreUpdate = (IKSolver.UpdateDelegate)Delegate.Remove(iKSolver.OnPreUpdate, new IKSolver.UpdateDelegate(OnSolverUpdate));
					IKSolver iKSolver2 = iK.GetIKSolver();
					iKSolver2.OnPostUpdate = (IKSolver.UpdateDelegate)Delegate.Remove(iKSolver2.OnPostUpdate, new IKSolver.UpdateDelegate(OnPostSolverUpdate));
				}
			}
		}
	}
}