solver.js

/**
 * Fully Patched Multi-Pulley and String Constraint Solver
 * Fixes vector inversion drift, clamped force runaway, and zero-acc log reports.

function solveStringConstraints(strings) {
  if (!Array.isArray(strings)) return;

  const DT = 1 / 60;
  const STIFFNESS = 0.3;
  const MAX_CORRECTION = 2.0;
  const SPRING_K = 1500;
  const MAX_TENSION = 500;
  const SLACK_THRESHOLD = 0.0001;

  for (const str of strings) {
    if (!str?.nodeA || !str?.nodeB) continue;

    const posA = getNodePos(str.nodeA);
    const posB = getNodePos(str.nodeB);
    if (!posA || !posB) continue;

    const bodyA = str.nodeA.body || str.nodeA;
    const bodyB = str.nodeB.body || str.nodeB;

    // --- 1. STATE INITIALIZATION ---
    if (!bodyA.vel) bodyA.vel = { x: 0, y: 0 };
    if (!bodyB.vel) bodyB.vel = { x: 0, y: 0 };
    if (!bodyA._prevVel) bodyA._prevVel = { x: bodyA.vel.x, y: bodyA.vel.y };
    if (!bodyB._prevVel) bodyB._prevVel = { x: bodyB.vel.x, y: bodyB.vel.y };
    if (!bodyA.mass) bodyA.mass = 1;
    if (!bodyB.mass) bodyB.mass = 1;

    if (bodyA.isDragging) { bodyA.vel.x = 0; bodyA.vel.y = 0; }
    if (bodyB.isDragging) { bodyB.vel.x = 0; bodyB.vel.y = 0; }

    // --- 2. CHAIN GEOMETRY ---
    let pulleys = [];
    if (Array.isArray(str.pulleys)) {
      pulleys = str.pulleys.filter(p => p && (p.pos || typeof p.x === 'number'));
    } else if (str.pulley) {
      pulleys = [str.pulley];
    }

    const chain = [posA];
    for (let i = 0; i < pulleys.length; i++) {
      const pPos = getNodePos(pulleys[i]);
      if (!pPos) continue;

      const r = pulleys[i].radius || 0.25;
      const ref = chain[chain.length - 1];
      const dx = pPos.x - ref.x;
      const dy = pPos.y - ref.y;
      const dist = Math.hypot(dx, dy) || 0.0001;

      chain.push({
        x: pPos.x - (dx / dist) * r,
        y: pPos.y - (dy / dist) * r,
        isPulleyRim: true
      });
    }
    chain.push(posB);

    // Calculate segments and total string length
    let currentTotalLength = 0;
    const segments = [];
    for (let i = 0; i < chain.length - 1; i++) {
      const p1 = chain[i];
      const p2 = chain[i + 1];
      const dx = p2.x - p1.x;
      const dy = p2.y - p1.y;
      const len = Math.hypot(dx, dy) || 0.0001;

      segments.push({ p1, p2, nx: dx / len, ny: dy / len });
      currentTotalLength += len;
    }

    // Dynamic UI / Resting length syncing
    if (!str.length || isNaN(str.length) || str.length <= 0) {
      str.length = Number(currentTotalLength.toFixed(3));
      str._uiLengthLast = str.length;
    } else if (str._uiLengthLast !== Number(str.length)) {
      str.length = Number(str.length);
      str._uiLengthLast = str.length;
    }

    const error = currentTotalLength - str.length;
    const firstSeg = segments[0];
    const lastSeg = segments[segments.length - 1];

    // --- 3. METRICS UPDATE ---
    const accA = {
      x: (bodyA.vel.x - bodyA._prevVel.x) / DT,
      y: (bodyA.vel.y - bodyA._prevVel.y) / DT
    };
    const accB = {
      x: (bodyB.vel.x - bodyB._prevVel.x) / DT,
      y: (bodyB.vel.y - bodyB._prevVel.y) / DT
    };

    const momA = { x: bodyA.mass * bodyA.vel.x, y: bodyA.mass * bodyA.vel.y };
    const momB = { x: bodyB.mass * bodyB.vel.x, y: bodyB.mass * bodyB.vel.y };

    // Force vectors point inward along the rope path when taut
    // A taut string can carry tension even when it is not stretched. Estimate
    // the required tension from each endpoint's force balance along the rope.
    const externalAlongA = bodyA.force ? bodyA.force.x * firstSeg.nx + bodyA.force.y * firstSeg.ny : 0;
    const externalAlongB = bodyB.force ? bodyB.force.x * lastSeg.nx + bodyB.force.y * lastSeg.ny : 0;
    const stretchTension = error > 0 ? error * SPRING_K : 0;
    const endpointIsFixed = (body) => body.isAnchored || body.isStatic || body.pin || body.isDragging;
    const isTaut = error > SLACK_THRESHOLD;
    const balanceTensionA = isTaut && !endpointIsFixed(bodyA) ? Math.max(0, -externalAlongA) : 0;
    const balanceTensionB = isTaut && !endpointIsFixed(bodyB) ? Math.max(0, externalAlongB) : 0;
    const tensionScalar = Math.min(Math.max(stretchTension, balanceTensionA, balanceTensionB), MAX_TENSION);
    const forceA = { x: firstSeg.nx * tensionScalar, y: firstSeg.ny * tensionScalar };
    const forceB = { x: -lastSeg.nx * tensionScalar, y: -lastSeg.ny * tensionScalar };

    bodyA._stringTensionForce = bodyA._stringTensionForce || { x: 0, y: 0 };
    bodyB._stringTensionForce = bodyB._stringTensionForce || { x: 0, y: 0 };
    bodyA._stringTensionForce.x += forceA.x;
    bodyA._stringTensionForce.y += forceA.y;
    bodyB._stringTensionForce.x += forceB.x;
    bodyB._stringTensionForce.y += forceB.y;

    bodyA.physicsMetrics = { position: { ...bodyA.pos }, velocity: { ...bodyA.vel }, acceleration: accA, momentum: momA, tensionForce: forceA };
    bodyB.physicsMetrics = { position: { ...bodyB.pos }, velocity: { ...bodyB.vel }, acceleration: accB, momentum: momB, tensionForce: forceB };

    // --- 4. RATE-LIMITED LOGGING ---
    if (!str._logCounter) str._logCounter = 0;
    if (!str._lastLoggedPos) str._lastLoggedPos = { ...bodyB.pos };

    const moveDistance = Math.hypot(bodyB.pos.x - str._lastLoggedPos.x, bodyB.pos.y - str._lastLoggedPos.y);
    const isHeartbeat = (++str._logCounter % 60 === 0);

    if (moveDistance > 0.5 || isHeartbeat) {
      str._lastLoggedPos = { ...bodyB.pos };
      console.log(
        `%c[Physics Metrics Report]%c\n` +
        `Node A -> Pos:(${bodyA.pos.x.toFixed(2)}, ${bodyA.pos.y.toFixed(2)}) | Vel:(${bodyA.vel.x.toFixed(2)}, ${bodyA.vel.y.toFixed(2)}) | Acc:(${accA.x.toFixed(2)}, ${accA.y.toFixed(2)}) | Mom:(${momA.x.toFixed(2)}, ${momA.y.toFixed(2)}) | Force:(${forceA.x.toFixed(2)}, ${forceA.y.toFixed(2)})\n` +
        `Node B -> Pos:(${bodyB.pos.x.toFixed(2)}, ${bodyB.pos.y.toFixed(2)}) | Vel:(${bodyB.vel.x.toFixed(2)}, ${bodyB.vel.y.toFixed(2)}) | Acc:(${accB.x.toFixed(2)}, ${accB.y.toFixed(2)}) | Mom:(${momB.x.toFixed(2)}, ${momB.y.toFixed(2)}) | Force:(${forceB.x.toFixed(2)}, ${forceB.y.toFixed(2)})`,
        'color: #00ffcc; font-weight: bold;', 'color: inherit;'
      );
    }

    // Abort solver corrections if string is slack
    if (error <= SLACK_THRESHOLD) {
      bodyA._prevVel = { x: bodyA.vel.x, y: bodyA.vel.y };
      bodyB._prevVel = { x: bodyB.vel.x, y: bodyB.vel.y };
      continue;
    }

    // --- 5. CONSTRAINT RESOLUTION ---
    const isFixed = (b) => b.isAnchored || b.isStatic || b.pin || b.isDragging;
    const invM1 = isFixed(bodyA) ? 0 : 1 / bodyA.mass;
    const invM2 = isFixed(bodyB) ? 0 : 1 / bodyB.mass;
    const sumInvM = invM1 + invM2;

    if (sumInvM === 0) continue;

    // Positional Correction (Move Node A toward rope, Node B toward rope)
    const correctionMagnitude = Math.min(error * STIFFNESS, MAX_CORRECTION);
    const posImpulse = correctionMagnitude / sumInvM;

    if (invM1 > 0) {
      bodyA.pos.x += firstSeg.nx * posImpulse * invM1;
      bodyA.pos.y += firstSeg.ny * posImpulse * invM1;
    }
    if (invM2 > 0) {
      bodyB.pos.x -= lastSeg.nx * posImpulse * invM2;
      bodyB.pos.y -= lastSeg.ny * posImpulse * invM2;
    }

    // Velocity Impulse Damping
    const v1 = bodyA.vel.x * firstSeg.nx + bodyA.vel.y * firstSeg.ny;
    const v2 = bodyB.vel.x * lastSeg.nx + bodyB.vel.y * lastSeg.ny;
    const relVel = v1 - v2;

    if (relVel < 0) {
      const velImpulse = relVel / sumInvM;

      if (invM1 > 0) {
        bodyA.vel.x -= firstSeg.nx * velImpulse * invM1;
        bodyA.vel.y -= firstSeg.ny * velImpulse * invM1;
      }
      if (invM2 > 0) {
        bodyB.vel.x += lastSeg.nx * velImpulse * invM2;
        bodyB.vel.y += lastSeg.ny * velImpulse * invM2;
      }
    }

    // Record velocities for accurate acceleration calculation next frame
    bodyA._prevVel = { x: bodyA.vel.x, y: bodyA.vel.y };
    bodyB._prevVel = { x: bodyB.vel.x, y: bodyB.vel.y };
  }
}
 */
