zach/dotter

// Numeric helpers that mirror Python semantics exactly, plus a port of

// plotink's ebb_calc (EBB firmware motion math) and the cubic solver used by

// the S-curve planner. Exactness matters here: the planner feeds these values

// straight into T3/TD commands, so a rounding difference is a different move.

export const EBB_ACCUMULATOR_MAX = 2147483648; // 2^31

const ACC_MAX_BIG = 2147483648n;

/** Python's round(): round half to even. */

export function pyRound(x) {

  const f = Math.floor(x);

  const diff = x - f;

  if (diff > 0.5) return f + 1;

  if (diff < 0.5) return f;

  return f % 2 === 0 ? f : f + 1;

}

/** Python's int() on a float: truncate toward zero. */

export const pyInt = Math.trunc;

/** Python's math.isclose(a, b, rel_tol=1e-9, abs_tol=absTol). */

export function isClose(a, b, absTol = 0) {

  if (a === b) return true;

  const diff = Math.abs(a - b);

  return diff <= Math.max(1e-9 * Math.max(Math.abs(a), Math.abs(b)), absTol);

}

export function distance(x, y) {

  return Math.sqrt(x * x + y * y);

}

/** plot_utils.dotProductXY: dot product clamped to [-1, 1]. */

export function dotProductXY(a, b) {

  const t = a[0] * b[0] + a[1] * b[1];

  if (t > 1) return 1;

  if (t < -1) return -1;

  return t;

}

export function constrainLimits(value, lo, hi) {

  return Math.max(lo, Math.min(hi, value));

}

/** plot_utils.checkLimitsTol: clamp, and report if beyond tolerance. */

export function checkLimitsTol(value, lo, hi, tol) {

  if (value > hi) return [hi, value > hi + tol];

  if (value < lo) return [lo, value < lo - tol];

  return [value, false];

}

function bigRoundHalfUpDiv6(n) {

  // Decimal ROUND_HALF_UP of n/6: ties go away from zero.

  return n >= 0n ? (n + 3n) / 6n : -((-n + 3n) / 6n);

}

/**

 * Final step count and accumulator after a T3 move on one axis

 * (ebb_calc.move_dist_t3). `accum` may be the string "clear".

 * Returns [steps, accumulator].

 */

export function moveDistT3(time, rate, accel, jerk, accum = 'clear') {

  time = pyInt(time);

  rate = pyInt(rate);

  accel = pyInt(accel);

  jerk = pyInt(jerk);

  if (time === 0) return [0, 0];

  const halfAccel = pyInt(accel / 2);

  const jerkOverSix = pyInt(jerk / 6);

  if (accum === 'clear') {

    accum = 0;

    let tempRate = rate - halfAccel + jerkOverSix + accel;

    if (tempRate < 0) {

      accum = 2147483647;

    } else if (tempRate === 0) {

      tempRate = accel + jerk;

      if (tempRate < 0) accum = 2147483647;

      else if (tempRate === 0 && jerk < 0) accum = 2147483647;

    }

  } else {

    accum = pyInt(accum);

  }

  // 6 * (accum + T*(r_eff + T*(a/2 + T*j/6))), where

  // 6 * r_eff = 6r + 3a - 6*int(a/2) + 6*int(j/6) - j. All exact integers.

  const T = BigInt(time);

  const r6 = 6n * BigInt(rate) + 3n * BigInt(accel) - 6n * BigInt(halfAccel) +

    6n * BigInt(jerkOverSix) - BigInt(jerk);

  const n6 = 6n * BigInt(accum) + T * (r6 + T * (3n * BigInt(accel) + T * BigInt(jerk)));

  const rounded = bigRoundHalfUpDiv6(n6);

  const pos = Math.floor(Number(rounded) / EBB_ACCUMULATOR_MAX);

  const remainder = rounded - ACC_MAX_BIG * BigInt(pos);

  return [pos, Number(remainder)];

}

/** Rate after a T3 move on one axis (ebb_calc.rate_t3). */

