/**
 * Verifikation der Port→Motor-Rückrechnung für ToDo_9 / Paket 4 (Sync-Command).
 *
 * Herleitung: doc/ToDo_9a_PortRueckrechnung.md
 *
 * Kernaussage: Für die PRODUKTIV-Verkabelung (startRobot.js) ist die Abbildung
 * der sieben Motorwerte auf die sieben gesendeten GRBL-Achswerte LINEAR und
 * EINDEUTIG umkehrbar. `motorStateFromPorts()` ist diese Umkehrung; die Tests
 * beweisen sie gegen den echten Sende-Pfad (`execCommand`) und gegen `portValue`.
 *
 * Damit ist klar: die B3-Mehrdeutigkeit (Ellbogen oben/unten) betrifft nur die
 * kartesische Inverskinematik (`calculateAngles3D`), die der Sync gar nicht nutzt.
 */
const TelnetSender = require('../robot/TelnetSenderGRBL');
const MotorPosition = require('../robot/RobotMotorPosition');
const Robot = require('../robot/kinematics/Arm3SegmentLinearX');

const D = 180 / Math.PI;

/* ---------- Produktiv-Verkabelung (1:1 wie startRobot.js) ---------- */
function buildSenders() {
  const mk = (...args) => new TelnetSender('test.test', ...args); // test.test → Fake-tSocket
  return {
    base:  mk(2300, 'x', 'y', 'z'),     // GRBL x←xMotor, y←alpha, z←(beta-alpha)
    elbow: mk(5000, 'a', null, null),   // GRBL x←a
    hand:  mk(5000, 'c', 'e', 'b'),     // GRBL x←(c-b), y←eMotor, z←b
  };
}

/* ---------- Die Umkehrung: GRBL-Readings → 7 Motorwerte ---------- */
// r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} }  (GRBL-Achswerte, Grad bzw. mm)
function motorStateFromPorts(r) {
  const xMotor = r.base.x;                        // x-Port = xMotor (mm, direkt)
  const alpha  = r.base.y / D;                    // y-Port = alpha·D
  const beta   = (r.base.z + r.base.y) / D;       // z-Port = (beta-alpha)·D  ⇒ beta = z/D + alpha
  const a      = r.elbow.x / D;                   // Elbow x-Port = a·D
  const b      = r.hand.z / D;                    // Hand z-Port = b·D
  const c      = (r.hand.x + r.hand.z) / D;       // Hand x-Port = (c-b)·D    ⇒ c = x/D + b
  const eMotor = r.hand.y / D;                    // Hand y-Port = eMotor·D
  return { xMotor, alpha, beta, a, b, c, eMotor };
}

/* ---------- Hilfen ---------- */
// {x,y,z,a,b,c,e}-pos aus Motorwerten (Feld-Mapping der RobotMotorPosition)
function posFromMotors(m) {
  return { x: m.xMotor, y: m.alpha, z: m.beta, a: m.a, b: m.b, c: m.c, e: m.eMotor };
}

// Port-Readings über die reine Funktion portValue (volle Präzision)
function portsViaPortValue(S, pos) {
  return {
    base:  { x: S.base.portValue('x', 'x', pos), y: S.base.portValue('y', 'y', pos), z: S.base.portValue('z', 'z', pos) },
    elbow: { x: S.elbow.portValue('x', 'a', pos) },
    hand:  { x: S.hand.portValue('x', 'c', pos), y: S.hand.portValue('y', 'e', pos), z: S.hand.portValue('z', 'b', pos) },
  };
}

// Achswerte aus einer real gesendeten G-Code-Zeile (z. B. "G1 x12.34 y90.00 z-90.00")
function parseAxes(line) {
  const out = {};
  const re = /([xyz])(-?\d+(?:\.\d+)?)/g;
  let m;
  while ((m = re.exec(line)) !== null) out[m[1]] = parseFloat(m[2]);
  return out;
}

