zach/dotter

import { describe, expect, test } from 'bun:test';

import { cubicSolve, isClose, isComplex, moveDistT3, peakRateT3, pyRound, rateT3 } from '../src/calc.js';

import { rng } from './helpers.js';

const ACC = 2147483648;

/** Independent tick-by-tick model of the firmware's T3 accumulator loop. */

function tickT3(time, rate, accel, jerk, accum) {

  let r = rate - Math.trunc(accel / 2) + Math.trunc(jerk / 6);

  let a = accel;

  let acc = BigInt(accum);

  for (let t = 0; t < time; t++) {

    r += a;

    a += jerk;

    acc += BigInt(r);

  }

  const big = BigInt(ACC);

  let pos = acc / big;

  if (acc % big < 0n) pos -= 1n;

  return { steps: Number(pos), accum: Number(acc - pos * big), rate: r };

}

describe('pyRound', () => {

  test('rounds half to even like Python', () => {

    expect(pyRound(0.5)).toBe(0);

    expect(pyRound(1.5)).toBe(2);

    expect(pyRound(2.5)).toBe(2);

    expect(pyRound(-0.5) === 0).toBe(true); // Python: round(-0.5) == 0

    expect(pyRound(-1.5)).toBe(-2);

    expect(pyRound(-2.5)).toBe(-2);

    expect(pyRound(2.4999999)).toBe(2);

    expect(pyRound(-2.6)).toBe(-3);

    expect(pyRound(1234567.5)).toBe(1234568);

  });

});

describe('isClose', () => {

  test('matches math.isclose defaults', () => {

    expect(isClose(1, 1 + 1e-10)).toBe(true);

    expect(isClose(1, 1 + 1e-8)).toBe(false);

    expect(isClose(0, 1e-12)).toBe(false); // rel_tol alone never matches zero

    expect(isClose(0, 1e-12, 1e-9)).toBe(true);

  });

});

describe('moveDistT3', () => {

  test('known values', () => {

    expect(moveDistT3(0, 5, 5, 5, 0)).toEqual([0, 0]);

    // One step per tick for 10 ticks.

    expect(moveDistT3(10, ACC, 0, 0, 0)).toEqual([10, 0]);

    // Half a step per tick for 3 ticks: 1 step, remainder half.

    expect(moveDistT3(3, ACC / 2, 0, 0, 0)).toEqual([1, ACC / 2]);

    // Negative rate from cleared accumulator starts at 2^31 - 1.

    expect(moveDistT3(1, -1, 0, 0)).toEqual([0, 2147483646]);

  });

  test('matches a tick-by-tick firmware model on random moves', () => {

    const rand = rng(1234);

    for (let i = 0; i < 400; i++) {

      const time = 1 + Math.floor(rand() * 3000);

      const jerk = Math.floor((rand() - 0.5) * 4000);

      const accel = Math.floor((rand() - 0.5) * 2e6);

      // Keep |rate| below 2^31 over the whole move, as the planner must.

      const rate = Math.floor((rand() - 0.5) * 8e8);

      const accum = Math.floor(rand() * ACC);

      const sim = tickT3(time, rate, accel, jerk, accum);

      if (Math.abs(sim.rate) >= ACC) continue;

      expect(moveDistT3(time, rate, accel, jerk, accum)).toEqual([sim.steps, sim.accum]);

      expect(rateT3(time, rate, accel, jerk)).toBe(sim.rate);

    }

  });

});

describe('peakRateT3', () => {

  test('equals the largest |rate| seen tick by tick', () => {

    const rand = rng(77);

    for (let i = 0; i < 300; i++) {

      const time = 1 + Math.floor(rand() * 2000);

      const jerk = Math.floor((rand() - 0.5) * 4000);

      const accel = Math.floor((rand() - 0.5) * 2e6);

      const rate = Math.floor((rand() - 0.5) * 8e8);

      let r = rate - Math.trunc(accel / 2) + Math.trunc(jerk / 6);

      let a = accel;

      let peak = 0;

      for (let t = 0; t < time; t++) {

        r += a;

        a += jerk;

        peak = Math.max(peak, Math.abs(r));

      }

      expect(peakRateT3(time, rate, accel, jerk)).toBe(peak);

    }

  });

});

describe('cubicSolve', () => {

  test('real root of the S-curve cubic j t^3 + 2 v t - d = 0', () => {

    const rand = rng(7);

    for (let i = 0; i < 200; i++) {

      const j = 100 + rand() * 50000;

      const v = rand() * 10;

      const d = 1e-4 + rand() * 5;

      const roots = cubicSolve(j, 0, 2 * v, -d).filter((r) => !isComplex(r));

      expect(roots.length).toBeGreaterThan(0);

      const t = roots[0];

      expect(Math.abs(j * t ** 3 + 2 * v * t - d)).toBeLessThan(1e-9 * Math.max(1, d));

    }

  });

  test('three real roots', () => {

    const roots = cubicSolve(1, -6, 11, -6).sort((a, b) => a - b); // (x-1)(x-2)(x-3)

    expect(roots[0]).toBeCloseTo(1, 9);

    expect(roots[1]).toBeCloseTo(2, 9);

    expect(roots[2]).toBeCloseTo(3, 9);

  });

});