using Rigging.Data;
using Rigging.Debugging;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace Rigging.Action
{
    //动作-控制双脚的位置，让双脚和双脚的中点尽可能和重心在水平的投影重合

    public class Balance : RiggingActionBase
    {

        public Rigidbody handLeft;
        public Rigidbody handRight;
        public Rigidbody footLeft; // kneeLeft
        public Rigidbody footRight; // kneeRight
        public Rigidbody hip;

        private Vector3 centerOfMass; // 5.0759, 0, -7.2038

        private Rigidbody[] allRigs//所有14个parts
        {
            get 
            {
                return player.status.body.allRigs;
            }
        }

        public AnimationParam<float> balanceForces;

        public AnimationParam<float> footCenterForces;

        private float muscleMultiplier; //1

        private float crouchMultiplier = 1f; //1

        protected override void OnStart()
        {
            handLeft = player.body.handLeft.GetComponent<Rigidbody>();
            handRight = player.body.handRight.GetComponent<Rigidbody>();
            footLeft = player.body.kneeLeft.GetComponent<Rigidbody>();
            footRight = player.body.kneeRight.GetComponent<Rigidbody>();
            hip = player.body.hip.GetComponent<Rigidbody>();

            footCenterForces.SetPlayer(player);
            balanceForces.SetPlayer(player);
        }

        private void BalanceLegs()
        {
            using GLScope s = new GLScope(false);

            // 计算双脚的中心位置
            Vector3 vector = footLeft.transform.position + footLeft.transform.forward * 0.5f;
            Vector3 vector2 = footRight.transform.position + footRight.transform.forward * 0.5f;

            GLHandle.DrawSphere(vector, 0.1f);
            GLHandle.DrawSphere(vector2, 0.1f);

            Vector3 vector3 = (vector + vector2) / 2f;

            GLHandle.DrawSphere(vector3, 0.1f);
            GLHandle.DrawSphere(vector3 + new Vector3(0, 0.3f, 0), 0.1f);

            // 迫使双脚向质心移动
            if (!(vector3.y + 0.3f > hip.worldCenterOfMass.y))
            {
                vector3.y = 0f;//同样只考虑XOZ平面，忽略高度
                Vector3 vector4 = centerOfMass - vector3; // 质心和双脚中心的偏移

                GLHandle.DrawSphere(vector3, 0.1f);
                GLHandle.DrawSphere(centerOfMass, 0.1f);
                GLHandle.DrawSphere(hip.worldCenterOfMass, 0.1f);

                GLHandle.DrawLine(vector, vector + vector4);
                GLHandle.DrawLine(vector2, vector2 + vector4);

                footLeft.AddForceAtPosition(vector4 * muscleMultiplier * crouchMultiplier * balanceForces.current, vector, ForceMode.Acceleration);
                footRight.AddForceAtPosition(vector4 * muscleMultiplier * crouchMultiplier * balanceForces.current, vector2, ForceMode.Acceleration);
            }
        }

        private void CenterLegs()
        {
            using GLScope s = new GLScope(false);

            Vector3 vector = footLeft.transform.position + footLeft.transform.forward * 0.5f;
            Vector3 vector2 = footRight.transform.position + footRight.transform.forward * 0.5f;

            GLHandle.DrawSphere(vector, 0.1f);
            GLHandle.DrawSphere(vector2, 0.1f);

            // 迫使双脚往质心移动

            // 左脚
            Vector3 vector3 = vector;
            if (vector.y + 0.3f < hip.worldCenterOfMass.y)
            {
                vector3.y = 0f;
                footLeft.AddForceAtPosition((centerOfMass - vector3) * muscleMultiplier * footCenterForces.current, vector, ForceMode.Acceleration);
                GLHandle.DrawLine(vector, vector + (centerOfMass - vector3));
            }

            // 右脚
            Vector3 vector4 = vector2;
            if (vector4.y + 0.3f < hip.worldCenterOfMass.y)
            {
                vector4.y = 0f;
                footRight.AddForceAtPosition((centerOfMass - vector4) * muscleMultiplier * footCenterForces.current, vector2, ForceMode.Acceleration);
                GLHandle.DrawLine(vector2, vector2 + (centerOfMass - vector4));
            }
        }

        protected override void OnFixedUpdate()
        {
            //muscleMultiplier = str.strength;

            // 计算角色所有rigidbody合起来的质心在XOZ平面的投影位置（即忽略高度）
            centerOfMass = Vector3.zero;
            int num = 0;
            Rigidbody[] array = allRigs;
            foreach (Rigidbody rigidbody in array)
            {
                if ((bool)rigidbody)
                {
                    centerOfMass += rigidbody.worldCenterOfMass;
                    num++;
                }
            }
            centerOfMass /= (float)Mathf.Max(num, 1);
            centerOfMass.y = 0f;

            BalanceLegs();
            CenterLegs();
        }

        protected override void OnUpdate()
        {
        }

    }

}