// __tests__/Robot.inverseKinematics.test.js
const Robot = require('../robot/Robot.js');

describe("Robot Kinematics Roundtrip", () => {

    test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 1", () => {

        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
        const L1 = 300;
        const L2 = 200;
        const L3 = 10;

        const A = new Robot(L1, L2, L3)

        // Beispiel-Eingabe
        A.x = 0;
        A.y = 310;
        A.z = 0;

        A.phi   = -Math.PI/2;
        A.theta = Math.PI/2;
        A.psi   = 0;
        A.e     = 0;

        A.calculateAngles3D();

        // Motorwerte aus Instanz A speichern
        const motor = {
            xMotor: A.xMotor,
            alpha:  A.alpha,
            beta:   A.beta,
            a:      A.a,
            b:      A.b,
            c:      A.c
        };

        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
        const B = new Robot(L1, L2, L3);

        B.xMotor = motor.xMotor;
        B.alpha  = motor.alpha;
        B.beta   = motor.beta;
        B.a      = motor.a;
        B.b      = motor.b;
        B.c      = motor.c;

        // Diese Funktion rekonstruiert nur x, y, z!
        B.calculatePositionFromMotorAngles();

        // === Vergleich mit Toleranz ===
        const EPS = 0.01;  // 1/1000 mm Genauigkeit

        expect(B.pY).toBeCloseTo(A.pY, EPS);
        expect(B.pZ).toBeCloseTo(A.pZ, EPS);
        expect(B.x).toBeCloseTo(A.x, EPS);
        expect(B.y).toBeCloseTo(A.y, EPS);
        expect(B.z).toBeCloseTo(A.z, EPS);
    });

    test("calculateAngles3D() <-> calculatePositionFromMotorAngles()  handgelenk 2", () => {

        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
        const L1 = 300;
        const L2 = 300;
        const L3 = 10;

        const A = new Robot(L1, L2, L3)

        // Beispiel-Eingabe
        A.x = 0;
        A.y = 410;
        A.z = 0;

        A.phi   = -Math.PI/2;
        A.theta = Math.PI/2;
        A.psi   = 0;
        A.e     = 0;

        A.calculateAngles3D();

        // Motorwerte aus Instanz A speichern
        const motor = {
            xMotor: A.xMotor,
            alpha:  A.alpha,
            beta:   A.beta,
            a:      A.a,
            b:      A.b,
            c:      A.c
        };

        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
        const B = new Robot(L1, L2, L3);

        B.xMotor = motor.xMotor;
        B.alpha  = motor.alpha;
        B.beta   = motor.beta;
        B.a      = motor.a;
        B.b      = motor.b;
        B.c      = motor.c; 

        // Diese Funktion rekonstruiert nur x, y, z!
        B.calculatePositionFromMotorAngles();

        // === Vergleich mit Toleranz ===
        const EPS = 0.01;  // 1/1000 mm Genauigkeit

        expect(B.pY).toBeCloseTo(A.pY, EPS);
        expect(B.pZ).toBeCloseTo(A.pZ, EPS);
    
        expect(B.x).toBeCloseTo(A.x, EPS);
        expect(B.y).toBeCloseTo(A.y, EPS);
        expect(B.z).toBeCloseTo(A.z, EPS);
    });

    test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 3", () => {

        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
        const L1 = 300;
        const L2 = 200;
        const L3 = 10;

        const A = new Robot(L1, L2, L3)

        // Beispiel-Eingabe
        A.x = 10;
        A.y = 500;
        A.z = 0;

        A.phi   = 0; //-Math.PI/2;
        A.theta = Math.PI/2;
        A.psi   = 0;
        A.e     = 0;

        A.calculateAngles3D();


        // Motorwerte aus Instanz A speichern
        const motor = {
            xMotor: A.xMotor,
            alpha:  A.alpha,
            beta:   A.beta,
            a:      A.a,
            b:      A.b,
            c:      A.c
        };

        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
        const B = new Robot(L1, L2, L3);

        B.xMotor = motor.xMotor;
        B.alpha  = motor.alpha;
        B.beta   = motor.beta;
        B.a      = motor.a;
        B.b      = motor.b;
        B.c      = motor.c;

        // Diese Funktion rekonstruiert nur x, y, z!
        B.calculatePositionFromMotorAngles();

        // === Vergleich mit Toleranz ===
        const EPS = 0.01;  // 1/1000 mm Genauigkeit

        expect(B.pY).toBeCloseTo(A.pY, EPS);
        expect(B.pZ).toBeCloseTo(A.pZ, EPS);
        expect(B.x).toBeCloseTo(A.x, EPS);
        expect(B.y).toBeCloseTo(A.y, EPS);
        expect(B.z).toBeCloseTo(A.z, EPS);
    });


    test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 4", () => {

        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
        const L1 = 300;
        const L2 = 200;
        const L3 = 10;

        const A = new Robot(L1, L2, L3)

        // Beispiel-Eingabe
        A.x = 10;
        A.y = 430;
        A.z = 30;

        A.phi   = Math.PI/7; //-Math.PI/2;
        A.theta = Math.PI/2;
        A.psi   = 0;
        A.e     = 0;

        A.calculateAngles3D();


        // Motorwerte aus Instanz A speichern
        const motor = {
            xMotor: A.xMotor,
            alpha:  A.alpha,
            beta:   A.beta,
            a:      A.a,
            b:      A.b,
            c:      A.c
        };

        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
        const B = new Robot(L1, L2, L3);

        B.xMotor = motor.xMotor;
        B.alpha  = motor.alpha;
        B.beta   = motor.beta;
        B.a      = motor.a;
        B.b      = motor.b;
        B.c      = motor.c;



        // Diese Funktion rekonstruiert nur x, y, z!
        B.calculatePositionFromMotorAngles();

        // === Vergleich mit Toleranz ===
        const EPS = 0.01;  // 1/1000 mm Genauigkeit

        expect(B.pY).toBeCloseTo(A.pY, EPS);
        expect(B.pZ).toBeCloseTo(A.pZ, EPS);
        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);
    });






});