// Phase 1: Der reale Roboter arbeitet in -Y (robot.json: Arm1 -> [0,-250,0]).
// alpha=0 muss nach -y zeigen, nicht nach +y. Siehe doc/Info_Koordinaten.md.
const Robot = require('../robot/kinematics/Arm3SegmentLinearX');
const GCode = require('../robot/GCode');
const D = 180 / Math.PI;

describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => {
  beforeAll(() => jest.spyOn(console, 'log').mockImplementation(() => {}));
  afterAll(() => jest.restoreAllMocks());

  const L1 = 250, L2 = 250, L3 = 90;

  function fkFromMotors(alphaDeg, betaDeg, aDeg, bDeg, cDeg, xMotor = 0) {
    const r = new Robot(L1, L2, L3);
    r.xMotor = xMotor;
    r.alpha = alphaDeg / D; r.beta = betaDeg / D;
    r.a = aDeg / D; r.b = bDeg / D; r.c = cDeg / D;
    r.calculatePositionFromMotorAngles();
    return r;
  }

  test('voll ausgestreckt (alpha=beta=0, Hand gerade) -> y ~ -590', () => {
    // B=0 = gerade Hand (Phase-2-Konvention)
    const r = fkFromMotors(0, 0, 0, 0, 0);
    expect(r.y).toBeLessThan(0);
    expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 0);   // ~ -590
    expect(r.z).toBeCloseTo(0, 6);
  });

  test('gemeldete Homing-Pose landet in -y (war faelschlich +405)', () => {
    // G92 X160.53 Y4.53 Z13.93 A124.04 B0 C0 (Homing: B=0 = gerade Hand, Phase-2-Konvention).
    // Phase 1: y=-405 (B=0 wurde als "gefaltet" interpretiert).
    // Phase 2: y=-579 (B=0 = gerade → Hand streckt sich in Armrichtung).
    const r = fkFromMotors(4.53, 13.93, 124.04, 0, 0, 160.53);
    expect(r.y).toBeLessThan(0);
    expect(r.y).toBeCloseTo(-579, 0);
    expect(r.z).toBeCloseTo(102, 0);
  });

  test('IK der -y-Grundstellung liefert alpha~0 / beta~0 (nicht ~180)', () => {
    const ref = fkFromMotors(0, 0, 0, 0, 0);      // -y-ausgestreckte Pose als Referenz (b=0 = gerade)
    const r = new Robot(L1, L2, L3);
    r.x = ref.x; r.y = ref.y; r.z = ref.z;
    r.phi = ref.phi; r.theta = ref.theta; r.psi = ref.psi; r.e = 0;
    r.calculateAngles3D();
    expect(r.alpha).toBeCloseTo(0, 3);
    expect(r.beta).toBeCloseTo(0, 3);
  });

  test('Round-Trip im -y-Arbeitsraum bleibt konsistent', () => {
    const A = new Robot(L1, L2, L3);
    A.x = 10; A.y = -430; A.z = 30;
    A.phi = Math.PI / 7; A.theta = Math.PI / 2; A.psi = Math.PI / 6; A.e = 0;
    A.calculateAngles3D();

    const B = new Robot(L1, L2, L3);
    B.xMotor = A.xMotor; B.alpha = A.alpha; B.beta = A.beta;
    B.a = A.a; B.b = A.b; B.c = A.c;
    B.calculatePositionFromMotorAngles();

    const EPS = 4;
    expect(B.x).toBeCloseTo(A.x, EPS);
    expect(B.y).toBeCloseTo(A.y, EPS);
    expect(B.z).toBeCloseTo(A.z, EPS);
    expect(B.phi).toBeCloseTo(A.phi, EPS);
    expect(B.theta).toBeCloseTo(A.theta, EPS);
    expect(B.psi).toBeCloseTo(A.psi, EPS);
  });

  test('Nullpose (alpha=beta=a=0, Hand gerade b=0) -> Fingerspitze (xMotor, -590, 0)', () => {
    const r = fkFromMotors(0, 0, 0, 0, 0, 7);
    expect(r.x).toBeCloseTo(7, 6);            // x = xMotor
    expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 6);
    expect(r.z).toBeCloseTo(0, 6);
  });

  test('a=0 -> Hand-Knick-Achse laeuft parallel zur x-Achse', () => {
    // Bei a=0 knickt die Hand (b) in der y-z-Ebene -> Fingerspitze.x bleibt = xMotor.
    // b=0=gerade, b=90=90°-Knick, b=135=135°-Knick (Phase-2-Konvention).
    const xM = 5;
    for (const bDeg of [0, 45, 90, 135]) {
      const r = fkFromMotors(0, 0, 0, bDeg, 0, xM);
      expect(r.x).toBeCloseTo(xM, 6);
    }
    // Gegenprobe: a=90 dreht die Knick-Achse aus der y-z-Ebene -> x aendert sich deutlich.
    const r90 = fkFromMotors(0, 0, 90, 45, 0, xM);
    expect(Math.abs(r90.x - xM)).toBeGreaterThan(1);
  });

  test('G28 Home: saubere Grundstellung (a=0, c=0, Finger entlang -y) — keine Singularitaets-Garbage', () => {
    const robot = new Robot(L1, L2, L3);
    GCode.receiveGCode(robot, 'G28');

    // Motorwerte sauber gesetzt (nicht der IK-Singularitaets-Muell a=135/c=45)
    expect(robot.a).toBeCloseTo(0, 9);
    expect(robot.c).toBeCloseTo(0, 9);
    expect(robot.alpha).toBeCloseTo(0, 9);
    expect(robot.beta).toBeCloseTo(0, 9);

    // Workspace: voll ausgestreckt entlang -y
    expect(robot.x).toBeCloseTo(0, 6);
    expect(robot.y).toBeCloseTo(-(L1 + L2 + L3), 6);
    expect(robot.z).toBeCloseTo(0, 6);

    // Finger (Handgelenk -> Fingerspitze) zeigt nach -y
    const hx = robot.x - robot.pX, hy = robot.y - robot.pY, hz = robot.z - robot.pZ;
    const n = Math.hypot(hx, hy, hz);
    expect(hy / n).toBeCloseTo(-1, 6);
  });
});