using UnityEngine;

namespace Passer.Humanoid {

    [System.Serializable]
    public class LegMovements : Movements {

        #region Update
        public static void Update(FootTarget footTarget) {
            if (footTarget == null || footTarget.foot.bone.transform == null || !footTarget.humanoid.calculateBodyPose)
                return;

            footTarget.CheckGrounded();

            if (Application.isPlaying &&
                footTarget.humanoid.targetsRig.runtimeAnimatorController != null
                && footTarget.foot.target.confidence.position == 0) {

                footTarget.legMovements.FullForwardKinematics(footTarget);
            }
            else {
                if (footTarget.foot.target.confidence.rotation >= footTarget.lowerLeg.target.confidence.rotation &&
                    (footTarget.foot.target.confidence.position >= footTarget.lowerLeg.target.confidence.rotation ||
                    footTarget.ground != null))

                    footTarget.legMovements.FullInverseKinematics(footTarget);
                else if (footTarget.lowerLeg.target.confidence.rotation > footTarget.foot.target.confidence.rotation)
                    footTarget.legMovements.LowerLegForwardKinematics(footTarget);
                else
                    footTarget.legMovements.FullForwardKinematics(footTarget);
            }
        }

        protected void FullInverseKinematics(FootTarget footTarget) {
            //Debug.Log("FullInverseKinematics");

            if (footTarget.foot.target.transform == null)
                return;

            Vector3 footPosition = NaturalFootPosition(footTarget);

            Quaternion upperLegRotation = NaturalUpperLegOrientation(footTarget, footPosition);
            footTarget.upperLeg.SetBoneRotation(upperLegRotation);

            Quaternion lowerLegRotation = NaturalLowerLegOrientation(footTarget, upperLegRotation, footPosition);
            footTarget.lowerLeg.SetBoneRotation(lowerLegRotation);

            Quaternion footOrientation = NaturalFootOrientation(footTarget);
            footTarget.foot.SetBoneRotation(footOrientation);

            Quaternion toesOrientation = NaturalToesOrientation(footTarget);
            footTarget.toes.SetBoneRotation(toesOrientation);

            if (!Application.isPlaying)
                PlaceFootOnLowerLeg(footTarget, lowerLegRotation);            
        }

        protected void FullForwardKinematics(FootTarget footTarget) {
            //Debug.Log("FullForwardKinematics");
            footTarget.upperLeg.SetBoneRotation(footTarget.upperLeg.target.transform.rotation);
            footTarget.lowerLeg.SetBoneRotation(footTarget.lowerLeg.target.transform.rotation);
            footTarget.foot.SetBoneRotation(footTarget.foot.target.transform.rotation);
            footTarget.toes.SetBoneRotation(footTarget.toes.target.transform.rotation);
        }

        private void LowerLegForwardKinematics(FootTarget footTarget) {
            //Debug.Log("LowerLegForwardKinematics");

            Vector3 upperLegPosition = footTarget.upperLeg.bone.transform.position;
            Vector3 lowerLegPosition = footTarget.lowerLeg.target.transform.position;
            Quaternion lowerLegRotation = footTarget.lowerLeg.target.transform.rotation;

            Quaternion upperLegRotation = CalculateUpperLegRotation(footTarget, upperLegPosition, lowerLegPosition, lowerLegRotation);
            footTarget.upperLeg.SetBoneRotation(upperLegRotation);

            footTarget.lowerLeg.SetBoneRotation(lowerLegRotation);
            footTarget.foot.SetBoneRotation(footTarget.foot.target.transform.rotation);
            footTarget.toes.SetBoneRotation(footTarget.toes.target.transform.rotation);
        }

        #endregion

        #region Upper Leg

        protected Quaternion NaturalUpperLegOrientation(FootTarget footTarget, Vector3 footPosition) {
            Quaternion oldUpperLegRotation = footTarget.upperLeg.bone.transform.rotation * footTarget.upperLeg.bone.toTargetRotation;
            Quaternion upperLegRotation = CalculateUpperLegRotation(footTarget, footTarget.upperLeg.bone.transform.position, footPosition, footTarget.foot.target.transform.rotation, footTarget.upperLeg.bone.length, footTarget.lowerLeg.bone.length);
            //upperLegRotation = LimitRotationSpeed(oldUpperLegRotation, upperLegRotation);
            if (footTarget.isLeft)
                upperLegRotation = MeasureRotationSpeed(oldUpperLegRotation, upperLegRotation);
            return upperLegRotation;
        }

