using System;
using UnityEngine;

namespace FIMSpace.FTools
{

    /// <summary>
    /// FC: Class for processing IK logics for multiple bones inverse kinematics
    /// </summary>
    [System.Serializable]
    public class FIK_CCDProcessor : FIK_ProcessorBase
    {
        #region CCDIK Processor

        public CCDIKBone[] IKBones; // { get; private set; }
        public CCDIKBone StartIKBone { get { return IKBones[0]; } }
        public CCDIKBone EndIKBone { get { return IKBones[IKBones.Length - 1]; } }

        public bool ContinousSolving = true;
        [Range(0f, 1f)]
        public float SyncWithAnimator = 1f;
        [Range(1, 12)]
        public int ReactionQuality = 2;
        [Range(0f, 1f)]
        public float Smoothing = 0f;
        [Range(0f, 1.5f)]
        public float StretchToTarget = 0f;
        public AnimationCurve StretchCurve = AnimationCurve.EaseInOut(0f, 0f, 1f, 1f);

        public bool Use2D = false;

        public float ActiveLength { get; private set; }

        /// <summary> Assigning bones for IK processor with CCD IK logics (unlimited bone count) </summary>
        public FIK_CCDProcessor(Transform[] bonesChain)
        {
            IKBones = new CCDIKBone[bonesChain.Length];
            Bones = new CCDIKBone[IKBones.Length];

            for (int i = 0; i < bonesChain.Length; i++)
            {
                IKBones[i] = new CCDIKBone(bonesChain[i]);
                Bones[i] = IKBones[i];
            }

            IKTargetPosition = EndBone.transform.position; IKTargetRotation = EndBone.transform.rotation;
        }


        public override void Init(Transform root)
        {
            if (Initialized) return;

            fullLength = 0f;
            for (int i = 0; i < Bones.Length; i++)
            {
                CCDIKBone b = IKBones[i];
                CCDIKBone child = null, parent = null;

                if (i > 0) parent = IKBones[i - 1];
                if (i < Bones.Length - 1)
                {
                    child = IKBones[i + 1];
                }


                if (i < Bones.Length - 1)
                {
                    IKBones[i].Init(child, parent);
                    fullLength += b.BoneLength;
                    b.ForwardOrientation = Quaternion.Inverse(b.transform.rotation) * (IKBones[i + 1].transform.position - b.transform.position);
                }
                else
                {
                    IKBones[i].Init(child, parent);
                    b.ForwardOrientation = Quaternion.Inverse(b.transform.rotation) * (IKBones[IKBones.Length - 1].transform.position - IKBones[0].transform.position);
                }
            }

            Initialized = true;
        }


        #region Methods


