// 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 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=180 = aktuelle "gerade Hand"-Konvention (Phase 2 macht daraus 0)
    const r = fkFromMotors(0, 0, 0, 180, 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  (B=C=0)
    const r = fkFromMotors(4.53, 13.93, 124.04, 0, 0, 160.53);
    expect(r.y).toBeLessThan(0);
    expect(r.y).toBeCloseTo(-405, 0);
    expect(r.z).toBeCloseTo(58, 0);
  });

  test('IK der -y-Grundstellung liefert alpha~0 / beta~0 (nicht ~180)', () => {
    const ref = fkFromMotors(0, 0, 0, 180, 0);   // -y-ausgestreckte Pose als Referenz
    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);
  });
});