        private Quaternion lastLocalUpperLegRotation = Quaternion.identity;

        public Quaternion CalculateUpperLegRotation(FootTarget footTarget, Vector3 upperLegPosition, Vector3 footPosition, Quaternion footRotation, float upperLegLength, float lowerLegLength) {
            Vector3 upperLegForward = footPosition - upperLegPosition;
            Vector3 upperLegRight = footTarget.humanoid.hipsTarget.hips.target.transform.right;
            Vector3 upperLegUp = Vector3.Cross(upperLegForward, upperLegRight);

            float hip2FootDistance = upperLegForward.magnitude;
            float hipAngle = Mathf.Acos((Square(hip2FootDistance) + Square(upperLegLength) - Square(lowerLegLength)) / (2 * upperLegLength * hip2FootDistance)) * Mathf.Rad2Deg;
            // NaN happens when the distance to the footTarget is longer than the length of the leg
            // We will stretch the leg full then (angle = 0)
            if (float.IsNaN(hipAngle))
                hipAngle = 0;

            Quaternion upperLegRotation = Quaternion.LookRotation(upperLegForward, upperLegUp);
            upperLegRotation = Quaternion.AngleAxis(hipAngle, upperLegRotation * Vector3.left) * upperLegRotation;

            upperLegRotation *= Quaternion.Euler(270, 0, 0);
            if (footTarget.upperLeg.bone.jointLimitations)
                upperLegRotation = LimitAngle(footTarget.upperLeg, ref lastLocalUpperLegRotation, upperLegRotation);

            return upperLegRotation;
        }

        public Quaternion CalculateUpperLegRotation(FootTarget footTarget, Vector3 upperLegPosition, Vector3 lowerLegPosition, Quaternion lowerLegRotation) {
            Vector3 upperLegForward = lowerLegPosition - upperLegPosition;
            Vector3 upperLegRight = lowerLegRotation * Vector3.right;
            Vector3 upperLegUp = Vector3.Cross(upperLegForward, upperLegRight);

            Quaternion upperLegRotation = Quaternion.LookRotation(upperLegForward, upperLegUp) * Quaternion.AngleAxis(270, Vector3.right);
            if (footTarget.upperLeg.bone.jointLimitations)
                upperLegRotation = LimitAngle(footTarget.upperLeg, ref lastLocalUpperLegRotation, upperLegRotation);

            return upperLegRotation;
        }

        #endregion

        #region Lower Leg

        protected Quaternion NaturalLowerLegOrientation(FootTarget footTarget, Quaternion upperLegRotation, Vector3 footPosition) {
            float lowerLegAngle = CalculateKneeAngle(footTarget.upperLeg.bone.transform.position, footPosition, footTarget.upperLeg.bone.length, footTarget.lowerLeg.bone.length);

            if (footTarget.lowerLeg.bone.jointLimitations)
                lowerLegAngle = LimitKneeAngle(footTarget.lowerLeg, lowerLegAngle);

            Quaternion localLowerLegRotation = Quaternion.AngleAxis(lowerLegAngle, Vector3.right);
            Quaternion lowerLegRotation = upperLegRotation * localLowerLegRotation;

            //lowerLegRotation = LimitRotationSpeed(oldLowerLegRotation, lowerLegRotation);
            return lowerLegRotation;
        }

        public static float LimitKneeAngle(FootTarget.TargetedLowerLegBone lowerLeg, float angle) {
            return UnityAngles.Clamp(angle, 0, lowerLeg.bone.maxAngle);
        }

        public static Quaternion CalculateLowerLegRotation(Vector3 lowerLegPosition, Vector3 footPosition, Quaternion hipsRotation) {
            Vector3 lowerLegUp = hipsRotation * Vector3.forward;
            Quaternion lowerLegRotation = Quaternion.LookRotation(footPosition - lowerLegPosition, lowerLegUp);

            lowerLegRotation *= Quaternion.Euler(270, 0, 0);
            return lowerLegRotation;
        }

