using System;
using NBF;
using UnityEngine;

[RequireComponent(typeof(LineRenderer))]
public class Rope : MonoBehaviour
{
    [Header("Anchors")] [SerializeField] public Rigidbody startAnchor;
    [SerializeField] public Rigidbody endAnchor;

    /// <summary>鱼线宽度倍数</summary>
    public int LineMultiple = 1;

    [Header("Physics (Dynamic Nodes, Fixed Segment Len)")] [SerializeField, Min(0.01f), Tooltip("物理每段固定长度（越小越细致越耗）")]
    private float physicsSegmentLen = 0.15f;

    [SerializeField, Range(2, 200)] private int minPhysicsNodes = 12;

    [SerializeField, Range(2, 400), Tooltip("物理节点上限（仅用于性能保护；与“最大长度不限制”不是一回事）")]
    private int maxPhysicsNodes = 120;

    [SerializeField] private float gravityStrength = 2.0f;
    [SerializeField, Range(0f, 1f)] private float velocityDampen = 0.95f;

    [SerializeField, Range(0.0f, 1.0f), Tooltip("约束修正强度，越大越硬。0.6~0.9 常用")]
    private float stiffness = 0.8f;

    [SerializeField, Range(1, 80), Tooltip("迭代次数。鱼线 10~30 通常够用")]
    private int iterations = 20;

    [SerializeField, Range(0, 16), Tooltip("主求解后追加的硬长度约束次数。只负责把 poly 拉回到 rest total，不改变可变长度逻辑")]
    private int hardTightenIterations = 2;

    [Header("Length Control (No Min/Max Clamp)")]
    [Tooltip("初始总长度（米）。如果为 0，则用 physicsSegmentLen*(minPhysicsNodes-1) 作为初始长度")]
    [SerializeField, Min(0f)]
    private float initialLength = 0f;

    [Tooltip("长度变化平滑时间（越小越跟手，越大越稳）")] [SerializeField, Min(0.0001f)]
    private float lengthSmoothTime = 0.15f;

    [Tooltip("当长度在变化时，额外把速度压掉一些（防抖）。0=不额外处理，1=变化时几乎清速度（建议只在收线生效）")] [SerializeField, Range(0f, 1f)]
    private float lengthChangeVelocityKill = 0.6f;

    [Tooltip("允许的最小松弛余量（避免目标长度刚好等于锚点距离时抖动）")] [SerializeField, Min(0f)]
    private float minSlack = 0.002f;

    [Header("Head Segment Clamp")] [Tooltip("第一段（起点->第1节点）允许的最小长度，避免收线时第一段被压到0导致数值炸")] [SerializeField, Min(0.0001f)]
    private float headMinLen = 0.01f;

    [Header("Node Count Stability")] [SerializeField, Tooltip("节点数切换迟滞（米）。避免长度在临界点抖动导致节点数来回跳 -> 卡顿")]
    private float nodeHysteresis = 0.05f;

    [Header("Simple Ground/Water Constraint (Cheap)")] [SerializeField]
    private bool constrainToGround = true;

    [SerializeField] private LayerMask groundMask = ~0;
    [SerializeField, Min(0f)] private float groundRadius = 0.01f;
    [SerializeField, Min(0f)] private float groundCastHeight = 1.0f;
    [SerializeField, Min(0.01f)] private float groundCastDistance = 2.5f;

    [SerializeField, Range(1, 8), Tooltip("每隔多少个节点做一次地面检测；越大越省")]
    private int groundSampleStep = 3;

    [SerializeField, Tooltip("未采样的点用插值还是直接拷贝邻近采样值")]
    private bool groundInterpolate = true;

    [SerializeField, Range(1, 8), Tooltip("每隔多少次FixedUpdate更新一次地面约束")]
    private int groundUpdateEvery = 2;

    [SerializeField, Range(0, 8), Tooltip("地面约束后，再做几次长度约束，减少 poly 被地面抬长")]
    private int groundPostConstraintIterations = 2;

    private int _groundFrameCounter;

    [Header("Simple Water Float (Cheap)")] [SerializeField, Tooltip("绳子落到水面以下时，是否把节点约束回水面")]
    private bool constrainToWaterSurface = true;

    [SerializeField, Tooltip("静态水面高度；如果你后面接波浪水面，可改成采样函数")]
    private float waterLevelY = 0f;

    [SerializeField, Min(0f), Tooltip("把线抬到水面上方一点，避免视觉穿插")]
    private float waterSurfaceOffset = 0.002f;

    [SerializeField, Range(1, 8), Tooltip("每隔多少个节点做一次水面约束采样；越大越省")]
    private int waterSampleStep = 2;

    [SerializeField, Tooltip("未采样节点是否插值水面高度")]
    private bool waterInterpolate = true;

