src/util/math.ts

// Pure math helpers. Kept SDK-free so every layer except robot/real.ts
// (the single SDK-touching file) can use them in any test environment.
// Conventions match the SDK/daemon wire format: rotation is intrinsic
// ZYX (yaw about Z, then pitch about Y, then roll about X), matrices are
// row-major 4×4 homogeneous, angles on the wire are radians. A unit test
// cross-validates rpyToMatrixDeg/matrixToRpyDeg against the SDK's own
// rpyToMatrix/matrixToRpy.

export function degToRad(deg: number): number {
  return (deg * Math.PI) / 180;
}

export function radToDeg(rad: number): number {
  return (rad * 180) / Math.PI;
}

export function clamp(v: number, lo: number, hi: number): number {
  return Math.min(hi, Math.max(lo, v));
}

export type Flat16 = number[];

/** Accepts a nested 4×4 (recorded-move JSON) or an already-flat array. */
export function flat16(m: number[][] | number[]): Flat16 {
  if (typeof m[0] === "number") return m as number[];
  return (m as number[][]).flat();
}

export function nest16(m: number[][] | number[]): number[][] {
  if (typeof m[0] !== "number") return m as number[][];
  const f = m as number[];
  return [f.slice(0, 4), f.slice(4, 8), f.slice(8, 12), f.slice(12, 16)];
}

/** Degrees → flat row-major 4×4 rotation matrix (ZYX intrinsic). */
export function rpyToMatrixDeg(rollDeg: number, pitchDeg: number, yawDeg: number): Flat16 {
  const r = degToRad(rollDeg);
  const p = degToRad(pitchDeg);
  const y = degToRad(yawDeg);
  const cr = Math.cos(r), sr = Math.sin(r);
  const cp = Math.cos(p), sp = Math.sin(p);
  const cy = Math.cos(y), sy = Math.sin(y);
  return [
    cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, 0,
    sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, 0,
    -sp,     cp * sr,               cp * cr,                0,
    0,       0,                     0,                      1,
  ];
}

export interface RpyDeg {
  roll: number;
  pitch: number;
  yaw: number;
}

/** Flat or nested 4×4 rotation matrix → degrees (ZYX intrinsic). */
export function matrixToRpyDeg(m: number[][] | number[]): RpyDeg {
  const f = flat16(m);
  const m00 = f[0]!, m10 = f[4]!, m20 = f[8]!, m21 = f[9]!, m22 = f[10]!;
  return {
    roll: radToDeg(Math.atan2(m21, m22)),
    pitch: radToDeg(-Math.asin(clamp(m20, -1, 1))),
    yaw: radToDeg(Math.atan2(m10, m00)),
  };
}

/**
 * The daemon's head matrix is world-frame: to keep the head pointing
 * "straight ahead relative to the body" while the body turns, the head
 * matrix must carry body yaw + local head yaw.
 */
export function worldHeadYawDeg(bodyYawDeg: number, headYawLocalDeg: number): number {
  return bodyYawDeg + headYawLocalDeg;
}