/* For documentation please refer to this address:
http://peyman-mass.blogspot.com/2017/12/using-multiple-bones-to-look-at-target.html */

using System.Collections;
using System.Collections.Generic;
using UnityEngine;


[System.Serializable]
public class HeadLookAtData
{
    /// <summary>
    /// 默认旋转
    /// </summary>
    private Quaternion m_DefaultRotation;
    /// <summary>
    /// 骨骼
    /// </summary>
    public Transform m_Bone;
    /// <summary>
    /// 旋转的极限角度
    /// </summary>
    public float m_RotationLimit = 90.0f;
    /// <summary>
    /// 旋转向上向量权重
    /// </summary>
    public float m_RotateAroundUpVectorWeight = 0.0f;

    /// <summary>
    /// 是否重置为默认旋转
    /// </summary>
    //public bool m_ResetToDefaultRotation = false;


    public void SetDefaultRotation(Quaternion rot)
    {
        m_DefaultRotation = rot;
    }
    /// <summary>
    /// 检测极限旋转数值有效性
    /// </summary>
    public void CheckJointRotation()
    {
        if (m_RotationLimit < Mathf.Epsilon)
        {
            Debug.LogError("极限旋转为零或负。 没有生效");
        }
    }
}


/// <summary>
/// 头看向目标
/// </summary>
public class HeadLookAtTarget : MonoBehaviour
{
    /// <summary>
    /// 游戏对象缩放
    /// </summary>
    //private Vector3 m_gameObjectScale;
    /// <summary>
    /// 目标对象
    /// </summary>
    public GameObject m_TargetObject;
    public Vector3 m_UpVector = Vector3.up;
    /// <summary>
    /// 权重
    /// </summary>
    public float m_Weight = 1.0f;
    /// <summary>
    /// 看向的混合速度
    /// </summary>
    public float m_LookAtBlendSpeed = 5.0f;


    public bool m_DrawDebugLookAtLines = false;

    /// <summary>
    /// 骨骼数据列表
    /// </summary>
    public HeadLookAtData[] m_LookAtBones;

    /// <summary>
    /// 骨骼旋转列表
    /// </summary>
    private Quaternion[] m_BlendedRotations;
    /// <summary>
    /// 上一帧骨骼旋转
    /// </summary>
    private Quaternion[] m_LastFrameRotations;



    /*************************************************************************/
    public static Quaternion GetWorldLookAtRotation(Vector3 targetVector, Vector3 fwdVectorInWorldSpace)
    {
        Quaternion worldLookAtRotation;
        Vector3 rotationAxis;
        float angle = Vector3.Angle(fwdVectorInWorldSpace, targetVector) * Mathf.Deg2Rad;

        if (Mathf.Abs(angle - Mathf.PI) < Mathf.Epsilon || angle <= Mathf.Epsilon)
        {
            rotationAxis = Vector3.up;
        }
        else
        {
            rotationAxis = Vector3.Cross(fwdVectorInWorldSpace, targetVector);
        }

        worldLookAtRotation = QuaternionFromAngleAxis(ref rotationAxis, angle);


        return worldLookAtRotation;
    }

    /*************************************************************************/
    private Quaternion PerfectLookAtSlerp(Quaternion a, Quaternion b, float weight)
    {
        if (Mathf.Abs(weight - 1.0f) < Mathf.Epsilon)
        {
            return b;
        }

        if (weight < Mathf.Epsilon)
        {
            return a;
        }

        return Quaternion.Slerp(a, b, weight);
    }

    /*************************************************************************/
    public static Quaternion QuaternionFromAngleAxis(ref Vector3 rotationAxis, float angleRad)
    {
        float halfAngleRad = angleRad * 0.5f;
        rotationAxis = rotationAxis.normalized * Mathf.Sin(halfAngleRad);
        float quatW = Mathf.Cos(halfAngleRad);
        return new Quaternion(rotationAxis.x, rotationAxis.y, rotationAxis.z, quatW);
    }

    /*************************************************************************/
    private float GetAngleFromQuaternionRad(Quaternion inputQuat)
    {
        float ret = Mathf.Acos(inputQuat.w) * 2.0f;
        return ret;
    }

    /*************************************************************************/
    private float GetAngleFromQuaternionRad(ref Quaternion inputQuat)
    {
        float ret = Mathf.Acos(inputQuat.w) * 2.0f;
        return ret;
    }

    ///*************************************************************************/
    //private Vector3 GetForwardVector( ref Transform inputTr, FwdDirection inputAxis )
    //{
    //    switch ( inputAxis )
    //    {
    //        case FwdDirection.X_AXIS:
    //            return inputTr.right;

    //        case FwdDirection.Y_AXIS:
    //            return inputTr.up;

    //        case FwdDirection.Z_AXIS:
    //            return inputTr.forward;