    [SerializeField, Range(1, 8), Tooltip("每隔多少次FixedUpdate更新一次水面约束")]
    private int waterUpdateEvery = 1;

    [SerializeField, Range(0f, 1f), Tooltip("水面约束抬升强度（每次更新的插值强度），越小越渐进")]
    private float waterLiftStrength = 0.25f;

    [SerializeField, Tooltip("startAnchor 在水下时，让其相邻端节点强制跟随 startAnchor，避免被抬到水面导致脱离")]
    private bool keepStartAdjacentNodeFollow = true;

    [SerializeField, Range(0, 8), Tooltip("水面约束后，再做几次长度约束，减少局部折角")]
    private int waterPostConstraintIterations = 2;

    private int _waterFrameCounter;

    [Header("Render (High Resolution)")] [SerializeField, Min(1), Tooltip("静止时每段物理线段插值加密数量（越大越顺，越耗）")]
    private int renderSubdivisionsIdle = 6;

    [SerializeField, Min(1), Tooltip("甩动时每段物理线段插值加密数量（动态降LOD以防卡顿）")]
    private int renderSubdivisionsMoving = 2;

    [SerializeField, Min(0f), Tooltip("平均速度超过该阈值认为在甩动（用于动态降 subdiv）")]
    private float movingSpeedThreshold = 2.0f;

    [SerializeField, Tooltip("是否使用 Catmull-Rom 平滑（开启更顺，但更耗）")]
    private bool smooth = true;

    [SerializeField, Min(0.0001f)] private float lineWidth = 0.001f;

    [Header("Performance")] [SerializeField, Tooltip("远端玩家鱼线不可见时，直接停止整条渲染线的模拟与绘制")]
    private bool cullRemoteRopeWhenInvisible = true;

    [SerializeField, Tooltip("本地玩家自己的鱼线始终保持完整计算")]
    private bool localOwnerAlwaysSimulate = true;

    [SerializeField, Range(1, 60), Tooltip("每隔多少个 FixedUpdate 重新判断一次可见性")]
    private int visibilityCheckEvery = 10;

    [SerializeField, Range(0f, 0.5f), Tooltip("屏幕边缘额外留白，避免刚进视野就闪现")]
    private float visibilityViewportPadding = 0.08f;

    [Header("Air Drag (Stable)")] [SerializeField, Range(0f, 5f), Tooltip("空气阻力（Y向），指数衰减，越大越不飘")]
    private float airDrag = 0.9f;

    [SerializeField, Range(0f, 2f), Tooltip("横向额外阻力（XZ），指数衰减，越大越不左右飘")]
    private float airDragXZ = 0.6f;

    private LineRenderer _lineRenderer;

    // physics
    private int _physicsNodes;
    private Vector3[] _pCurr;
    private Vector3[] _pPrev;

    // render (一次性分配到最大，后续不再 new)
    private Vector3[] _rPoints;
    private int _rCapacity;

    private Vector3 _gravity;

    // length control runtime
    private float _targetLength;
    private float _currentLength;
    private float _lengthSmoothVel;

    // rest length head
    private float _headRestLen;

    // node stability
    private int _lastDesiredNodes = 0;

    // caches
    private Transform _startTr;
    private Transform _endTr;

    // precomputed
    private float _dt;
    private float _dt2;
    private float _kY;
    private float _kXZ;
    private Transform _cameraTr;
    private int _visibilityCheckCounter;
    private bool _isCulledByVisibility;
    private int _tIdleSubdiv = -1;
    private int _tMovingSubdiv = -1;

    private FRod _rod;
    public void Init(FRod rod)
    {
        _rod = rod;
        if (Application.isPlaying)
            RefreshVisibilityState(true);
    }

    // Catmull t caches（只缓存 idle/moving 两档，减少每帧重复乘法）
    private struct TCaches
    {
        public float[] t;
        public float[] t2;
        public float[] t3;
    }

    private TCaches _tIdle;
    private TCaches _tMoving;

    private void Awake()
    {
        _lineRenderer = GetComponent<LineRenderer>();
        _gravity = new Vector3(0f, -gravityStrength, 0f);

        RefreshAnchorTransforms();

        InitLengthSystem();
        AllocateAndInitNodes();
        EnsureRenderCaches();
        RefreshVisibilityState(true);
    }