// Port-Readings über den ECHTEN Sende-Pfad execCommand (inkl. 2-Dezimal-Rundung)
function portsViaExecCommand(S, m) {
  const mNew = new MotorPosition(m.xMotor, m.alpha, m.beta, m.a, m.b, m.c, m.eMotor);
  for (const f of ['xMotorChanged','yMotorChanged','zMotorChanged','aMotorChanged','bMotorChanged','cMotorChanged','eMotorChanged']) mNew[f] = true;
  const mOld = new MotorPosition(0, 0, 0, 0, 0, 0, 0);

  S.base.execCommand('G1', mOld, mNew);
  S.elbow.execCommand('G1', mOld, mNew);
  S.hand.execCommand('G1', mOld, mNew);

  return {
    base:  parseAxes(S.base.tSocket.written),
    elbow: parseAxes(S.elbow.tSocket.written),
    hand:  parseAxes(S.hand.tSocket.written),
  };
}

/* ---------- Repräsentative Motorzustände (plausible Bereiche) ---------- */
const CASES = [
  { name: 'Nullstellung',     xMotor: 0,   alpha: 0,    beta: 0,    a: 0,    b: 0,    c: 0,    eMotor: 0 },
  { name: 'gemischt klein',   xMotor: 100, alpha: 0.30, beta: 0.50, a: 0.20, b: 0.40, c: 0.90, eMotor: 1.0 },
  { name: 'negative Winkel',  xMotor: -55, alpha:-0.70, beta:-0.25, a:-1.10, b:-0.60, c:-0.30, eMotor:-0.5 },
  { name: 'große Winkel',     xMotor: 250, alpha: 1.20, beta: 2.00, a: 2.90, b: 1.50, c: 3.00, eMotor: 2.5 },
  { name: 'gekoppelt c≈b',    xMotor:  42, alpha: 0.10, beta: 0.10, a: 0.05, b: 1.20, c: 1.25, eMotor: 0.0 },
];

beforeAll(() => { jest.spyOn(console, 'log').mockImplementation(() => {}); });
afterAll(() => { jest.restoreAllMocks(); });

describe('Port→Motor-Rückrechnung (ToDo_9, Baustein für Paket 4)', () => {

  describe('A) exakte Umkehrung gegen portValue (volle Präzision)', () => {
    for (const c of CASES) {
      test(c.name, () => {
        const S = buildSenders();
        const ports = portsViaPortValue(S, posFromMotors(c));
        const rec = motorStateFromPorts(ports);
        for (const k of ['xMotor','alpha','beta','a','b','c','eMotor']) {
          expect(rec[k]).toBeCloseTo(c[k], 9);
        }
      });
    }
  });

  describe('B) Umkehrung gegen den echten Sende-Pfad execCommand (mit Rundung)', () => {
    for (const c of CASES) {
      test(c.name, () => {
        const S = buildSenders();
        const ports = portsViaExecCommand(S, c);
        const rec = motorStateFromPorts(ports);
        // xMotor in mm (Rundung 0.01) → 0.02 Toleranz; Winkel in rad (Rundung 0.01° je Port) → 1e-3
        expect(rec.xMotor).toBeCloseTo(c.xMotor, 2);
        for (const k of ['alpha','beta','a','b','c','eMotor']) {
          expect(Math.abs(rec[k] - c[k])).toBeLessThan(1e-3);
        }
      });
    }
  });

  describe('C) Voll-Kette Sync: Ports → Motoren → Vorwärtskinematik → Pose', () => {
    // Beweist, dass die rekonstruierten Motorwerte über die Vorwärtskinematik
    // exakt dieselbe Pose liefern wie die Originalwerte (= was Sync dem Client meldet).
    for (const c of CASES) {
      test(c.name, () => {
        const S = buildSenders();

        // Referenz-Pose direkt aus den Original-Motorwerten
        const ref = new Robot(250, 264, 100);
        Object.assign(ref, { xMotor: c.xMotor, alpha: c.alpha, beta: c.beta, a: c.a, b: c.b, c: c.c, eMotor: c.eMotor });
        ref.calculatePositionFromMotorAngles();

        // Sync-Pfad: Ports → Inverse → Motoren setzen → Vorwärtskinematik
        const rec = motorStateFromPorts(portsViaPortValue(S, posFromMotors(c)));
        const sync = new Robot(250, 264, 100);
        Object.assign(sync, { xMotor: rec.xMotor, alpha: rec.alpha, beta: rec.beta, a: rec.a, b: rec.b, c: rec.c, eMotor: rec.eMotor });
        sync.calculatePositionFromMotorAngles();

        for (const k of ['x','y','z','phi','theta','psi']) {
          expect(sync[k]).toBeCloseTo(ref[k], 6);
        }
      });
    }
  });
});