using UnityEngine;

public interface IWaterProvider
{
    float GetWaterHeight(Vector3 worldPos);
    Vector3 GetWaterNormal(Vector3 worldPos);
    Vector3 GetWaterVelocity(Vector3 worldPos);
}

/// <summary>
/// 稳定优先的浮力（只支持 CapsuleCollider / SphereCollider）
/// - 竖直方向：目标吃水深度 + PD 控制（稳定，不抖、不弹飞）
/// - 姿态：Righting Torque 扶正（受 Rigidbody.centerOfMass 影响）
/// - 入水比例：带平滑（避免水面附近开关抖动）
/// </summary>
[DisallowMultipleComponent]
[RequireComponent(typeof(Rigidbody))]
public class BuoyancyCapsuleSphere : MonoBehaviour
{
    [Header("Collider (choose one)")]
    public CapsuleCollider capsule;
    public SphereCollider sphere;

    [Header("Water")]
    public MonoBehaviour waterProviderBehaviour; // 可选
    private IWaterProvider waterProvider;

    [Tooltip("没有 provider 时的水面高度")]
    public float waterLevel = 0f;

    [Header("Density -> Draft")]
    [Tooltip("水密度 kg/m^3（淡水约1000）")]
    public float waterDensity = 1000f;

    [Tooltip("物体等效密度 kg/m^3。越小越浮。浮漂可 80~400 之间调")]
    public float objectDensity = 250f;

    [Tooltip("额外浮力比例（手感调整）。1=按密度算")]
    public float buoyancyScale = 1f;

    [Header("Vertical PD (Stability key)")]
    [Tooltip("竖直弹簧强度（越大越“顶住”目标吃水）")]
    public float verticalKp = 35f;

    [Tooltip("竖直阻尼（越大越不弹、不抖）")]
    public float verticalKd = 12f;

    [Tooltip("最大向上加速度 m/s^2（防止从高处落下入水被顶飞）")]
    public float maxUpAccel = 25f;

    [Tooltip("最大向下加速度 m/s^2（防止强行拉下去造成抖动）")]
    public float maxDownAccel = 10f;

    [Header("Submergence smoothing")]
    [Tooltip("入水比例变化速度（1/s）。越大越快响应，越小越稳")]
    public float submergenceSpeed = 8f;

    [Tooltip("水面外的缓冲(m)，让浮力更平滑接管")]
    public float surfaceMargin = 0.01f;

    [Header("Righting (Rotation)")]
    [Tooltip("扶正强度（把 transform.up 拉向水面法线/世界上）")]
    public float rightingKp = 8f;

    [Tooltip("扶正阻尼（抑制旋转抖动）")]
    public float rightingKd = 3f;

    [Header("Water drag (optional but helpful)")]
    [Tooltip("入水时额外线阻尼（通过 rb.drag 混合）")]
    public float extraLinearDragInWater = 2.5f;

    [Tooltip("入水时额外角阻尼（通过 rb.angularDrag 混合）")]
    public float extraAngularDragInWater = 2.0f;

    [Header("Center of Mass")]
    [Tooltip("本地重心偏移：例如 (0,-0.02,0) 让底部更重、更容易站漂")]
    public Vector3 centerOfMassOffset = Vector3.zero;

    private Rigidbody rb;
    private float baseDrag, baseAngularDrag;

    // 关键：入水比例必须有“记忆”（滤波），否则水面边界必抖
    private float subFiltered = 0f;

    void Reset()
    {
        rb = GetComponent<Rigidbody>();
        rb.useGravity = true;
        rb.interpolation = RigidbodyInterpolation.Interpolate;
        rb.collisionDetectionMode = CollisionDetectionMode.Continuous;

        capsule = GetComponent<CapsuleCollider>();
        sphere = GetComponent<SphereCollider>();
    }

    void Awake()
    {
        rb = GetComponent<Rigidbody>();
        rb.centerOfMass = centerOfMassOffset;

        baseDrag = rb.linearDamping;
        baseAngularDrag = rb.angularDamping;

        waterProvider = waterProviderBehaviour as IWaterProvider;

        // 只允许一个
        if (capsule != null && sphere != null)
            sphere = null;
    }

    void OnValidate()
    {
        objectDensity = Mathf.Max(1e-3f, objectDensity);
        waterDensity = Mathf.Max(1e-3f, waterDensity);
        submergenceSpeed = Mathf.Max(0.1f, submergenceSpeed);
        surfaceMargin = Mathf.Max(0f, surfaceMargin);
        maxUpAccel = Mathf.Max(0f, maxUpAccel);
        maxDownAccel = Mathf.Max(0f, maxDownAccel);
    }

