/* 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 PerfectLookAtDataAdon
{
    /// <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 PerfectLookAtAdon : 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 PerfectLookAtDataAdon[] 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++ )
        {
            PerfectLookAtDataAdon lookAtBoneData = m_LookAtBones[ i ];
            lookAtBoneData.SetDefaultRotation( lookAtBoneData.m_Bone.localRotation );//设置默认旋转
            m_LastFrameRotations[ i ] = lookAtBoneData.m_Bone.localRotation;//设置上一帧旋转

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

    /*************************************************************************/
    void Update()
    {
        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 )
        {
            Debug.LogWarning( "No target object set for the component. Component won't work without a target object", this );
            return;
        }

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

            return;
        }

        //更新骨骼旋转
        if ( m_LookAtBones.Length > 0 )
        {
            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++ )
            {
                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++ )
            {
                PerfectLookAtDataAdon 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;
            }
        }
    }
}