using System;
using Gaia;
using UnityEngine;
using WaveHarmonic.Crest;

[DisallowMultipleComponent]
[RequireComponent(typeof(Rigidbody), typeof(CapsuleCollider))]
public class CapsuleBuoyancyStable : MonoBehaviour
{
    [Header("Buoyancy")] [Tooltip("完全浸没时总浮力 = mass*g*buoyancyScale。>1 更浮。")]
    public float buoyancyScale = 1.6f;

    [Tooltip("沿胶囊轴向采样点数量（建议 7~11）。")] [Range(3, 15)]
    public int samplePoints = 9;

    [Tooltip("浸没比例曲线（0=刚碰水, 1=充分在水下）。")] public AnimationCurve submergenceCurve = AnimationCurve.Linear(0, 0, 1, 1);

    [Header("Damping")] [Tooltip("上浮方向速度阻尼（越大越不弹）。")]
    public float verticalDamping = 3.0f;

    [Tooltip("整体角速度阻尼（只施加一次，不要太大）。")] public float angularDamping = 0.6f;

    [Header("Optional Upright Stabilizer (Recommended for bobber)")] [Tooltip("让胶囊轴向更倾向于对齐世界Up。0=关闭。")]
    public float uprightSpring = 0.0f;

    [Tooltip("upright 的角速度阻尼。")] public float uprightDamping = 0.5f;

    [Tooltip("胶囊轴向：0=X,1=Y,2=Z（通常 CapsuleCollider.direction 也一样）。")]
    public int uprightAxis = 1;

    [Header("Water Drag")] public float extraDragInWater = 0.8f;
    public float extraAngularDragInWater = 0.8f;

    #region Crest5相关信息

    public Transform Water;

    private WaterRenderer _waterRenderer;

    [Tooltip("要瞄准哪一层水的碰撞层。")] [SerializeField]
    CollisionLayer _Layer = CollisionLayer.AfterAnimatedWaves;

    [Header("波响应")] [Tooltip("用于物理计算的物体宽度。\n\n此值越大，波响应的滤波效果和平滑程度就越高。如果无法对较大波长进行滤波，则应增加 LOD 级别。")] [SerializeField]
    float _ObjectWidth = 3f;

    readonly SampleFlowHelper _SampleFlowHelper = new();

    /// <summary>
    /// 查询水面信息点位
    /// </summary>
    Vector3[] _QueryPoints;

    Vector3[] _QueryResultDisplacements;
    Vector3[] _QueryResultVelocities;
    Vector3[] _QueryResultNormal;

    #endregion

    [Header("Debug")] public bool drawDebug = false;

    Rigidbody _rb;
    CapsuleCollider _cap;
    float _baseDrag, _baseAngularDrag;

    void Awake()
    {
        _rb = GetComponent<Rigidbody>();
        _cap = GetComponent<CapsuleCollider>();
        _baseDrag = _rb.linearDamping;
        _baseAngularDrag = _rb.angularDamping;
    }

    private void Start()
    {
        int length = Mathf.Max(3, samplePoints);
        _QueryPoints = new Vector3[length];
        _QueryResultDisplacements = new Vector3[length];
        _QueryResultVelocities = new Vector3[length];
        _QueryResultNormal = new Vector3[length];
        if(Water)
            _waterRenderer = Water.GetComponent<WaterRenderer>();
    }

