using NBF;
using UnityEngine;

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

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

    [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("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] private bool constrainToWater = false;
    [SerializeField] private float waterHeight = 0f;
    [SerializeField, Min(0f)] private float waterRadius = 0.01f;

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

    [SerializeField, Tooltip("是否使用 Catmull-Rom 平滑（推荐开启）")]
    private bool smooth = true;

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

    private LineRenderer lr;

    // physics
    private int physicsNodes;
    private Vector3[] pCurr;
    private Vector3[] pPrev;

    // render
    private Vector3[] rPoints;
    private int rCountCached = -1;

    private Vector3 gravity;

    // length control runtime
    private float targetLength;
    private float currentLength;
    private float lengthSmoothVel;

    // Only-head-change trick:
    // Total rest length = headRestLen + (physicsNodes - 2) * physicsSegmentLen
    private float headRestLen;


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

        InitLengthSystem();
        AllocateAndInitNodes();
        RebuildRenderBufferIfNeeded();
    }
    
    private FRod _rod;

    public void Init(FRod rod)
    {
        _rod = rod;
    }

    private void OnValidate()
    {
        renderSubdivisions = Mathf.Max(renderSubdivisions, 1);
        iterations = Mathf.Clamp(iterations, 1, 80);
        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);
    }

    private void InitLengthSystem()
    {
        // 没有 min/max 长度限制：初始长度只做一个“非负”保障
        float defaultLen = physicsSegmentLen * (Mathf.Max(minPhysicsNodes, 2) - 1);
        currentLength = (initialLength > 0f) ? initialLength : defaultLen;
        targetLength = currentLength;
    }

    private void AllocateAndInitNodes()
    {
        // 若锚点存在：最小长度就是两锚点直线距离 + minSlack（防抖）
        if (startAnchor && endAnchor)
        {
            float minFeasible = Vector3.Distance(startAnchor.position, endAnchor.position) + minSlack;
            currentLength = Mathf.Max(currentLength, minFeasible);
            targetLength = Mathf.Max(targetLength, minFeasible);
        }

        physicsNodes = Mathf.Clamp(ComputeDesiredNodes(currentLength), 2, maxPhysicsNodes);
        pCurr = new Vector3[physicsNodes];
        pPrev = new Vector3[physicsNodes];

        // 初始从起点往下排
        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)
    {
        // nodes = floor(length/segLen)+1
        int desired = Mathf.FloorToInt(Mathf.Max(0f, lengthMeters) / physicsSegmentLen) + 1;
        desired = Mathf.Clamp(desired, minPhysicsNodes, maxPhysicsNodes);
        return desired;
    }

    /// <summary>设置目标总长度（米）。不做最小/最大长度限制（最小可行由锚点距离决定）。</summary>
    public void SetTargetLength(float lengthMeters)
    {
        targetLength = Mathf.Max(0f, lengthMeters);
    }

    /// <summary>增加/减少目标总长度（米）。正数放线，负数收线。</summary>
    public void AddTargetLength(float deltaMeters)
    {
        SetTargetLength(targetLength + deltaMeters);
    }

    public float GetCurrentLength() => currentLength;
    public float GetTargetLength() => targetLength;

    public float GetAnchorDistance()
    {
        if (startAnchor != null && endAnchor != null)
        {
            return Vector3.Distance(startAnchor.position, endAnchor.position);
        }

        return 0;
    }

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

        gravity.y = -gravityStrength;

        UpdateLengthSmooth(); // 只保证 >= 锚点直线距离 + minSlack
        UpdateNodesFromLength(); // 只从头部增/减节点
        UpdateHeadRestLenFromCurrentLength(); // 第一段补余量 => 变化集中在头部

        Simulate();

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

        if (constrainToWater || constrainToGround)
            ConstrainToGroundAndWater();

        LockAnchorsHard();
    }

    private void LateUpdate()
    {
        DrawHighResLine();
        FixedUpdate2();
    }

    private void UpdateLengthSmooth()
    {
        float anchorDist = Vector3.Distance(startAnchor.position, endAnchor.position);
        float minFeasible = anchorDist + minSlack;

        // ✅ 最小长度 = 起点终点直线距离(+slack)，最大不限制
        float desired = Mathf.Max(targetLength, minFeasible);

        float prevLen = currentLength;

        currentLength = Mathf.SmoothDamp(
            currentLength,
            desired,
            ref lengthSmoothVel,
            lengthSmoothTime,
            Mathf.Infinity,
            Time.fixedDeltaTime
        );

        float lenDelta = Mathf.Abs(currentLength - prevLen);
        if (lenDelta > 1e-5f && lengthChangeVelocityKill > 0f && pPrev != null)
        {
            float kill = Mathf.Clamp01(lengthChangeVelocityKill);
            for (int i = 1; i < physicsNodes - 1; i++)
                pPrev[i] = Vector3.Lerp(pPrev[i], pCurr[i], kill);
        }
    }

    private void UpdateNodesFromLength()
    {
        int desired = ComputeDesiredNodes(currentLength);
        if (desired == physicsNodes) return;

        while (physicsNodes < desired)
            AddNodeAtStart();

        while (physicsNodes > desired)
            RemoveNodeAtStart();

        RebuildRenderBufferIfNeeded();
    }

    private void AddNodeAtStart()
    {
        int newCount = Mathf.Min(physicsNodes + 1, maxPhysicsNodes);
        if (newCount == physicsNodes) return;

        Vector3[] newCurr = new Vector3[newCount];
        Vector3[] newPrev = new Vector3[newCount];

        newCurr[0] = pCurr[0];
        newPrev[0] = pPrev[0];

        for (int i = 2; i < newCount; i++)
        {
            newCurr[i] = pCurr[i - 1];
            newPrev[i] = pPrev[i - 1];
        }

        Vector3 s = startAnchor.position;
        Vector3 dir = Vector3.down;

        if (physicsNodes >= 2)
        {
            Vector3 toOld1 = (pCurr[1] - s);
            if (toOld1.sqrMagnitude > 1e-6f) dir = toOld1.normalized;
        }

        Vector3 pos = s + dir * physicsSegmentLen;
        newCurr[1] = pos;
        newPrev[1] = pos;

        pCurr = newCurr;
        pPrev = newPrev;
        physicsNodes = newCount;

        LockAnchorsHard();
    }

    private void RemoveNodeAtStart()
    {
        int newCount = Mathf.Max(physicsNodes - 1, 2);
        if (newCount == physicsNodes) return;

        Vector3[] newCurr = new Vector3[newCount];
        Vector3[] newPrev = new Vector3[newCount];

        newCurr[0] = pCurr[0];
        newPrev[0] = pPrev[0];

        for (int i = 1; i < newCount - 1; i++)
        {
            newCurr[i] = pCurr[i + 1];
            newPrev[i] = pPrev[i + 1];
        }

        newCurr[newCount - 1] = pCurr[physicsNodes - 1];
        newPrev[newCount - 1] = pPrev[physicsNodes - 1];

        pCurr = newCurr;
        pPrev = newPrev;
        physicsNodes = newCount;

        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()
    {
        float dt = Time.fixedDeltaTime;

        for (int i = 0; i < physicsNodes; i++)
        {
            Vector3 v = (pCurr[i] - pPrev[i]) * velocityDampen;
            pPrev[i] = pCurr[i];

            pCurr[i] += v;
            pCurr[i] += gravity * dt;
        }

        LockAnchorsHard();
    }

    private void LockAnchorsHard()
    {
        if (!startAnchor || !endAnchor || pCurr == null || pPrev == null || physicsNodes < 2) return;
        float dt = Time.fixedDeltaTime;
        Vector3 s = startAnchor.position;
        Vector3 e = 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()
    {
        for (int i = 0; i < physicsNodes - 1; i++)
        {
            float rest = (i == 0) ? headRestLen : physicsSegmentLen;

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

            Vector3 delta = b - a;
            float dist = delta.magnitude;
            if (dist < 1e-6f) continue;

            float diff = (dist - rest) / dist;
            Vector3 corr = delta * diff * stiffness;

            if (i != 0)
                pCurr[i] += corr * 0.5f;

            if (i + 1 != physicsNodes - 1)
                pCurr[i + 1] -= corr * 0.5f;
        }
    }

    private void ConstrainToGroundAndWater()
    {
        for (int i = 1; i < physicsNodes - 1; i++)
        {
            Vector3 p = pCurr[i];

            if (constrainToWater)
            {
                float minY = waterHeight + waterRadius;
                if (p.y < minY) p.y = minY;
            }

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

            pCurr[i] = p;
        }
    }

    private void DrawHighResLine()
    {
        if (pCurr == null || physicsNodes < 2) return;

        RebuildRenderBufferIfNeeded();

        lr.startWidth = lineWidth;
        lr.endWidth = lineWidth;

        if (!smooth)
        {
            lr.positionCount = physicsNodes;
            lr.SetPositions(pCurr);
            return;
        }

        int idx = 0;

        for (int seg = 0; seg < physicsNodes - 1; seg++)
        {
            Vector3 p0 = pCurr[Mathf.Max(seg - 1, 0)];
            Vector3 p1 = pCurr[seg];
            Vector3 p2 = pCurr[seg + 1];
            Vector3 p3 = pCurr[Mathf.Min(seg + 2, physicsNodes - 1)];

            for (int s = 0; s < renderSubdivisions; s++)
            {
                float t = s / (float)renderSubdivisions;
                Vector3 pt = CatmullRom_XZ_LinearY(p0, p1, p2, p3, t);

                // 如果水面约束开启：渲染点也夹一下，避免视觉上又穿回去
                if (constrainToWater)
                {
                    float minY = waterHeight + waterRadius;
                    if (pt.y < minY) pt.y = minY;
                }

                rPoints[idx++] = pt;
            }
        }

        rPoints[idx++] = pCurr[physicsNodes - 1];

        lr.positionCount = idx;
        lr.SetPositions(rPoints);
    }

    private void RebuildRenderBufferIfNeeded()
    {
        int targetCount = (physicsNodes - 1) * renderSubdivisions + 1;
        if (rPoints == null || rCountCached != targetCount)
        {
            rPoints = new Vector3[targetCount];
            rCountCached = targetCount;
        }
    }

    private static Vector3 CatmullRom(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t)
    {
        float t2 = t * t;
        float t3 = t2 * t;

        return 0.5f * (
            (2f * p1) +
            (-p0 + p2) * t +
            (2f * p0 - 5f * p1 + 4f * p2 - p3) * t2 +
            (-p0 + 3f * p1 - 3f * p2 + p3) * t3
        );
    }

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

    private static Vector3 CatmullRom_XZ_LinearY(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t)
    {
        // XZ 做 Catmull-Rom
        Vector3 cr = CatmullRom(p0, p1, p2, p3, t);

        // Y 不做样条，改成线性（不会过冲）
        cr.y = Mathf.Lerp(p1.y, p2.y, t);

        return cr;
    }
}