        /// <summary> Updating processor with n-bones oriented inverse kinematics </summary>
        public override void Update()
        {
            if (!Initialized) return;
            if (IKWeight <= 0f) return;
            CCDIKBone wb = IKBones[0];

            // Restoring previous IK progress for continous solving
            if (ContinousSolving)
            {
                while (wb != null)
                {
                    wb.LastKeyLocalRotation = wb.transform.localRotation;
                    wb.transform.localPosition = wb.LastIKLocPosition;
                    wb.transform.localRotation = wb.LastIKLocRotation;
                    wb = wb.IKChild;
                }
            }
            else
            {
                if (SyncWithAnimator > 0f)
                    // Memory for animator syncing
                    while (wb != null)
                    {
                        wb.LastKeyLocalRotation = wb.transform.localRotation;
                        wb = wb.IKChild;
                    }
            }

            if (ReactionQuality < 0) ReactionQuality = 1;

            Vector3 goalPivotOffset = Vector3.zero;
            if (ReactionQuality > 1) goalPivotOffset = GetGoalPivotOffset();

            for (int itr = 0; itr < ReactionQuality; itr++)
            {
                // Restrictions for multiple interations
                if (itr >= 1)
                    if (goalPivotOffset.sqrMagnitude == 0)
                        if (Smoothing > 0)
                            if (GetVelocityDifference() < Smoothing * Smoothing) break;

                LastLocalDirection = RefreshLocalDirection();

                Vector3 ikGoal = IKTargetPosition + goalPivotOffset;

                // Going in iterations in reversed way, from pre end child to root parent
                wb = IKBones[IKBones.Length - 2];

                if (!Use2D) // Full 3D space rotations calculations
                {
                    while (wb != null)
                    {
                        float weight = wb.MotionWeight * IKWeight;

                        if (weight > 0f)
                        {
                            Quaternion targetRotation = Quaternion.FromToRotation(Bones[Bones.Length - 1].transform.position - wb.transform.position /*fromThisToEndChildBone*/, ikGoal - wb.transform.position /*fromThisToIKGoal*/) * wb.transform.rotation;
                            if (weight < 1f) wb.transform.rotation = Quaternion.Lerp(wb.transform.rotation, targetRotation, weight);
                            else wb.transform.rotation = targetRotation;
                        }

                        wb.AngleLimiting();
                        wb = wb.IKParent;
                    }
                }
                else
                {
                    // Going in while() loop is 2x faster than for(i;i;i;) when there is more iterations
                    while (wb != null)
                    {
                        float weight = wb.MotionWeight * IKWeight;

                        if (weight > 0f)
                        {
                            Vector3 fromThisToEndChildBone = Bones[Bones.Length - 1].transform.position - wb.transform.position;
                            Vector3 fromThisToIKGoal = ikGoal - wb.transform.position;
                            wb.transform.rotation = Quaternion.AngleAxis(Mathf.DeltaAngle(Mathf.Atan2(fromThisToEndChildBone.x, fromThisToEndChildBone.y) * Mathf.Rad2Deg /* Angle to last bone */, Mathf.Atan2(fromThisToIKGoal.x, fromThisToIKGoal.y) * Mathf.Rad2Deg /* Angle to goal position */) * weight, Vector3.back) * wb.transform.rotation;
                        }

                        wb.AngleLimiting();
                        wb = wb.IKParent;
                    }
                }
            }

            LastLocalDirection = RefreshLocalDirection();

            // Support for stretching
            if (StretchToTarget > 0f)
            {
                float remainingDist = (IKTargetPosition - EndIKBone.transform.position).magnitude;

                ActiveLength = Mathf.Epsilon;
                wb = IKBones[0];
                int ind = 0;
                float boneMul = Mathf.Max(1f, StretchToTarget);

                while (wb.IKChild != null)
                {
                    if (remainingDist <= 0f)
                    {
                        break;
                    }

                    Vector3 toTarget = (IKTargetPosition - wb.transform.position);
                    Vector3 toTargetN = toTarget.normalized;

                    Vector3 prePos = wb.transform.position;
                    Vector3 preChildPos = wb.IKChild.transform.position;
                    Vector3 toNext = preChildPos - prePos;
                    Vector3 norm = toNext.normalized;

                    float dot = Vector3.Dot(norm, toTargetN);

                    if (dot > 0f)
                    {
                        float moveBy = wb.BoneLength * boneMul * dot;
                        if (moveBy > remainingDist) moveBy = remainingDist;

                        Vector3 newChildPos = preChildPos + norm * (moveBy);
                        wb.IKChild.transform.position = Vector3.Lerp(preChildPos, newChildPos, StretchToTarget);
                        wb.transform.rotation = wb.transform.rotation * Quaternion.FromToRotation(preChildPos - prePos, wb.Child.transform.position - wb.transform.position);

                        remainingDist -= Vector3.Distance(preChildPos, newChildPos);
                    }

                    wb = wb.IKChild;
                    ind += 1;
                }

                //if (stretch > 1f) for (int i = 1; i < IKBones.Length; ++i) IKBones[i].transform.position += (toGoal.normalized) * ((IKBones[i - 1].BoneLength ) * StretchCurve.Evaluate(-(1f - stretch)));
            }


            wb = IKBones[0];
            while (wb != null)
            {
                // Storing final rotations for animator offset
                wb.LastIKLocRotation = wb.transform.localRotation;
                wb.LastIKLocPosition = wb.transform.localPosition;

                // Offset based rotation sync with animator
                Quaternion ikDiff = wb.LastIKLocRotation * Quaternion.Inverse(wb.InitialLocalRotation);
                wb.transform.localRotation = Quaternion.Lerp(wb.LastIKLocRotation, ikDiff * wb.LastKeyLocalRotation, SyncWithAnimator);

                if (IKWeight < 1f)
                    wb.transform.localRotation = Quaternion.Lerp(wb.LastKeyLocalRotation, wb.transform.localRotation, IKWeight);

                wb = wb.IKChild;
            }
        }


