import { describe, expect, it } from "vitest";
import {
  clamp,
  degToRad,
  flat16,
  matrixToRpyDeg,
  nest16,
  radToDeg,
  rpyToMatrixDeg,
  worldHeadYawDeg,
} from "../../src/util/math";
// Cross-validation oracle: the SDK's own math utilities.
import { matrixToRpy as sdkMatrixToRpy, rpyToMatrix as sdkRpyToMatrix } from "@pollen-robotics/reachy-mini-sdk";

describe("basic conversions", () => {
  it("deg/rad round-trip", () => {
    expect(degToRad(180)).toBeCloseTo(Math.PI, 10);
    expect(radToDeg(Math.PI / 2)).toBeCloseTo(90, 10);
    expect(radToDeg(degToRad(33.3))).toBeCloseTo(33.3, 10);
  });

  it("clamp", () => {
    expect(clamp(5, -1, 1)).toBe(1);
    expect(clamp(-5, -1, 1)).toBe(-1);
    expect(clamp(0.5, -1, 1)).toBe(0.5);
  });

  it("flat16/nest16 round-trip", () => {
    const flat = Array.from({ length: 16 }, (_, i) => i);
    const nested = nest16(flat);
    expect(nested).toHaveLength(4);
    expect(nested[2]).toEqual([8, 9, 10, 11]);
    expect(flat16(nested)).toEqual(flat);
    expect(flat16(flat)).toEqual(flat);
  });

  it("worldHeadYawDeg composes body + local", () => {
    expect(worldHeadYawDeg(45, -25)).toBe(20);
  });
});

describe("rpy ↔ matrix", () => {
  it("identity at zero", () => {
    const m = rpyToMatrixDeg(0, 0, 0);
    expect(m[0]).toBeCloseTo(1);
    expect(m[5]).toBeCloseTo(1);
    expect(m[10]).toBeCloseTo(1);
    expect(m[15]).toBe(1);
  });

  it("round-trips across the safe envelope", () => {
    for (const [r, p, y] of [
      [0, 0, 0],
      [10, -20, 30],
      [-39, 39, -170],
      [5.5, -12.25, 61],
    ] as const) {
      const back = matrixToRpyDeg(rpyToMatrixDeg(r, p, y));
      expect(back.roll).toBeCloseTo(r, 6);
      expect(back.pitch).toBeCloseTo(p, 6);
      expect(back.yaw).toBeCloseTo(y, 6);
    }
  });

  it("matches the SDK's rotation convention", () => {
    for (const [r, p, y] of [
      [10, 20, 30],
      [-25, 15, -60],
      [0, -40, 120],
    ] as const) {
      const ours = rpyToMatrixDeg(r, p, y);
      const theirs = flat16(sdkRpyToMatrix(r, p, y) as unknown as number[][] | number[]);
      theirs.forEach((v, i) => expect(ours[i]).toBeCloseTo(v, 8));

      const back = sdkMatrixToRpy(nest16(ours) as never) as { roll: number; pitch: number; yaw: number };
      expect(back.roll).toBeCloseTo(r, 6);
      expect(back.pitch).toBeCloseTo(p, 6);
      expect(back.yaw).toBeCloseTo(y, 6);
    }
  });
});