// Kinematik-RoundTrip-Tests für den Joy-IT „Grab-It" (Robot02),
// implementiert als Arm3SegmentRotaryBase (Roadmap 12, Phase 3).
const Arm3SegmentRotaryBase = require('../robot/kinematics/Arm3SegmentRotaryBase');
const { createRobotFromEnv } = require('../robot/KinematicsFactory');
const RobotBase = require('../robot/RobotBase');

// Default-Geometrie (Schätzwerte): l1=105, l2=98, l3=100, baseHeight=110
const mk = () => new Arm3SegmentRotaryBase();

describe('Arm3SegmentRotaryBase — Workspace → Motor → Workspace', () => {

    // Erreichbare Workspace-Punkte mit konsistentem (impliziten) Azimut.
    const cases = [
        { x: 200, y:   0, z: 110, theta: Math.PI / 2, psi:  0.0,  e: 10 },
        { x: 120, y: 120, z: 200, theta: Math.PI / 3, psi: -0.5,  e: 20 },
        { x: -90, y:  90, z: 150, theta: Math.PI / 2, psi:  0.4,  e:  5 },
        { x:  80, y: -60, z:  90, theta: 2 * Math.PI / 3, psi: 0.2, e: 0 },
    ];

    test.each(cases)('roundtrip x=$x y=$y z=$z', (c) => {
        // A: Inverse (Workspace → Motorwinkel)
        const A = mk();
        A.x = c.x; A.y = c.y; A.z = c.z;
        A.theta = c.theta; A.psi = c.psi; A.e = c.e;
        A.calculateAngles3D();

        // B: Vorwärts (Motorwinkel → Workspace)
        const B = mk();
        B.xMotor = A.xMotor;
        B.alpha  = A.alpha;
        B.beta   = A.beta;
        B.a      = A.a;
        B.b      = A.b;
        B.eMotor = A.eMotor;
        B.calculatePositionFromMotorAngles();

        // Die steuerbaren DOF müssen exakt zurückkommen.
        expect(B.x).toBeCloseTo(c.x, 4);
        expect(B.y).toBeCloseTo(c.y, 4);
        expect(B.z).toBeCloseTo(c.z, 4);
        expect(B.theta).toBeCloseTo(c.theta, 5);
        expect(B.psi).toBeCloseTo(c.psi, 5);

        // phi ist gekoppelt: gleich dem Azimut der Position (5-DOF-Constraint).
        expect(B.phi).toBeCloseTo(Math.atan2(c.y, c.x), 5);
    });
});

describe('Arm3SegmentRotaryBase — Motor → Workspace → Motor', () => {

    // Motorwinkel mit Ellbogen im gewählten Zweig (beta ∈ [0, π]).
    const motors = [
        { xMotor: 0.5,  alpha: 0.8,  beta: 0.6, a: -0.3, b: 0.2,  eMotor: 15 },
        { xMotor: -0.7, alpha: 0.4,  beta: 1.0, a:  0.5, b: -0.4, eMotor: 30 },
        { xMotor: 1.2,  alpha: 1.1,  beta: 0.3, a: -0.6, b: 0.1,  eMotor:  0 },
    ];

    test.each(motors)('roundtrip alpha=$alpha beta=$beta', (m) => {
        // A: Vorwärts (Motor → Workspace)
        const A = mk();
        Object.assign(A, m);
        A.calculatePositionFromMotorAngles();

        // B: Inverse (Workspace → Motor)
        const B = mk();
        B.x = A.x; B.y = A.y; B.z = A.z;
        B.theta = A.theta; B.psi = A.psi; B.e = A.eMotor;
        B.calculateAngles3D();

        expect(B.xMotor).toBeCloseTo(m.xMotor, 5);
        expect(B.alpha).toBeCloseTo(m.alpha, 5);
        expect(B.beta).toBeCloseTo(m.beta, 5);
        expect(B.a).toBeCloseTo(m.a, 5);
        expect(B.b).toBeCloseTo(m.b, 5);
        expect(B.eMotor).toBeCloseTo(m.eMotor, 5);
    });
});

describe('Arm3SegmentRotaryBase — 5-DOF-Eigenschaften', () => {

    test('phi-Eingabe wird in der Inversen ignoriert', () => {
        const base = { x: 150, y: 60, z: 130, theta: Math.PI / 2, psi: 0.1, e: 0 };

        const withPhi = mk();
        Object.assign(withPhi, base, { phi: 2.5 }); // bewusst widersprüchliches phi
        withPhi.calculateAngles3D();

        const noPhi = mk();
        Object.assign(noPhi, base, { phi: -1.0 }); // anderes phi
        noPhi.calculateAngles3D();

        // Trotz unterschiedlicher phi-Eingabe identische Motorwinkel.
        for (const k of ['xMotor', 'alpha', 'beta', 'a', 'b', 'eMotor']) {
            expect(noPhi[k]).toBeCloseTo(withPhi[k], 9);
        }
        // phi wird aus der Position abgeleitet.
        expect(withPhi.xMotor).toBeCloseTo(Math.atan2(base.y, base.x), 9);
    });

    test('c-Slot bleibt ungenutzt (= 0)', () => {
        const r = mk();
        r.x = 180; r.y = 0; r.z = 120; r.theta = Math.PI / 2; r.psi = 0; r.e = 0;
        r.calculateAngles3D();
        expect(r.c).toBe(0);
    });

    test('unerreichbarer Punkt lässt die Motor-Slots unverändert', () => {
        const r = mk();
        r.alpha = 0; r.beta = 0; r.a = 0; // bekannter Startzustand
        r.x = 5000; r.y = 0; r.z = 0; r.theta = Math.PI / 2; r.psi = 0; r.e = 0;
        r.calculateAngles3D();
        // R > l1+l2 → frühzeitiges return, alpha/beta/a unverändert
        expect(r.alpha).toBe(0);
        expect(r.beta).toBe(0);
        expect(r.a).toBe(0);
    });
});

describe('KinematicsFactory lädt den Grab-It', () => {

    test('Alias "grabit" liefert eine Arm3SegmentRotaryBase-Instanz', () => {
        const robot = createRobotFromEnv({ ROBOT_KINEMATICS: 'grabit' });
        expect(robot).toBeInstanceOf(Arm3SegmentRotaryBase);
        expect(robot).toBeInstanceOf(RobotBase);
    });

    test('ROBOT_KINEMATICS_PARAMS setzt Längen und baseHeight', () => {
        const robot = createRobotFromEnv({
            ROBOT_KINEMATICS: 'robot02',
            ROBOT_KINEMATICS_PARAMS: '{"l1":110,"l2":100,"l3":95,"baseHeight":130}',
        });
        expect([robot.l1, robot.l2, robot.l3]).toEqual([110, 100, 95]);
        expect(robot.baseHeight).toBe(130);
    });

    test('Default-baseHeight wenn nicht gesetzt', () => {
        const robot = createRobotFromEnv({ ROBOT_KINEMATICS: 'grabit' });
        expect(robot.baseHeight).toBe(110);
    });
});