    private void OnValidate()
    {
        renderSubdivisionsIdle = Mathf.Max(renderSubdivisionsIdle, 1);
        renderSubdivisionsMoving = Mathf.Max(renderSubdivisionsMoving, 1);
        iterations = Mathf.Clamp(iterations, 1, 80);
        hardTightenIterations = Mathf.Clamp(hardTightenIterations, 0, 16);
        groundCastDistance = Mathf.Max(groundCastDistance, 0.01f);
        groundCastHeight = Mathf.Max(groundCastHeight, 0f);
        lineWidth = Mathf.Max(lineWidth, 0.0001f);

        lengthSmoothTime = Mathf.Max(lengthSmoothTime, 0.0001f);

        physicsSegmentLen = Mathf.Max(physicsSegmentLen, 0.01f);
        minPhysicsNodes = Mathf.Max(minPhysicsNodes, 2);
        maxPhysicsNodes = Mathf.Max(maxPhysicsNodes, minPhysicsNodes);

        headMinLen = Mathf.Max(headMinLen, 0.0001f);
        nodeHysteresis = Mathf.Max(0f, nodeHysteresis);

        groundSampleStep = Mathf.Max(1, groundSampleStep);
        groundUpdateEvery = Mathf.Max(1, groundUpdateEvery);
        groundPostConstraintIterations = Mathf.Clamp(groundPostConstraintIterations, 0, 8);

        waterSampleStep = Mathf.Max(1, waterSampleStep);
        waterUpdateEvery = Mathf.Max(1, waterUpdateEvery);
        waterSurfaceOffset = Mathf.Max(0f, waterSurfaceOffset);
        waterLiftStrength = Mathf.Clamp01(waterLiftStrength);
        waterPostConstraintIterations = Mathf.Clamp(waterPostConstraintIterations, 0, 8);
        visibilityCheckEvery = Mathf.Clamp(visibilityCheckEvery, 1, 60);
        visibilityViewportPadding = Mathf.Clamp(visibilityViewportPadding, 0f, 0.5f);
    }

    private void RefreshAnchorTransforms()
    {
        _startTr = startAnchor ? startAnchor.transform : null;
        _endTr = endAnchor ? endAnchor.transform : null;
    }

    private bool ShouldAlwaysSimulate()
    {
        if (!localOwnerAlwaysSimulate)
            return false;

        var owner = _rod?.PlayerItem?.Owner;
        return owner == null || owner.IsSelf;
    }

    private Transform GetActiveCameraTransform()
    {
        Camera main = BaseCamera.Main;
        if (main)
        {
            _cameraTr = main.transform;
            return _cameraTr;
        }

        if (!_cameraTr)
        {
            Camera fallback = Camera.main;
            if (fallback)
                _cameraTr = fallback.transform;
        }

        return _cameraTr;
    }

    private static bool IsViewportPointVisible(Vector3 viewportPoint, float padding)
    {
        if (viewportPoint.z <= 0f)
            return false;

        return viewportPoint.x >= -padding && viewportPoint.x <= 1f + padding &&
               viewportPoint.y >= -padding && viewportPoint.y <= 1f + padding;
    }

    private bool IsVisibleToMainCamera()
    {
        Transform camTr = GetActiveCameraTransform();
        if (!camTr)
            return true;

        Camera cam = camTr.GetComponent<Camera>();
        if (!cam)
            cam = BaseCamera.Main ? BaseCamera.Main : Camera.main;
        if (!cam)
            return true;

        Vector3 start = _startTr ? _startTr.position : (startAnchor ? startAnchor.position : transform.position);
        Vector3 end = _endTr ? _endTr.position : (endAnchor ? endAnchor.position : transform.position);
        Vector3 middle = (start + end) * 0.5f;
        float padding = visibilityViewportPadding;

        return IsViewportPointVisible(cam.WorldToViewportPoint(start), padding) ||
               IsViewportPointVisible(cam.WorldToViewportPoint(end), padding) ||
               IsViewportPointVisible(cam.WorldToViewportPoint(middle), padding);
    }

    private void RefreshVisibilityState(bool force = false)
    {
        if (!cullRemoteRopeWhenInvisible || ShouldAlwaysSimulate())
        {
            _isCulledByVisibility = false;
            if (_lineRenderer)
                _lineRenderer.enabled = true;
            return;
        }

        if (!force)
        {
            _visibilityCheckCounter++;
            if (_visibilityCheckCounter < visibilityCheckEvery)
                return;
        }

        _visibilityCheckCounter = 0;
        bool wasCulled = _isCulledByVisibility;
        _isCulledByVisibility = !IsVisibleToMainCamera();

        if (_lineRenderer)
            _lineRenderer.enabled = !_isCulledByVisibility;

        if (wasCulled && !_isCulledByVisibility)
            SyncVisibleStateAfterCulling();
    }

    private void SyncVisibleStateAfterCulling()
    {
        _currentLength = Mathf.Max(_targetLength, 0.01f);
        UpdateNodesFromLength();
        UpdateHeadRestLenFromCurrentLength();
        ResetNodesBetweenAnchors();
        LockAnchorsHard();
    }