    //        case FwdDirection.MINUS_X_AXIS:
    //            return -inputTr.right;

    //        case FwdDirection.MINUS_Y_AXIS:
    //            return -inputTr.up;

    //        case FwdDirection.MINUS_Z_AXIS:
    //            return -inputTr.forward;

    //        default:
    //            return inputTr.forward;
    //    } 
    //}




    /*************************************************************************/
    void Start()
    {
        //新建骨骼旋转列表
        m_BlendedRotations = new Quaternion[m_LookAtBones.Length];
        m_LastFrameRotations = new Quaternion[m_LookAtBones.Length];
        for (int i = 0; i < m_LookAtBones.Length; i++)
        {
            HeadLookAtData lookAtBoneData = m_LookAtBones[i];
            if (lookAtBoneData.m_Bone != null)
            {
                lookAtBoneData.SetDefaultRotation(lookAtBoneData.m_Bone.localRotation);//设置默认旋转
                m_LastFrameRotations[i] = lookAtBoneData.m_Bone.localRotation;//设置上一帧旋转

                lookAtBoneData.CheckJointRotation();//检测极限旋转数据有效性
            }
        }
    }

    /*************************************************************************/
    void Update()
    {
        if (m_TargetObject == null)
        {
            return;
        }

        if (m_LookAtBones.Length > 0)//骨骼列表存在
        {
            if (m_DrawDebugLookAtLines)//如果画线
            {
                Vector3 destination = m_TargetObject.transform.position - m_LookAtBones[0].m_Bone.position;//目标与跟骨骼的向量
                Debug.DrawLine(m_LookAtBones[0].m_Bone.position, m_LookAtBones[0].m_Bone.position + destination);//画线
            }
        }
    }