export function rateT3(time, rate, accel, jerk) {

  time = pyInt(time);

  if (time === 0) return rate + accel + jerk;

  const base = pyInt(rate) - pyInt(accel / 2) + pyInt(jerk / 6);

  return pyRound(base + (pyInt(accel) - jerk / 2) * time + jerk * time * time / 2);

}

/**

 * Exact peak |rate| over ticks 1..time of a T3 move on one axis. The rate

 * at tick t is r0 + accel*t + jerk*(t^2 - t)/2, with r0 the firmware's

 * pre-adjusted rate; the peak is at an endpoint or next to the vertex.

 */

export function peakRateT3(time, rate, accel, jerk) {

  const r0 = rate - pyInt(accel / 2) + pyInt(jerk / 6);

  const at = (t) => Math.abs(r0 + accel * t + jerk * (t * t - t) / 2);

  let peak = Math.max(at(1), at(time));

  if (jerk !== 0) {

    const vertex = 0.5 - accel / jerk;

    for (const t of [Math.floor(vertex), Math.ceil(vertex)]) {

      if (t > 1 && t < time) peak = Math.max(peak, at(t));

    }

  }

  return peak;

}

/** Peak |rate| over a T3 or TD command's parameters, both axes. */

export function peakRateCommand(kind, p) {

  if (kind === 'T3') {

    const [t, r1, a1, j1, r2, a2, j2] = p;

    return Math.max(peakRateT3(t, r1, a1, j1), peakRateT3(t, r2, a2, j2));

  }

  const [t, r1a, r1b, a1, j1, r2a, r2b, a2, j2] = p;

  return Math.max(peakRateT3(t, r1a, 0, j1), peakRateT3(t, r1b, a1, -j1),

    peakRateT3(t, r2a, 0, j2), peakRateT3(t, r2b, a2, -j2));

}

const COMPLEX = Symbol('complex');

/**

 * Roots of a*x^3 + b*x^2 + c*x + d = 0 (port of cubic_eqn.solve).

 * Complex roots are returned as the COMPLEX marker so callers can skip them,

 * as the Python planner does with isinstance(item, complex).

 */

export function cubicSolve(a, b, c, d) {

  if (a === 0 && b === 0) return [(-d * 1.0) / c];

  if (a === 0) {

    const disc = c * c - 4.0 * b * d;

    if (disc >= 0) {

      const s = Math.sqrt(disc);

      return [(-c + s) / (2.0 * b), (-c - s) / (2.0 * b)];

    }

    return [COMPLEX, COMPLEX];

  }

  const f = ((3.0 * c / a) - ((b ** 2.0) / (a ** 2.0))) / 3.0;

  const g = (((2.0 * (b ** 3.0)) / (a ** 3.0)) - ((9.0 * b * c) / (a ** 2.0)) + (27.0 * d / a)) / 27.0;

  const h = (g ** 2.0) / 4.0 + (f ** 3.0) / 27.0;

  if (f === 0 && g === 0 && h === 0) {

    const x = (d / a) >= 0 ? -((d / (1.0 * a)) ** (1 / 3.0)) : (-d / (1.0 * a)) ** (1 / 3.0);

    return [x, x, x];

  }

  if (h <= 0) {

    const i = Math.sqrt(((g ** 2.0) / 4.0) - h);

    const j = i ** (1 / 3.0);

    const k = Math.acos(-(g / (2 * i)));

    const L = -j;

    const M = Math.cos(k / 3.0);

    const N = Math.sqrt(3) * Math.sin(k / 3.0);

    const P = -(b / (3.0 * a));

    return [2 * j * Math.cos(k / 3.0) - (b / (3.0 * a)), L * (M + N) + P, L * (M - N) + P];

  }

  const R = -(g / 2.0) + Math.sqrt(h);

  const S = R >= 0 ? R ** (1 / 3.0) : -((-R) ** (1 / 3.0));

  const T = -(g / 2.0) - Math.sqrt(h);

  const U = T >= 0 ? T ** (1 / 3.0) : -((-T) ** (1 / 3.0));

  // Python builds x2, x3 with a 0.5j factor, so they are always complex-typed.

  return [(S + U) - (b / (3.0 * a)), COMPLEX, COMPLEX];

}

export function isComplex(root) {

  return root === COMPLEX;

}