    private void ResetNodesBetweenAnchors()
    {
        if (_physicsNodes < 2)
            return;

        Vector3 start = _startTr ? _startTr.position : (startAnchor ? startAnchor.position : transform.position);
        Vector3 end = _endTr ? _endTr.position : (endAnchor ? endAnchor.position : transform.position);
        int last = _physicsNodes - 1;

        for (int i = 0; i <= last; i++)
        {
            float t = (last > 0) ? i / (float)last : 0f;
            Vector3 pos = Vector3.Lerp(start, end, t);
            _pCurr[i] = pos;
            _pPrev[i] = pos;
        }
    }

    private void EnsureRenderCaches()
    {
        int idle = Mathf.Max(1, renderSubdivisionsIdle);
        if (_tIdleSubdiv != idle)
        {
            BuildTCaches(idle, ref _tIdle);
            _tIdleSubdiv = idle;
        }

        int moving = Mathf.Max(1, renderSubdivisionsMoving);
        if (_tMovingSubdiv != moving)
        {
            BuildTCaches(moving, ref _tMoving);
            _tMovingSubdiv = moving;
        }

        int maxSubdiv = Mathf.Max(idle, moving);
        int neededCapacity = (maxPhysicsNodes - 1) * maxSubdiv + 1;
        if (_rPoints == null || neededCapacity > _rCapacity)
        {
            _rCapacity = neededCapacity;
            _rPoints = new Vector3[_rCapacity];
        }
    }

    private void InitLengthSystem()
    {
        float defaultLen = physicsSegmentLen * (Mathf.Max(minPhysicsNodes, 2) - 1);
        _currentLength = (initialLength > 0f) ? initialLength : defaultLen;
        _targetLength = _currentLength;
    }

    private void AllocateAndInitNodes()
    {
        _physicsNodes = Mathf.Clamp(ComputeDesiredNodesStable(_currentLength), 2, maxPhysicsNodes);

        _pCurr = new Vector3[maxPhysicsNodes];
        _pPrev = new Vector3[maxPhysicsNodes];

        Vector3 start = startAnchor ? startAnchor.position : transform.position;
        Vector3 dir = Vector3.down;

        for (int i = 0; i < _physicsNodes; i++)
        {
            Vector3 pos = start + dir * (physicsSegmentLen * i);
            _pCurr[i] = pos;
            _pPrev[i] = pos;
        }

        UpdateHeadRestLenFromCurrentLength();

        if (startAnchor && endAnchor)
            LockAnchorsHard();
    }

    private int ComputeDesiredNodes(float lengthMeters)
    {
        int desired = Mathf.RoundToInt(Mathf.Max(0f, lengthMeters) / physicsSegmentLen) + 1;
        desired = Mathf.Clamp(desired, minPhysicsNodes, maxPhysicsNodes);
        return desired;
    }

    private int ComputeDesiredNodesStable(float lengthMeters)
    {
        int desired = ComputeDesiredNodes(lengthMeters);

        if (_lastDesiredNodes == 0)
        {
            _lastDesiredNodes = desired;
            return desired;
        }

        if (desired == _lastDesiredNodes)
            return desired;

        float boundary = (_lastDesiredNodes - 1) * physicsSegmentLen;
        if (Mathf.Abs(lengthMeters - boundary) < nodeHysteresis)
            return _lastDesiredNodes;

        _lastDesiredNodes = desired;
        return desired;
    }

    public void SetTargetLength(float lengthMeters) => _targetLength = Mathf.Max(0f, lengthMeters);
    public float GetCurrentLength() => _currentLength;
    public float GetTargetLength() => _targetLength;
    public float GetLengthSmoothVel() => _lengthSmoothVel;

    public float GetLengthByPoints()
    {
        if (!smooth)
            return GetPhysicsPolylineLength();

        if (_rPoints == null || _lineRenderer == null) return 0f;

        int count = _lineRenderer.positionCount;
        if (count < 2) return 0f;

        float totalLength = 0f;
        for (int i = 1; i < count; i++)
        {
            Vector3 a = _rPoints[i - 1];
            Vector3 b = _rPoints[i];
            totalLength += Vector3.Distance(a, b);
        }

        return totalLength;
    }

    public float GetPhysicsPolylineLength()
    {
        float total = 0f;
        for (int i = 1; i < _physicsNodes; i++)
            total += Vector3.Distance(_pCurr[i - 1], _pCurr[i]);
        return total;
    }