    void FixedUpdate()
    {
        if (!capsule && !sphere) return;

        waterProvider = waterProviderBehaviour as IWaterProvider;

        // 取“浮体中心点”作为控制点（稳定，不戳点）
        Vector3 centerWorld;
        float shapeHeight; // 近似“高度”（sphere=直径，capsule=高度）
        GetCenterAndHeight(out centerWorld, out shapeHeight);

        // 水面信息
        float waterH = (waterProvider != null) ? waterProvider.GetWaterHeight(centerWorld) : waterLevel;
        Vector3 waterN = (waterProvider != null) ? waterProvider.GetWaterNormal(centerWorld) : Vector3.up;
        if (waterN.sqrMagnitude < 1e-6f) waterN = Vector3.up;
        waterN.Normalize();

        // 当前中心“浸没深度”(>0 表示中心在水下)
        float centerDepth = waterH - centerWorld.y;

        // 近似入水比例：centerDepth = -H/2 -> 0; centerDepth = +H/2 -> 1
        float rawSub = Mathf.Clamp01((centerDepth + (shapeHeight * 0.5f) + surfaceMargin) / (shapeHeight + 2f * surfaceMargin));

        // 入水比例滤波（非常关键）
        float dt = Time.fixedDeltaTime;
        subFiltered = Mathf.MoveTowards(subFiltered, rawSub, submergenceSpeed * dt);

        // 混合拖拽（让水中更稳）
        rb.linearDamping = Mathf.Lerp(baseDrag, baseDrag + extraLinearDragInWater, subFiltered);
        rb.angularDamping = Mathf.Lerp(baseAngularDrag, baseAngularDrag + extraAngularDragInWater, subFiltered);

        if (subFiltered <= 1e-4f)
            return; // 基本没入水，不做任何浮力/扶正

        // 目标吃水比例：理想静态平衡 ≈ objectDensity / waterDensity（<1 才会浮）
        float desiredSub = Mathf.Clamp01((objectDensity / waterDensity) * buoyancyScale);

        // 把 desiredSub 转成目标中心深度
        // desiredSub=0 -> centerDepthTarget = -H/2（完全出水）
        // desiredSub=1 -> centerDepthTarget = +H/2（完全入水）
        float centerDepthTarget = desiredSub * shapeHeight - shapeHeight * 0.5f;

        // 竖直 PD：只沿“世界上/重力反方向”控制，最稳
        Vector3 up = (-Physics.gravity).sqrMagnitude > 1e-6f ? (-Physics.gravity).normalized : Vector3.up;
        float vUp = Vector3.Dot(rb.linearVelocity, up);

        float error = centerDepth - centerDepthTarget;     // 深了为正 -> 需要向上推
        float accelUp = (-verticalKp * error) - (verticalKd * vUp);

        // 限制上下加速度，避免顶飞或强拉抖动
        accelUp = Mathf.Clamp(accelUp, -maxDownAccel, maxUpAccel);

        // 随入水比例渐入（避免水面边界突然接管）
        accelUp *= subFiltered;

        // 施加竖直加速度（Acceleration 不受质量影响，更稳定）
        rb.AddForce(up * accelUp, ForceMode.Acceleration);

        // 扶正力矩：把物体 up 拉向 waterN（平静水就是 Vector3.up）
        // 注意：这个扶正与重心偏移一起工作，会形成“站漂/躺漂”的稳定姿态
        Vector3 currentUp = transform.up;
        Vector3 axis = Vector3.Cross(currentUp, waterN);

        // 小角度近似：axis 的大小约等于 sin(theta)
        // 扶正加速度型扭矩（同样用 Acceleration，减少质量/惯量差带来的抖动）
        Vector3 angVel = rb.angularVelocity;
        Vector3 torqueAccel = axis * rightingKp - angVel * rightingKd;

        torqueAccel *= subFiltered;

        rb.AddTorque(torqueAccel, ForceMode.Acceleration);
    }

    private void GetCenterAndHeight(out Vector3 centerWorld, out float heightWorld)
    {
        if (sphere)
        {
            // sphere：center + 半径*缩放（近似取最大缩放）
            Transform t = sphere.transform;
            centerWorld = t.TransformPoint(sphere.center);

            Vector3 s = t.lossyScale;
            float r = sphere.radius * Mathf.Max(Mathf.Abs(s.x), Mathf.Abs(s.y), Mathf.Abs(s.z));
            heightWorld = Mathf.Max(1e-6f, r * 2f);
            return;
        }

        // capsule
        Transform ct = capsule.transform;
        centerWorld = ct.TransformPoint(capsule.center);

        Vector3 ls = ct.lossyScale;
        float sx = Mathf.Abs(ls.x), sy = Mathf.Abs(ls.y), sz = Mathf.Abs(ls.z);

        float heightScale = capsule.direction switch
        {
            0 => sx,
            1 => sy,
            _ => sz,
        };

        heightWorld = Mathf.Max(1e-6f, capsule.height * heightScale);
    }
}