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

[DisallowMultipleComponent]
[RequireComponent(typeof(Rigidbody))]
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("上浮方向速度阻尼（越大越不弹）。本版本：只在“浮力中心”施加一次，不再在每个采样点施加，避免90°附近转不动。")]
    public float verticalDamping = 0.6f;

    [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;

    [Header("Anti-stiction near upright")]
    [Tooltip("在接近竖直(例如90->80度附近)时，降低vertical damping，避免“粘住”。0=关闭。")]
    [Range(0f, 1f)]
    public float nearUprightDampingReduce = 0.6f;

    [Tooltip("接近竖直的判定角度（度）。例如 12 表示在 |angle| < 12° 附近逐步降低阻尼。")] [Range(1f, 30f)]
    public float nearUprightAngleDeg = 12f;

    #region Crest5相关信息

    public WaterRenderer _waterRenderer;

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

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

    readonly SampleFlowHelper _SampleFlowHelper = new();

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

    #endregion

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

    Rigidbody _rb;
    CapsuleCollider _cap;
    float _baseDrag, _baseAngularDrag;

    [SerializeField] private bool _init = false;

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

    void Start()
    {
        int length = Mathf.Max(3, samplePoints);
        _QueryPoints = new Vector3[length];
        _QueryResultDisplacements = new Vector3[length];
        _QueryResultVelocities = new Vector3[length];
        _QueryResultNormal = new Vector3[length];
        
    }

    public void InitBobber()
    {
        if (_waterRenderer == null && SceneSettings.Instance)
        {
            _waterRenderer = SceneSettings.Instance.Water;
        }
        _cap = GetComponentInChildren<CapsuleCollider>();
        _init = true;
    }

    void FixedUpdate()
    {
        if (!_init) return;
        if (!_waterRenderer) return;

        GetWorldCapsule(out Vector3 a, out Vector3 b, out float radius);

        int n = Mathf.Max(3, samplePoints);
        if (_QueryPoints == null || _QueryPoints.Length != n)
        {
            _QueryPoints = new Vector3[n];
            _QueryResultDisplacements = new Vector3[n];
            _QueryResultVelocities = new Vector3[n];
            _QueryResultNormal = new Vector3[n];
        }

        float fullBuoyancy = _rb.mass * Physics.gravity.magnitude * buoyancyScale;
        float perPointMax = fullBuoyancy / n;

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

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

        float subSum = 0f;
        int wetCount = 0;

        // 用于计算“浮力中心”(Center of Buoyancy)与水流速度平均
        Vector3 cobSum = Vector3.zero;
        Vector3 wvSum = Vector3.zero;
        float cobW = 0f;

        // 1) 多点只加浮力（不再在每点加vertical damping）
        for (int i = 0; i < n; i++)
        {
            Vector3 p = _QueryPoints[i];

            float waterH = _QueryResultDisplacements[i].y + _waterRenderer.SeaLevel;
            float depth = waterH - p.y;

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

            sub = Mathf.Clamp01(submergenceCurve.Evaluate(sub));

            subSum += sub;
            wetCount++;

            cobSum += p * sub;
            wvSum += _QueryResultVelocities[i] * sub;
            cobW += sub;

            Vector3 buoyForce = Vector3.up * (perPointMax * sub);
            _rb.AddForceAtPosition(buoyForce, p, ForceMode.Force);

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

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

        // 2) vertical damping：只在“浮力中心”施加一次（关键修复：不再产生抑制旋转的力矩）
        if (subAvg > 0f && cobW > 1e-6f)
        {
            Vector3 cob = cobSum / cobW;
            Vector3 waterVelAvg = wvSum / cobW;

            // 接近竖直时降低vertical damping，避免90->80度“粘住”
            float vdScale = 1f;
            if (nearUprightDampingReduce > 0f)
            {
                Vector3 axisWorld = GetAxisWorld(uprightAxis);
                float angleFromUp = Vector3.Angle(axisWorld, Vector3.up); // 0=竖直
                float t = Mathf.Clamp01(angleFromUp / Mathf.Max(0.001f, nearUprightAngleDeg));
                // t=0(很竖直) -> 1(离开竖直)
                vdScale = Mathf.Lerp(1f - nearUprightDampingReduce, 1f, t);
            }

            Vector3 pointVel = _rb.GetPointVelocity(cob);
            Vector3 relVel = pointVel - waterVelAvg;
            float vUp = Vector3.Dot(relVel, Vector3.up);

            Vector3 dampForce = -Vector3.up * (vUp * verticalDamping * _rb.mass * subAvg * vdScale);
            _rb.AddForceAtPosition(dampForce, cob, ForceMode.Force);

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

        // 3) 角阻尼：只加一次
        if (subAvg > 0f)
        {
            _rb.AddTorque(-_rb.angularVelocity * (angularDamping * _rb.mass * subAvg), ForceMode.Force);
        }

        // 4) upright（保持你原逻辑）
        if (subAvg > 0f && uprightSpring > 0f)
        {
            Vector3 axisWorld = GetAxisWorld(uprightAxis);
            Vector3 targetUp = Vector3.up;

            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);
            }
        }

        // 5) 入水 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;

        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;
    }
}