import { describe, expect, it } from 'vitest';
import { hingeAngleGrad } from '../src/sim/hinge';

// random bent configuration around an edge along x
function config(seed: number): Float64Array {
  let s = seed;
  const r = () => ((s = (s * 16807) % 2147483647) / 2147483647 - 0.5) * 0.3;
  // w0, w1, e0, e1
  return Float64Array.from([0.5 + r(), 0.8 + r(), r(), 0.4 + r(), -0.7 + r(), 0.3 + r(), r(), r(), r(), 1 + r(), r(), r()]);
}

describe('hinge', () => {
  it('angle is zero when flat and matches a fold of known angle', () => {
    const flat = Float64Array.from([0.5, 0, 1, 0.5, 0, -1, 0, 0, 0, 1, 0, 0]);
    expect(hingeAngleGrad(flat, 0, 1, 2, 3)).toBeCloseTo(0, 12);
    const a = 0.3; // fold w1 down by angle a about the x axis
    const fold = Float64Array.from([0.5, 0, 1, 0.5, -Math.sin(a), -Math.cos(a), 0, 0, 0, 1, 0, 0]);
    expect(Math.abs(hingeAngleGrad(fold, 0, 1, 2, 3))).toBeCloseTo(a, 12);
  });
  it('gradient matches finite differences', () => {
    for (let seed = 1; seed <= 5; seed++) {
      const X = config(seed * 7919);
      const out = new Float64Array(13);
      hingeAngleGrad(X, 0, 1, 2, 3, out);
      const order = [0, 1, 2, 3]; // node k's gradient is at out[1 + 3k]
      const h = 1e-6;
      for (const node of order)
        for (let c = 0; c < 3; c++) {
          const Xp = X.slice();
          const Xm = X.slice();
          Xp[3 * node + c] += h;
          Xm[3 * node + c] -= h;
          const fd = (hingeAngleGrad(Xp, 0, 1, 2, 3) - hingeAngleGrad(Xm, 0, 1, 2, 3)) / (2 * h);
          expect(out[1 + 3 * node + c]).toBeCloseTo(fd, 6);
        }
    }
  });
});