        public static float CalculateKneeAngle(Vector3 upperLegPosition, Vector3 footPosition, float upperLegLength, float lowerLegLength) {
            float dHipTarget = Vector3.Distance(upperLegPosition, footPosition);

            float kneeAngle = Mathf.Acos((Square(upperLegLength) + Square(lowerLegLength) - dHipTarget * dHipTarget) / (2 * upperLegLength * lowerLegLength)) * Mathf.Rad2Deg;
            if (float.IsNaN(kneeAngle))
                kneeAngle = 180;
            return 180 - kneeAngle;
        }

        #endregion

        #region Foot

        // Calculate the foot position taking body limitations into account
        protected Vector3 NaturalFootPosition(FootTarget footTarget) {
            if (footTarget.ground == null || !footTarget.physics)
                return footTarget.foot.target.transform.position;
            else {
                return footTarget.foot.target.transform.position + footTarget.groundDistance * footTarget.humanoid.up;
            }
        }

        // Calculate the foot orientation taking body limitation into account
        protected Quaternion NaturalFootOrientation(FootTarget footTarget) {
            Quaternion footOrientation = DetermineFootOrientation(footTarget);
            if (footTarget.foot.bone.jointLimitations)
                footOrientation = LimitAngle(footTarget.foot, ref lastLocalFootRotation, footOrientation);
            //footOrientation = LimitFootSpeed(footOrientation);
            return footOrientation;
        }

        // Limit the rotation speed of the foot to natural speeds
        private Quaternion LimitFootSpeed(FootTarget footTarget, Quaternion newFootOrientation) {
            Quaternion oldFootRotation = footTarget.foot.bone.transform.rotation * footTarget.foot.bone.toTargetRotation;

            if (footTarget.rotationSpeedLimitation)
                newFootOrientation = LimitRotationSpeed(oldFootRotation, newFootOrientation);

            return newFootOrientation;
        }

        // Limit the foot orientation from the body limitations
        private Quaternion lastLocalFootRotation = Quaternion.identity;

        // Determine foot orientation without body limitations
        private Quaternion DetermineFootOrientation(FootTarget footTarget) {
            Vector3 footAngles;
            Quaternion footOrientation;
            if (footTarget.upperLeg.target.confidence.rotation > footTarget.foot.target.confidence.rotation) {
                Vector3 upperLegAngles = footTarget.upperLeg.target.transform.eulerAngles;
                footAngles = footTarget.foot.target.transform.eulerAngles;
                footOrientation = Quaternion.Euler(footAngles.x, upperLegAngles.y, footAngles.z);
            }
            else {
                if (footTarget.ground != null) {
                    Quaternion footRotation = footTarget.foot.target.transform.rotation;
                    footOrientation = Quaternion.LookRotation(footTarget.groundNormal, footRotation * Vector3.back) * Quaternion.Euler(90, 0, 0);
                }
                else {
                    footOrientation = footTarget.foot.target.transform.rotation;
                }
            }
            return footOrientation;
        }

        public static Vector3 CalculateFootPosition(Vector3 upperLegPosition, Quaternion upperLegOrientation, float upperLegLength, Quaternion lowerLegOrientation, float lowerLegLength) {
            Vector3 lowerLegPosition = upperLegPosition + upperLegOrientation * Vector3.up * upperLegLength;
            Vector3 footPosition = lowerLegPosition + lowerLegOrientation * Vector3.up * lowerLegLength;
            return footPosition;
        }

        public static void PlaceFootOnLowerLeg(FootTarget footTarget, Quaternion lowerLegRotation) {
            footTarget.foot.bone.transform.position = footTarget.lowerLeg.bone.transform.position + lowerLegRotation * -footTarget.humanoid.up * footTarget.lowerLeg.bone.length;
        }

        #endregion

        #region Toes

        protected Quaternion NaturalToesOrientation(FootTarget footTarget) {
            Quaternion footRotation = footTarget.foot.bone.transform.rotation * footTarget.foot.bone.toTargetRotation;
            if (footTarget.ground != null) {
                return Quaternion.LookRotation(footTarget.groundNormal, footRotation * Vector3.back) * Quaternion.Euler(90, 0, 0);
            }
            else {
                return footRotation;
            }
        }

        #endregion

        private static float Square(float x) {
            return x * x;
        }
    }
}