    public void DebugLength()
    {
        float solverRestTotal = (_physicsNodes - 2) * physicsSegmentLen + _headRestLen;
        float poly = GetPhysicsPolylineLength();
        float maxSegDelta = 0f;
        float avgSegDelta = 0f;
        for (int i = 1; i < _physicsNodes; i++)
        {
            float rest = (i == 1) ? _headRestLen : physicsSegmentLen;
            float segLen = Vector3.Distance(_pCurr[i - 1], _pCurr[i]);
            float delta = segLen - rest;
            if (delta > maxSegDelta) maxSegDelta = delta;
            avgSegDelta += delta;
        }

        if (_physicsNodes > 1)
            avgSegDelta /= (_physicsNodes - 1);

        Debug.Log(
            $"current={_currentLength}, target={_targetLength}, nodes={_physicsNodes}, " +
            $"seg={physicsSegmentLen}, head={_headRestLen}, headMin={headMinLen}, " +
            $"solverRestTotal={solverRestTotal}, poly={poly}, delta={poly - solverRestTotal}, " +
            $"maxSegDelta={maxSegDelta}, avgSegDelta={avgSegDelta}"
        );
    }

    private void FixedUpdate()
    {
        if (!startAnchor || !endAnchor) return;

        RefreshAnchorTransforms();
        RefreshVisibilityState();
        if (_isCulledByVisibility)
            return;

        _dt = Time.fixedDeltaTime;
        if (_dt < 1e-6f) _dt = 1e-6f;
        _dt2 = _dt * _dt;

        _gravity.y = -gravityStrength;

        _kY = Mathf.Exp(-airDrag * _dt);
        _kXZ = Mathf.Exp(-airDragXZ * _dt);

        UpdateLengthSmooth();
        UpdateNodesFromLength();
        UpdateHeadRestLenFromCurrentLength();

        Simulate_VerletFast();


        for (int it = 0; it < iterations; it++)
        {
            LockAnchorsHard();
            SolveDistanceConstraints_HeadOnly_Fast();
        }

        SolveHardDistanceConstraints(hardTightenIterations);
        LockAnchorsHard();

        if (constrainToGround)
        {
            _groundFrameCounter++;
            if (_groundFrameCounter >= groundUpdateEvery)
            {
                _groundFrameCounter = 0;
                ConstrainToGround();
                SolveHardDistanceConstraints(groundPostConstraintIterations);
            }
        }

        if (constrainToWaterSurface)
        {
            _waterFrameCounter++;
            if (_waterFrameCounter >= waterUpdateEvery)
            {
                _waterFrameCounter = 0;
                ConstrainToWaterSurface();

                // 水面抬升后补几次长度约束，让形状更顺一点
                SolveHardDistanceConstraints(waterPostConstraintIterations);
            }
        }

        LockAnchorsHard();
    }

    private void LateUpdate()
    {
        if (!startAnchor || !endAnchor || _pCurr == null || _physicsNodes < 2) return;

        RefreshAnchorTransforms();
        if (_isCulledByVisibility)
            return;

        EnsureRenderCaches();

        int last = _physicsNodes - 1;

        Vector3 s = _startTr.position;
        Vector3 e = _endTr.position;

        _pCurr[0] = s;
        _pCurr[last] = e;
        // _pPrev[0] = s;
        // _pPrev[last] = e;

        DrawHighResLine_Fast();
    }

    private void UpdateLengthSmooth()
    {
        float minFeasible = 0.01f;
        float desired = Mathf.Max(_targetLength, minFeasible);

        _currentLength = Mathf.SmoothDamp(
            _currentLength,
            desired,
            ref _lengthSmoothVel,
            lengthSmoothTime,
            Mathf.Infinity,
            Time.fixedDeltaTime
        );

        // 长度变化时额外压一点速度，减少收放线时抖动
        float delta = Mathf.Abs(_targetLength - _currentLength);
        if (delta > 0.0001f && lengthChangeVelocityKill > 0f)
        {
            float keep = 1f - Mathf.Clamp01(lengthChangeVelocityKill);
            for (int i = 1; i < _physicsNodes - 1; i++)
            {
                Vector3 curr = _pCurr[i];
                Vector3 prev = _pPrev[i];
                Vector3 disp = curr - prev;
                _pPrev[i] = curr - disp * keep;
            }
        }
    }

    private void UpdateNodesFromLength()
    {
        int desired = ComputeDesiredNodesStable(_currentLength);
        desired = Mathf.Clamp(desired, 2, maxPhysicsNodes);
        if (desired == _physicsNodes) return;

        if (desired > _physicsNodes) AddNodesAtStart(desired - _physicsNodes);
        else RemoveNodesAtStart(_physicsNodes - desired);

        _physicsNodes = desired;
    }