/**
 * Fully Patched Multi-Pulley and String Constraint Solver
 * Fixes: Infinite coasting, missing floor contact, micro-drift velocity chatter.
 */
function solveStringConstraints(strings, globalGroundY = null) {
  if (!Array.isArray(strings)) return;

  const DT = 1 / 60;
  const STIFFNESS = 0.15;      // Moderate constraint stiffness (only applied once now)
  const MAX_CORRECTION = 1.0;  // Reasonable correction limit
  const SPRING_K = 1500;
  const MAX_TENSION = 500;
  const SLACK_THRESHOLD = 0.0001;

  // --- RESTING & DAMPING CONSTANTS ---
  const DAMPING = 0.99;            // Natural velocity decay (air resistance)
  const SLEEP_THRESHOLD = 0.005;   // Velocities below this clamp to exact 0
  const RESTITUTION = 0.0;         // 0 = perfectly inelastic floor contact (no bounce)

  for (const str of strings) {
    if (!str?.nodeA || !str?.nodeB) continue;

    const posA = getNodePos(str.nodeA);
    const posB = getNodePos(str.nodeB);
    if (!posA || !posB) continue;

    const bodyA = str.nodeA.body || str.nodeA;
    const bodyB = str.nodeB.body || str.nodeB;

    // --- 1. STATE INITIALIZATION ---
    if (!bodyA.vel) bodyA.vel = { x: 0, y: 0 };
    if (!bodyB.vel) bodyB.vel = { x: 0, y: 0 };
    if (!bodyA._prevVel) bodyA._prevVel = { x: bodyA.vel.x, y: bodyA.vel.y };
    if (!bodyB._prevVel) bodyB._prevVel = { x: bodyB.vel.x, y: bodyB.vel.y };
    if (!bodyA.mass) bodyA.mass = 1;
    if (!bodyB.mass) bodyB.mass = 1;

    if (bodyA.isDragging) { bodyA.vel.x = 0; bodyA.vel.y = 0; }
    if (bodyB.isDragging) { bodyB.vel.x = 0; bodyB.vel.y = 0; }

    // --- 2. CHAIN GEOMETRY ---
    let pulleys = [];
    if (Array.isArray(str.pulleys)) {
      pulleys = str.pulleys.filter(p => p && (p.pos || typeof p.x === 'number'));
    } else if (str.pulley) {
      pulleys = [str.pulley];
    }

    const chain = [posA];
    for (let i = 0; i < pulleys.length; i++) {
      const pPos = getNodePos(pulleys[i]);
      if (!pPos) continue;

      const r = pulleys[i].radius || 0.25;
      const ref = chain[chain.length - 1];
      const dx = pPos.x - ref.x;
      const dy = pPos.y - ref.y;
      const dist = Math.hypot(dx, dy) || 0.0001;

      chain.push({
        x: pPos.x - (dx / dist) * r,
        y: pPos.y - (dy / dist) * r,
        isPulleyRim: true
      });
    }
    chain.push(posB);

    // Calculate segments and total string length
    let currentTotalLength = 0;
    const segments = [];
    for (let i = 0; i < chain.length - 1; i++) {
      const p1 = chain[i];
      const p2 = chain[i + 1];
      const dx = p2.x - p1.x;
      const dy = p2.y - p1.y;
      const len = Math.hypot(dx, dy) || 0.0001;

      segments.push({ p1, p2, nx: dx / len, ny: dy / len });
      currentTotalLength += len;
    }

    // Dynamic UI / Resting length syncing
    if (!str.length || isNaN(str.length) || str.length <= 0) {
      str.length = Number(currentTotalLength.toFixed(3));
      str._uiLengthLast = str.length;
    } else if (str._uiLengthLast !== Number(str.length)) {
      str.length = Number(str.length);
      str._uiLengthLast = str.length;
    }

    const error = currentTotalLength - str.length;
    const firstSeg = segments[0];
    const lastSeg = segments[segments.length - 1];

    // --- 3. METRICS UPDATE ---
    const accA = {
      x: (bodyA.vel.x - bodyA._prevVel.x) / DT,
      y: (bodyA.vel.y - bodyA._prevVel.y) / DT
    };
    const accB = {
      x: (bodyB.vel.x - bodyB._prevVel.x) / DT,
      y: (bodyB.vel.y - bodyB._prevVel.y) / DT
    };

    const momA = { x: bodyA.mass * bodyA.vel.x, y: bodyA.mass * bodyA.vel.y };
    const momB = { x: bodyB.mass * bodyB.vel.x, y: bodyB.mass * bodyB.vel.y };

    // A taut string can carry tension even when it is not stretched. Estimate
    // the required tension from each endpoint's force balance along the rope.
    const externalAlongA = bodyA.force ? bodyA.force.x * firstSeg.nx + bodyA.force.y * firstSeg.ny : 0;
    const externalAlongB = bodyB.force ? bodyB.force.x * lastSeg.nx + bodyB.force.y * lastSeg.ny : 0;
    const stretchTension = error > 0 ? error * SPRING_K : 0;
    const endpointIsFixed = (body) => body.isAnchored || body.isStatic || body.pin || body.isDragging;
    const isTaut = error > SLACK_THRESHOLD;
    const balanceTensionA = isTaut && !endpointIsFixed(bodyA) ? Math.max(0, -externalAlongA) : 0;
    const balanceTensionB = isTaut && !endpointIsFixed(bodyB) ? Math.max(0, externalAlongB) : 0;
    const tensionScalar = Math.min(Math.max(stretchTension, balanceTensionA, balanceTensionB), MAX_TENSION);
    const forceA = { x: firstSeg.nx * tensionScalar, y: firstSeg.ny * tensionScalar };
    const forceB = { x: -lastSeg.nx * tensionScalar, y: -lastSeg.ny * tensionScalar };

    bodyA._stringTensionForce = bodyA._stringTensionForce || { x: 0, y: 0 };
    bodyB._stringTensionForce = bodyB._stringTensionForce || { x: 0, y: 0 };
    bodyA._stringTensionForce.x += forceA.x;
    bodyA._stringTensionForce.y += forceA.y;
    bodyB._stringTensionForce.x += forceB.x;
    bodyB._stringTensionForce.y += forceB.y;

    bodyA.physicsMetrics = { position: { ...bodyA.pos }, velocity: { ...bodyA.vel }, acceleration: accA, momentum: momA, tensionForce: forceA };
    bodyB.physicsMetrics = { position: { ...bodyB.pos }, velocity: { ...bodyB.vel }, acceleration: accB, momentum: momB, tensionForce: forceB };

    // --- 4. RATE-LIMITED LOGGING ---
    if (!str._logCounter) str._logCounter = 0;
    if (!str._lastLoggedPos) str._lastLoggedPos = { ...bodyB.pos };

    const moveDistance = Math.hypot(bodyB.pos.x - str._lastLoggedPos.x, bodyB.pos.y - str._lastLoggedPos.y);
    const isHeartbeat = (++str._logCounter % 60 === 0);

    if (moveDistance > 0.5 || isHeartbeat) {
      str._lastLoggedPos = { ...bodyB.pos };
      console.log(
        `%c[Physics Metrics Report]%c\n` +
        `Node A -> Pos:(${bodyA.pos.x.toFixed(2)}, ${bodyA.pos.y.toFixed(2)}) | Vel:(${bodyA.vel.x.toFixed(2)}, ${bodyA.vel.y.toFixed(2)}) | Acc:(${accA.x.toFixed(2)}, ${accA.y.toFixed(2)}) | Mom:(${momA.x.toFixed(2)}, ${momA.y.toFixed(2)}) | Force:(${forceA.x.toFixed(2)}, ${forceA.y.toFixed(2)})\n` +
        `Node B -> Pos:(${bodyB.pos.x.toFixed(2)}, ${bodyB.pos.y.toFixed(2)}) | Vel:(${bodyB.vel.x.toFixed(2)}, ${bodyB.vel.y.toFixed(2)}) | Acc:(${accB.x.toFixed(2)}, ${accB.y.toFixed(2)}) | Mom:(${momB.x.toFixed(2)}, ${momB.y.toFixed(2)}) | Force:(${forceB.x.toFixed(2)}, ${forceB.y.toFixed(2)})`,
        'color: #00ffcc; font-weight: bold;', 'color: inherit;'
      );
    }

    // --- 5. GROUND COLLISION RESPONSE ---
    const groundY = str.groundY ?? globalGroundY;
    if (groundY !== null && groundY !== undefined) {
      [bodyA, bodyB].forEach(body => {
        const radius = body.radius || 0.2;
        if (body.pos.y - radius <= groundY) {
          body.pos.y = groundY + radius;
          if (body.vel.y < 0) {
            body.vel.y *= -RESTITUTION; // Kill downward velocity
          }
          const bodyFriction = body.friction ?? 0.3;
          const surfaceFriction = 0.3;
          body.vel.x *= 1 - Math.min(1, (bodyFriction + surfaceFriction) / 2);
        }
      });
    }

    // Abort constraint step if string is slack
    if (error <= SLACK_THRESHOLD) {
      bodyA._prevVel = { x: bodyA.vel.x, y: bodyA.vel.y };
      bodyB._prevVel = { x: bodyB.vel.x, y: bodyB.vel.y };
      continue;
    }

    // --- 6. CONSTRAINT RESOLUTION ---
    const isFixed = (b) => b.isAnchored || b.isStatic || b.pin || b.isDragging;
    const invM1 = isFixed(bodyA) ? 0 : 1 / bodyA.mass;
    const invM2 = isFixed(bodyB) ? 0 : 1 / bodyB.mass;
    const sumInvM = invM1 + invM2;

    if (sumInvM === 0) continue;

    // Positional Correction (now the only string constraint applied)
    const correctionMagnitude = Math.min(error * STIFFNESS, MAX_CORRECTION);
    const posImpulse = correctionMagnitude / sumInvM;

    if (invM1 > 0) {
      bodyA.pos.x += firstSeg.nx * posImpulse * invM1;
      bodyA.pos.y += firstSeg.ny * posImpulse * invM1;
    }
    if (invM2 > 0) {
      bodyB.pos.x -= lastSeg.nx * posImpulse * invM2;
      bodyB.pos.y -= lastSeg.ny * posImpulse * invM2;
    }

    // Velocity Impulse (now the only velocity constraint applied)
    const v1 = bodyA.vel.x * firstSeg.nx + bodyA.vel.y * firstSeg.ny;
    const v2 = bodyB.vel.x * lastSeg.nx + bodyB.vel.y * lastSeg.ny;
    const relVel = v1 - v2;

    if (relVel < 0) {
      const velImpulse = relVel / sumInvM;

      if (invM1 > 0) {
        bodyA.vel.x -= firstSeg.nx * velImpulse * invM1;
        bodyA.vel.y -= firstSeg.ny * velImpulse * invM1;
      }
      if (invM2 > 0) {
        bodyB.vel.x += lastSeg.nx * velImpulse * invM2;
        bodyB.vel.y += lastSeg.ny * velImpulse * invM2;
      }
    }

    // --- 7. ENERGY DAMPING & SLEEPING THRESHOLD ---
    // Decays velocity over time to bleed system energy
    bodyA.vel.x *= DAMPING;
    bodyA.vel.y *= DAMPING;
    bodyB.vel.x *= DAMPING;
    bodyB.vel.y *= DAMPING;

    // Forces exact rest when velocity drops below resting threshold
    if (Math.hypot(bodyA.vel.x, bodyA.vel.y) < SLEEP_THRESHOLD) {
      bodyA.vel.x = 0;
      bodyA.vel.y = 0;
    }
    if (Math.hypot(bodyB.vel.x, bodyB.vel.y) < SLEEP_THRESHOLD) {
      bodyB.vel.x = 0;
      bodyB.vel.y = 0;
    }

    // Record velocities for accurate acceleration calculation next frame
    bodyA._prevVel = { x: bodyA.vel.x, y: bodyA.vel.y };
    bodyB._prevVel = { x: bodyB.vel.x, y: bodyB.vel.y };
  }
}