    /*************************************************************************/
    void LateUpdate()
    {
        if (m_TargetObject == null)
        {
            return;
        }

        if (m_Weight < Mathf.Epsilon)
        {
            //即使权重为零，也更新最后一帧旋转
            for (int i = 0; i < m_LookAtBones.Length; i++)
            {
                if (m_LookAtBones[i].m_Bone == null)
                {
                    continue;
                }

                HeadLookAtData lookAtBoneData = m_LookAtBones[i];
                m_LastFrameRotations[i] = lookAtBoneData.m_Bone.localRotation;
            }

            return;
        }

        //更新骨骼旋转
        if (m_LookAtBones.Length > 0)
        {
            if (m_LookAtBones[0].m_Bone == null)
            {
                return;
            }

            Vector3 rotatedInitFwdVec = m_LookAtBones[0].m_Bone.up;//初始向量
            Vector3 currentFwdVec;
            Vector3 parentFwdVec;
            Vector3 targetVector = m_TargetObject.transform.position - m_LookAtBones[0].m_Bone.position; //current vector is being updated in UpdateCurrentTargetVector
            Vector3 firstBoneRotatedInitFwdVec = rotatedInitFwdVec;
            byte numBonesToRotate = 1;

            for (int i = 0; i < m_LookAtBones.Length; i++)
            {
                if (m_LookAtBones[i].m_Bone == null)
                {
                    continue;
                }

                Transform currentBone = m_LookAtBones[i].m_Bone;
                Transform parentBone = currentBone.parent; ;

                float rotationLimit = m_LookAtBones[i].m_RotationLimit;
                bool parentExists = true;

                currentFwdVec = currentBone.up;
                parentFwdVec = parentBone.up;

                if (parentBone != null)
                {
                    currentFwdVec = currentBone.up; ;
                    parentFwdVec = parentBone.up;
                    float diffAngleFromAnim = Vector3.Angle(currentFwdVec, parentFwdVec);

                    //in case animation already has a bone with relative rotation higher than the limit specified by user
                    if (diffAngleFromAnim > rotationLimit)
                    {
                        rotationLimit = diffAngleFromAnim;
                    }
                }
                else
                {
                    parentExists = false;
                }

                Quaternion lookAtRot = GetWorldLookAtRotation(targetVector, rotatedInitFwdVec);

                if (m_LookAtBones[i].m_RotateAroundUpVectorWeight > 0.0f)
                {
                    Vector3 currentRotationAxis;
                    currentRotationAxis.x = lookAtRot.x;
                    currentRotationAxis.y = lookAtRot.y;
                    currentRotationAxis.z = lookAtRot.z;

                    //checking the up vector direction for rotation
                    float rotationSign = Mathf.Sign(Vector3.Cross(firstBoneRotatedInitFwdVec, targetVector).y);
                    Vector3 finalUpVector = rotationSign * m_UpVector;
                    finalUpVector = Vector3.Lerp(currentRotationAxis, finalUpVector, m_LookAtBones[i].m_RotateAroundUpVectorWeight);


                    lookAtRot = QuaternionFromAngleAxis(ref finalUpVector, GetAngleFromQuaternionRad(lookAtRot));

                }

                Quaternion childRotation = lookAtRot * currentBone.rotation;

                if (parentExists)
                {
                    //checking the angle difference 
                    float diffAngle = Mathf.Abs(Vector3.Angle(parentFwdVec, lookAtRot * currentFwdVec)) - rotationLimit;
                    if (diffAngle > 0)
                    {
                        {
                            Vector3 rotationAxis = new Vector3(lookAtRot.x, lookAtRot.y, lookAtRot.z);
                            float limitAngleRad = GetAngleFromQuaternionRad(ref lookAtRot) + Mathf.Deg2Rad * (-diffAngle);

                            lookAtRot = QuaternionFromAngleAxis(ref rotationAxis, limitAngleRad);
                            childRotation = lookAtRot * currentBone.rotation;

                            Quaternion currentBoneRotationLS = currentBone.localRotation;
                            m_LookAtBones[i].m_Bone.rotation = childRotation;
                            m_BlendedRotations[i] = PerfectLookAtSlerp(currentBoneRotationLS, m_LookAtBones[i].m_Bone.localRotation, m_Weight);
                        }

                        //
                        if (i != m_LookAtBones.Length - 1)
                        {
                            Vector3 currentBoneToParentPosDiff = m_LookAtBones[0].m_Bone.position - m_LookAtBones[i + 1].m_Bone.position;
                            Vector3 firstBoneToTargetDiff = m_TargetObject.transform.position - m_LookAtBones[0].m_Bone.position;
                            rotatedInitFwdVec = m_LookAtBones[0].m_Bone.up;
                            rotatedInitFwdVec.Normalize();
                            rotatedInitFwdVec = firstBoneToTargetDiff.magnitude * rotatedInitFwdVec;
                            rotatedInitFwdVec = currentBoneToParentPosDiff + rotatedInitFwdVec;

                            targetVector = currentBoneToParentPosDiff + firstBoneToTargetDiff;

                            numBonesToRotate++;

                            if (m_DrawDebugLookAtLines)
                            {
                                Debug.DrawLine(m_LookAtBones[i + 1].m_Bone.position, m_LookAtBones[i + 1].m_Bone.position + rotatedInitFwdVec, Color.green);
                                Debug.DrawLine(m_LookAtBones[i + 1].m_Bone.position, m_LookAtBones[i + 1].m_Bone.position + targetVector, Color.red);
                            }
                        }
                    }
                    else
                    {
                        Quaternion currentBoneRotationLS = currentBone.localRotation;
                        m_LookAtBones[i].m_Bone.rotation = childRotation;
                        m_BlendedRotations[i] = PerfectLookAtSlerp(currentBoneRotationLS, m_LookAtBones[i].m_Bone.localRotation, m_Weight);

                        break;
                    }
                }
                else
                {
                    Quaternion currentBoneRotationLS = currentBone.localRotation;
                    m_LookAtBones[i].m_Bone.rotation = childRotation;
                    m_BlendedRotations[i] = PerfectLookAtSlerp(currentBoneRotationLS, m_LookAtBones[i].m_Bone.localRotation, m_Weight);

                    if (i < m_LookAtBones.Length - 1)
                    {
                        Debug.LogWarning("Warning Bone name doesn't have a parent. The rest of the PerfectLookAt bone chain won't work after this bone!", this);
                        break;
                    }
                }
            }

            //Updating last frame rotations. Two loops are separated to have less jumps in memory and be more cache friendly.
            bool isBlending = Mathf.Abs(m_Weight - 1.0f) > Mathf.Epsilon;

            // Calculating the blend weight to blend between the current frame and last frame to achieve smooth rotations in dynamic animations with large range of movements.
            float smoothingWeight = Mathf.Clamp(m_LookAtBlendSpeed * Time.deltaTime, 0.0f, 1.0f);

            smoothingWeight = (smoothingWeight - 1.0f) * m_Weight + 1.0f;

            for (int k = 0; k < m_LookAtBones.Length; k++)
            {
                HeadLookAtData lookAtBoneData = m_LookAtBones[k];
                Quaternion boneLocalRotation;
                if (isBlending && k < numBonesToRotate)
                {
                    boneLocalRotation = m_BlendedRotations[k];
                }
                else
                {
                    boneLocalRotation = lookAtBoneData.m_Bone.localRotation;
                }

                Quaternion localRotationFromAnim = lookAtBoneData.m_Bone.localRotation;

                lookAtBoneData.m_Bone.localRotation = PerfectLookAtSlerp(m_LastFrameRotations[k], boneLocalRotation, smoothingWeight);

                m_LastFrameRotations[k] = lookAtBoneData.m_Bone.localRotation;
            }
        }
    }
}