    private void AddNodesAtStart(int addCount)
    {
        if (addCount <= 0) return;

        int oldCount = _physicsNodes;
        int newCount = Mathf.Min(oldCount + addCount, maxPhysicsNodes);
        addCount = newCount - oldCount;
        if (addCount <= 0) return;

        Array.Copy(_pCurr, 1, _pCurr, 1 + addCount, oldCount - 1);
        Array.Copy(_pPrev, 1, _pPrev, 1 + addCount, oldCount - 1);

        Vector3 s = _startTr ? _startTr.position : startAnchor.position;

        Vector3 dir = Vector3.down;
        int firstOld = 1 + addCount;
        if (oldCount >= 2 && firstOld < maxPhysicsNodes)
        {
            Vector3 toOld1 = (_pCurr[firstOld] - s);
            float sq = toOld1.sqrMagnitude;
            if (sq > 1e-6f) dir = toOld1 / Mathf.Sqrt(sq);
        }

        Vector3 inheritDisp = Vector3.zero;
        if (oldCount >= 2 && firstOld < maxPhysicsNodes)
            inheritDisp = (_pCurr[firstOld] - _pPrev[firstOld]);

        for (int k = 1; k <= addCount; k++)
        {
            Vector3 pos = s + dir * (physicsSegmentLen * k);
            _pCurr[k] = pos;
            _pPrev[k] = pos - inheritDisp;
        }

        LockAnchorsHard();
    }

    private void RemoveNodesAtStart(int removeCount)
    {
        if (removeCount <= 0) return;

        int oldCount = _physicsNodes;
        int newCount = Mathf.Max(oldCount - removeCount, 2);
        removeCount = oldCount - newCount;
        if (removeCount <= 0) return;

        Array.Copy(_pCurr, 1 + removeCount, _pCurr, 1, newCount - 2);
        Array.Copy(_pPrev, 1 + removeCount, _pPrev, 1, newCount - 2);

        LockAnchorsHard();
    }

    private void UpdateHeadRestLenFromCurrentLength()
    {
        int fixedSegCount = Mathf.Max(0, _physicsNodes - 2);
        float baseLen = fixedSegCount * physicsSegmentLen;

        _headRestLen = _currentLength - baseLen;
        _headRestLen = Mathf.Clamp(_headRestLen, headMinLen, physicsSegmentLen * 1.5f);
    }

    private void Simulate_VerletFast()
    {
        for (int i = 1; i < _physicsNodes - 1; i++)
        {
            Vector3 disp = _pCurr[i] - _pPrev[i];

            disp.x *= _kXZ;
            disp.z *= _kXZ;
            disp.y *= _kY;

            disp *= velocityDampen;

            Vector3 next = _pCurr[i] + disp + _gravity * _dt2;

            _pPrev[i] = _pCurr[i];
            _pCurr[i] = next;
        }
    }

    private void LockAnchorsHard()
    {
        if (!startAnchor || !endAnchor || _pCurr == null || _pPrev == null || _physicsNodes < 2) return;

        Vector3 s = _startTr ? _startTr.position : startAnchor.position;
        Vector3 e = _endTr ? _endTr.position : endAnchor.position;

        _pCurr[0] = s;
        _pPrev[0] = s - startAnchor.linearVelocity * _dt;

        int last = _physicsNodes - 1;
        _pCurr[last] = e;
        _pPrev[last] = e - endAnchor.linearVelocity * _dt;
    }

    private void SolveDistanceConstraints_HeadOnly_Fast()
    {
        SolveDistanceConstraints_HeadOnly_Bidirectional(stiffness);
    }

    private void SolveHardDistanceConstraints(int extraIterations)
    {
        for (int it = 0; it < extraIterations; it++)
        {
            LockAnchorsHard();
            SolveDistanceConstraints_HeadOnly_Hard();
        }
    }

    private void SolveDistanceConstraints_HeadOnly_Hard()
    {
        SolveDistanceConstraints_HeadOnly_Bidirectional(1f);
    }

    private void SolveDistanceConstraints_HeadOnly_Bidirectional(float combinedStiffness)
    {
        int last = _physicsNodes - 1;
        if (last <= 0) return;

        float clamped = Mathf.Clamp01(combinedStiffness);
        float sweepStiffness = (clamped >= 0.999999f) ? 1f : 1f - Mathf.Sqrt(1f - clamped);
        SolveDistanceConstraintsSweep_Fast(0, last, 1, last, sweepStiffness);
        SolveDistanceConstraintsSweep_Fast(last - 1, -1, -1, last, sweepStiffness);
    }

    private void SolveDistanceConstraintsSweep_Fast(int start, int endExclusive, int step, int last, float sweepStiffness)
    {
        for (int i = start; i != endExclusive; i += step)
        {
            float rest = (i == 0) ? _headRestLen : physicsSegmentLen;

            Vector3 a = _pCurr[i];
            Vector3 b = _pCurr[i + 1];

            Vector3 delta = b - a;
            float sq = delta.sqrMagnitude;
            if (sq < 1e-12f) continue;

            float dist = Mathf.Sqrt(sq);
            float diff = (dist - rest) / dist;
            Vector3 corr = delta * (diff * sweepStiffness);

            bool aLocked = (i == 0);
            bool bLocked = (i + 1 == last);

            if (!aLocked && !bLocked)
            {
                _pCurr[i] = a + corr * 0.5f;
                _pCurr[i + 1] = b - corr * 0.5f;
            }
            else if (aLocked && !bLocked)
            {
                _pCurr[i + 1] = b - corr; // 首段：node1 吃满
            }
            else if (!aLocked)
            {
                _pCurr[i] = a + corr; // 尾段：last-1 吃满
            }
            // 两边都锁的情况理论上不会出现
        }
    }