        protected Vector3 GetGoalPivotOffset()
        {
            if (!GoalPivotOffsetDetected()) return Vector3.zero;

            Vector3 IKDirection = (IKTargetPosition - IKBones[0].transform.position).normalized;
            Vector3 secondaryDirection = new Vector3(IKDirection.y, IKDirection.z, IKDirection.x);

            if (IKBones[IKBones.Length - 2].AngleLimit < 180 || IKBones[IKBones.Length - 2].TwistAngleLimit < 180)
                secondaryDirection = IKBones[IKBones.Length - 2].transform.rotation * IKBones[IKBones.Length - 2].ForwardOrientation;

            return Vector3.Cross(IKDirection, secondaryDirection) * IKBones[IKBones.Length - 2].BoneLength * 0.5f;
        }

        private bool GoalPivotOffsetDetected()
        {
            if (!Initialized) return false;

            Vector3 toLastDirection = Bones[Bones.Length - 1].transform.position - Bones[0].transform.position;
            Vector3 toGoalDirection = IKTargetPosition - Bones[0].transform.position;

            float toLastMagn = toLastDirection.magnitude;
            float toGoalMagn = toGoalDirection.magnitude;

            if (toGoalMagn == 0) return false;
            if (toLastMagn == 0) return false;
            if (toLastMagn < toGoalMagn) return false;
            if (toLastMagn < fullLength - (Bones[Bones.Length - 2].BoneLength * 0.1f)) return false;
            if (toGoalMagn > toLastMagn) return false;

            float dot = Vector3.Dot(toLastDirection / toLastMagn, toGoalDirection / toGoalMagn);
            if (dot < 0.999f) return false;

            return true;
        }

        Vector3 RefreshLocalDirection()
        {
            LocalDirection = Bones[0].transform.InverseTransformDirection(Bones[Bones.Length - 1].transform.position - Bones[0].transform.position);
            return LocalDirection;
        }

        float GetVelocityDifference()
        { return Vector3.SqrMagnitude(LocalDirection - LastLocalDirection); }



        /// <summary> Limiting angle for all IK bones </summary>
        public void AutoLimitAngle(float angleLimit = 60f, float twistAngleLimit = 50f)
        {
            if (IKBones == null) return;

            float step = 1f / (float)IKBones.Length;
            for (int i = 0; i < IKBones.Length; i++)
            {
                IKBones[i].AngleLimit = angleLimit * Mathf.Min(1f, (i + 1) * step * 3f);
                IKBones[i].TwistAngleLimit = twistAngleLimit * Mathf.Min(1f, (i + 1) * step * 4.5f);
            }
        }


        /// <summary> Spreading weight over IK bones automatically </summary>
        public void AutoWeightBones(float baseValue = 1f)
        {
            float step = baseValue / (float)(Bones.Length * 1.3f);
            for (int i = 0; i < Bones.Length; i++)
            {
                Bones[i].MotionWeight = baseValue - step * i;
                //Bones[i].MotionWeight *= Mathf.Min(1f, (i + 1) * step1 * 3f);
            }
        }


        /// <summary> Spreading weight over IK bones with curve (Clamped01) </summary>
        public void AutoWeightBones(AnimationCurve weightCurve)
        {
            for (int i = 0; i < Bones.Length; i++)
                Bones[i].MotionWeight = Mathf.Clamp(weightCurve.Evaluate((float)i / (float)Bones.Length), 0f, 1f);
        }


        #endregion

        #endregion