    void FixedUpdate()
    {
        if (Water)
        {
            
        }

        if (!_waterRenderer)
        {
            return;
        }
        GetWorldCapsule(out Vector3 a, out Vector3 b, out float radius);

        int n = Mathf.Max(3, samplePoints);
        float fullBuoyancy = _rb.mass * Physics.gravity.magnitude * buoyancyScale;
        float perPointMax = fullBuoyancy / n;

        float subSum = 0f;
        int wetCount = 0;

        for (int i = 0; i < _QueryPoints.Length; i++)
        {
            float t = (float)i / (n - 1);
            Vector3 p = Vector3.Lerp(a, b, t);
            _QueryPoints[i] = p;
        }


        // 查询
        var collisions = _waterRenderer.AnimatedWavesLod.Provider;
        collisions.Query(GetHashCode(), _ObjectWidth, _QueryPoints, _QueryResultDisplacements,
            _QueryResultNormal, _QueryResultVelocities, _Layer);


        for (int i = 0; i < n; i++)
        {
            float t = (float)i / (n - 1);
            Vector3 p = Vector3.Lerp(a, b, t);

            float waterH =
                _QueryResultDisplacements[i].y + _waterRenderer.SeaLevel; //GaiaConstants.Water.GetWaterHeight(p);
            float depth = waterH - p.y; // >0 在水下

            float sub = Mathf.InverseLerp(-radius, radius, depth); // 0..1
            if (sub <= 0f) continue;

            sub = Mathf.Clamp01(submergenceCurve.Evaluate(sub));
            subSum += sub;
            wetCount++;

            Vector3 buoyDir = Vector3.up;

            // Vector3 waterVel = GaiaConstants.Water.GetWaterVelocity(p);
            Vector3 waterVel = _QueryResultVelocities[i];
            Vector3 pointVel = _rb.GetPointVelocity(p);
            Vector3 relVel = pointVel - waterVel;

            // 浮力
            Vector3 buoyForce = buoyDir * (perPointMax * sub);

            // 只阻尼上浮方向速度分量（防弹跳）
            float vUp = Vector3.Dot(relVel, buoyDir);
            Vector3 dampForce = -buoyDir * (vUp * verticalDamping * _rb.mass * sub);

            _rb.AddForceAtPosition(buoyForce + dampForce, p, ForceMode.Force);

            if (drawDebug)
            {
                Debug.DrawLine(p, p + buoyForce / (_rb.mass * 10f), Color.cyan, 0f, false);
                Debug.DrawLine(p, p + dampForce / (_rb.mass * 10f), Color.yellow, 0f, false);
            }
        }

        float subAvg = (wetCount > 0) ? (subSum / wetCount) : 0f;

        // 角阻尼：只加一次（关键修复点）
        if (subAvg > 0f)
        {
            _rb.AddTorque(-_rb.angularVelocity * (angularDamping * _rb.mass * subAvg), ForceMode.Force);
        }

        // 可选：upright 稳定器（更像“浮漂自动立起来”）
        if (subAvg > 0f && uprightSpring > 0f)
        {
            Vector3 axisWorld = GetAxisWorld(uprightAxis);
            Vector3 targetUp = Vector3.up;

            // 误差轴：axisWorld 需要对齐 targetUp（也可反过来按你浮漂模型选）
            Vector3 errorAxis = Vector3.Cross(axisWorld, targetUp);
            float errorMag = errorAxis.magnitude;

            if (errorMag > 1e-6f)
            {
                errorAxis /= errorMag;

                // “弹簧”力矩 + 阻尼（防止在两个角度间抽动）
                Vector3 springTorque = errorAxis * (uprightSpring * errorMag * _rb.mass);
                Vector3 dampTorque = -_rb.angularVelocity * (uprightDamping * _rb.mass);

                _rb.AddTorque((springTorque + dampTorque) * subAvg, ForceMode.Force);
            }
        }

        // 入水整体 drag
        if (subAvg > 0.001f)
        {
            _rb.linearDamping = _baseDrag + extraDragInWater * subAvg;
            _rb.angularDamping = _baseAngularDrag + extraAngularDragInWater * subAvg;
        }
        else
        {
            _rb.linearDamping = _baseDrag;
            _rb.angularDamping = _baseAngularDrag;
        }
    }

    Vector3 GetAxisWorld(int axis)
    {
        return axis switch
        {
            0 => transform.right,
            2 => transform.forward,
            _ => transform.up,
        };
    }

    void GetWorldCapsule(out Vector3 a, out Vector3 b, out float radius)
    {
        Vector3 lossy = transform.lossyScale;
        int dir = _cap.direction; // 0=X,1=Y,2=Z

        float scaleAlong = (dir == 0) ? Mathf.Abs(lossy.x) : (dir == 1) ? Mathf.Abs(lossy.y) : Mathf.Abs(lossy.z);

        float scaleR;
        if (dir == 0) scaleR = Mathf.Max(Mathf.Abs(lossy.y), Mathf.Abs(lossy.z));
        else if (dir == 1) scaleR = Mathf.Max(Mathf.Abs(lossy.x), Mathf.Abs(lossy.z));
        else scaleR = Mathf.Max(Mathf.Abs(lossy.x), Mathf.Abs(lossy.y));

        radius = _cap.radius * scaleR;

        Vector3 center = transform.TransformPoint(_cap.center);
        Vector3 axisWorld = (dir == 0) ? transform.right : (dir == 1) ? transform.up : transform.forward;

        float heightWorld = Mathf.Max(0f, _cap.height * scaleAlong);
        float cylinderLen = Mathf.Max(0f, heightWorld - 2f * radius);

        Vector3 half = axisWorld * (cylinderLen * 0.5f);
        a = center - half;
        b = center + half;
    }


    private void OnDrawGizmosSelected()
    {
        if (drawDebug)
        {
            // 绘制 Rigidbody 的重心点位
            Vector3 centerOfMassWorld = transform.TransformPoint(_rb != null ? _rb.centerOfMass : Vector3.zero);
            Gizmos.color = Color.cyan;
            Gizmos.DrawSphere(centerOfMassWorld, 0.1f);
            Gizmos.DrawLine(centerOfMassWorld, centerOfMassWorld + Vector3.up * 0.5f);
        }
    }
}