    private void ConstrainToGround()
    {
        if (groundMask == 0) return;

        int last = _physicsNodes - 1;
        int step = Mathf.Max(1, groundSampleStep);

        int prevSampleIdx = 1;
        float prevMinY = SampleMinY(_pCurr[prevSampleIdx]);

        ApplyMinY(prevSampleIdx, prevMinY);

        for (int i = 1 + step; i < last; i += step)
        {
            float nextMinY = SampleMinY(_pCurr[i]);
            ApplyMinY(i, nextMinY);

            if (groundInterpolate)
            {
                int a = prevSampleIdx;
                int b = i;
                int span = b - a;
                for (int j = 1; j < span; j++)
                {
                    int idx = a + j;
                    float t = j / (float)span;
                    float minY = Mathf.Lerp(prevMinY, nextMinY, t);
                    ApplyMinY(idx, minY);
                }
            }
            else
            {
                for (int idx = prevSampleIdx + 1; idx < i; idx++)
                    ApplyMinY(idx, prevMinY);
            }

            prevSampleIdx = i;
            prevMinY = nextMinY;
        }

        for (int i = prevSampleIdx + 1; i < last; i++)
            ApplyMinY(i, prevMinY);
    }

    private float SampleMinY(Vector3 p)
    {
        Vector3 origin = p + Vector3.up * groundCastHeight;
        if (Physics.Raycast(origin, Vector3.down, out RaycastHit hit, groundCastDistance, groundMask,
                QueryTriggerInteraction.Ignore))
            return hit.point.y + groundRadius;

        return float.NegativeInfinity;
    }

    private void ApplyMinY(int i, float minY)
    {
        if (float.IsNegativeInfinity(minY)) return;

        Vector3 p = _pCurr[i];
        if (p.y < minY)
        {
            p.y = minY;
            _pCurr[i] = p;

            // prev 同步抬上来，避免下一帧又被惯性拉回去造成抖动
            Vector3 prev = _pPrev[i];
            if (prev.y < minY) prev.y = minY;
            _pPrev[i] = prev;
        }
    }

    private void ConstrainToWaterSurface()
    {
        int last = _physicsNodes - 1;
        if (last <= 1) return;

        int step = Mathf.Max(1, waterSampleStep);
        float surfaceY = waterLevelY + waterSurfaceOffset;
        bool startUnderWater = _pCurr[0].y < surfaceY;
        int startAdjacentIdx = GetStartAdjacentNodeIndex(last);

        int prevSampleIdx = 1;
        float prevSurfaceY = surfaceY;

        ApplyWaterSurface(prevSampleIdx, prevSurfaceY, startUnderWater, startAdjacentIdx);

        for (int i = 1 + step; i < last; i += step)
        {
            float nextSurfaceY = surfaceY;
            ApplyWaterSurface(i, nextSurfaceY, startUnderWater, startAdjacentIdx);

            if (waterInterpolate)
            {
                int a = prevSampleIdx;
                int b = i;
                int span = b - a;
                for (int j = 1; j < span; j++)
                {
                    int idx = a + j;
                    float t = j / (float)span;
                    float y = Mathf.Lerp(prevSurfaceY, nextSurfaceY, t);
                    ApplyWaterSurface(idx, y, startUnderWater, startAdjacentIdx);
                }
            }
            else
            {
                for (int idx = prevSampleIdx + 1; idx < i; idx++)
                    ApplyWaterSurface(idx, prevSurfaceY, startUnderWater, startAdjacentIdx);
            }

            prevSampleIdx = i;
            prevSurfaceY = nextSurfaceY;
        }

        for (int i = prevSampleIdx + 1; i < last; i++)
            ApplyWaterSurface(i, prevSurfaceY, startUnderWater, startAdjacentIdx);
    }

    private int GetStartAdjacentNodeIndex(int last)
    {
        if (last <= 1) return 1;

        Vector3 s = _pCurr[0];
        float d1 = (_pCurr[1] - s).sqrMagnitude;
        float d2 = (_pCurr[last - 1] - s).sqrMagnitude;
        return d1 <= d2 ? 1 : last - 1;
    }