        [System.Serializable]
        public class CCDIKBone : FIK_IKBoneBase
        {
            public CCDIKBone IKParent { get; private set; }
            public CCDIKBone IKChild { get; private set; }

            [Range(0f, 180f)] public float AngleLimit = 45f;
            [Range(0f, 180f)] public float TwistAngleLimit = 5f;

            /// <summary> Defined at Init() of CCD IK processor </summary>
            public Vector3 ForwardOrientation;
            public float FrameWorldLength = 1f;

            public Vector2 HingeLimits = Vector2.zero;
            public Quaternion PreviousHingeRotation;
            public float PreviousHingeAngle;

            public Vector3 LastIKLocPosition;
            public Quaternion LastIKLocRotation;

            public CCDIKBone(Transform t) : base(t) { }

            public void Init(CCDIKBone child, CCDIKBone parent)
            {
                LastIKLocPosition = transform.localPosition;
                IKParent = parent;
                if (child != null) SetChild(child);
                IKChild = child;
            }

            public override void SetChild(FIK_IKBoneBase child)
            {
                base.SetChild(child);
            }

            #region CCD IK Methods

            public void AngleLimiting()
            {
                Quaternion localRotation = Quaternion.Inverse(LastKeyLocalRotation) * transform.localRotation;
                Quaternion limitedRotation = localRotation;

                if (FEngineering.VIsZero(HingeLimits))
                {
                    if (AngleLimit < 180) limitedRotation = LimitSpherical(limitedRotation);
                    if (TwistAngleLimit < 180) limitedRotation = LimitZ(limitedRotation);
                }
                else limitedRotation = LimitHinge(limitedRotation);

                if (FEngineering.QIsSame(limitedRotation, localRotation)) return;

                transform.localRotation = LastKeyLocalRotation * limitedRotation;
            }

            private Quaternion LimitSpherical(Quaternion rotation)
            {
                if (FEngineering.QIsZero(rotation)) return rotation;
                Vector3 currentForward = rotation * ForwardOrientation;
                Quaternion limitAngle = Quaternion.RotateTowards(Quaternion.identity, Quaternion.FromToRotation(ForwardOrientation, currentForward), AngleLimit);
                return Quaternion.FromToRotation(currentForward, limitAngle * ForwardOrientation) * rotation;
            }

            private Quaternion LimitZ(Quaternion currentRotation)
            {
                Vector3 orthoOrientation = new Vector3(ForwardOrientation.y, ForwardOrientation.z, ForwardOrientation.x);
                Vector3 normal = currentRotation * ForwardOrientation;
                Vector3 tangent = orthoOrientation;
                Vector3.OrthoNormalize(ref normal, ref tangent);

                orthoOrientation = currentRotation * orthoOrientation;
                Vector3.OrthoNormalize(ref normal, ref orthoOrientation);

                Quaternion limitRot = Quaternion.FromToRotation(orthoOrientation, tangent) * currentRotation;
                if (TwistAngleLimit <= 0) return limitRot;

                return Quaternion.RotateTowards(limitRot, currentRotation, TwistAngleLimit);
            }

            private Quaternion LimitHinge(Quaternion rotation)
            {
                Quaternion addRotation = (Quaternion.FromToRotation(rotation * ForwardOrientation, ForwardOrientation) * rotation) * Quaternion.Inverse(PreviousHingeRotation);
                float addAngle = Quaternion.Angle(Quaternion.identity, addRotation);

                Vector3 orthoOrientation = new Vector3(ForwardOrientation.z, ForwardOrientation.x, ForwardOrientation.y);
                Vector3 cross = Vector3.Cross(orthoOrientation, ForwardOrientation);
                if (Vector3.Dot(addRotation * orthoOrientation, cross) > 0f) addAngle = -addAngle;

                PreviousHingeAngle = Mathf.Clamp(PreviousHingeAngle + addAngle, HingeLimits.x, HingeLimits.y);
                PreviousHingeRotation = Quaternion.AngleAxis(PreviousHingeAngle, ForwardOrientation);
                return PreviousHingeRotation;
            }

            #endregion

        }

    }


}