    private void ApplyWaterSurface(int i, float surfaceY, bool startUnderWater, int startAdjacentIdx)
    {
        if (keepStartAdjacentNodeFollow && startUnderWater && i == startAdjacentIdx)
        {
            Vector3 s = _pCurr[0];
            _pCurr[i] = s;
            _pPrev[i] = s;
            return;
        }

        Vector3 p = _pCurr[i];
        if (p.y < surfaceY)
        {
            p.y = Mathf.Lerp(p.y, surfaceY, waterLiftStrength);
            _pCurr[i] = p;

            // 渐进同步 prev，削弱向下惯性，避免反复穿透水面
            Vector3 prev = _pPrev[i];
            if (prev.y < p.y) prev.y = Mathf.Lerp(prev.y, p.y, waterLiftStrength);
            _pPrev[i] = prev;
        }
    }

    private void DrawHighResLine_Fast()
    {
        if (_pCurr == null || _physicsNodes < 2) return;

        float w = lineWidth * LineMultiple;
        _lineRenderer.startWidth = w;
        _lineRenderer.endWidth = w;

        if (!smooth)
        {
            _lineRenderer.positionCount = _physicsNodes;
            _lineRenderer.SetPositions(_pCurr);
            return;
        }

        int subdiv = PickRenderSubdivisions_Fast();
        TCaches tc = (subdiv == renderSubdivisionsMoving) ? _tMoving : _tIdle;

        int needed = (_physicsNodes - 1) * subdiv + 1;
        if (needed > _rCapacity)
        {
            _rCapacity = needed;
            _rPoints = new Vector3[_rCapacity];
        }

        int idx = 0;
        int last = _physicsNodes - 1;

        for (int seg = 0; seg < last; seg++)
        {
            int i0 = seg - 1;
            if (i0 < 0) i0 = 0;
            int i1 = seg;
            int i2 = seg + 1;
            int i3 = seg + 2;
            if (i3 > last) i3 = last;

            Vector3 p0 = _pCurr[i0];
            Vector3 p1 = _pCurr[i1];
            Vector3 p2 = _pCurr[i2];
            Vector3 p3 = _pCurr[i3];

            for (int s = 0; s < subdiv; s++)
            {
                float t = tc.t[s];
                float t2 = tc.t2[s];
                float t3 = tc.t3[s];

                Vector3 cr =
                    0.5f * (
                        (2f * p1) +
                        (-p0 + p2) * t +
                        (2f * p0 - 5f * p1 + 4f * p2 - p3) * t2 +
                        (-p0 + 3f * p1 - 3f * p2 + p3) * t3
                    );

                // y 也使用平滑曲线，再做单调夹紧；避免垂直时因为线性 y 插值导致切线断裂，看起来像折线。
                cr.y = ClampMonotonic(cr.y, p0.y, p1.y, p2.y, p3.y);

                _rPoints[idx++] = cr;
            }
        }

        _rPoints[idx++] = _pCurr[last];

        _lineRenderer.positionCount = idx;
        _lineRenderer.SetPositions(_rPoints);
    }

    private static float ClampMonotonic(float value, float p0, float p1, float p2, float p3)
    {
        bool rising = p0 <= p1 && p1 <= p2 && p2 <= p3;
        bool falling = p0 >= p1 && p1 >= p2 && p2 >= p3;
        if (!rising && !falling)
            return value;

        float min = Mathf.Min(p1, p2);
        float max = Mathf.Max(p1, p2);
        return Mathf.Clamp(value, min, max);
    }

    private int PickRenderSubdivisions_Fast()
    {
        int idle = Mathf.Max(1, renderSubdivisionsIdle);
        int moving = Mathf.Max(1, renderSubdivisionsMoving);

        float thr = movingSpeedThreshold;
        float thrSq = (thr * _dt) * (thr * _dt);

        float sumSq = 0f;
        int count = Mathf.Max(1, _physicsNodes - 2);

        for (int i = 1; i < _physicsNodes - 1; i++)
        {
            Vector3 disp = _pCurr[i] - _pPrev[i];
            sumSq += disp.sqrMagnitude;
        }

        float avgSq = sumSq / count;

        return (avgSq > thrSq) ? moving : idle;
    }

    private static void BuildTCaches(int subdiv, ref TCaches caches)
    {
        subdiv = Mathf.Max(1, subdiv);
        caches.t = new float[subdiv];
        caches.t2 = new float[subdiv];
        caches.t3 = new float[subdiv];

        float inv = 1f / subdiv;
        for (int s = 0; s < subdiv; s++)
        {
            float t = s * inv;
            float t2 = t * t;
            caches.t[s] = t;
            caches.t2[s] = t2;
            caches.t3[s] = t2 * t;
        }
    }

    private void OnDrawGizmosSelected()
    {
        if (_pCurr == null) return;
        Gizmos.color = Color.yellow;
        for (int i = 0; i < _physicsNodes; i++)
            Gizmos.DrawSphere(_pCurr[i], 0.01f);
    }
}