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

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);
    });








    // --- phi/theta/psi Round-Trip (psi != 0) ---
    // Prueft ob calculatePositionFromMotorAngles() auch die Orientierung
    // (phi, theta, psi) korrekt zurueckrechnet, nicht nur x/y/z.

    function roundTrip(L1, L2, L3, x, y, z, phi, theta, psi) {
        const A = new Robot(L1, L2, L3);
        A.x = x;  A.y = y;  A.z = z;
        A.phi = phi;  A.theta = theta;  A.psi = psi;
        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();
        return { A, B };
    }

    test("phi/theta/psi Round-Trip: psi = +45 Grad", () => {
        const { A, B } = roundTrip(300, 200, 10,  10, 430, 30,  Math.PI/7, Math.PI/2, Math.PI/4);
        const EPS = 4; // 4 Dezimalstellen, ca. 0.1 mrad
        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("phi/theta/psi Round-Trip: psi = -30 Grad", () => {
        const { A, B } = roundTrip(300, 200, 10,  10, 430, 30,  Math.PI/7, Math.PI/2, -Math.PI/6);
        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("phi/theta/psi Round-Trip: psi = +90 Grad, phi = -90 Grad", () => {
        const { A, B } = roundTrip(300, 200, 10,  0, 450, 0,  -Math.PI/2, Math.PI/2, Math.PI/2);
        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);
    });


    // --- Null-Stellung und reale Positionen in -Y Richtung ---
    // Der Roboter arbeitet mit y < 0 als Hauptrichtung.
    // Null-Stellung: Arm voll ausgestreckt in -Y, Fingerspitze bei y=-(l1+l2+l3), z=0.
    // Hand zeigt ebenfalls in -Y => Handvektor (Fingerspitze->Handgelenk) zeigt in +Y
    //   => phi = pi/2, theta = pi/2.

    test("Null-Stellung: Arm voll ausgestreckt in -Y, y=-(L1+L2+L3), z=0", () => {
        const L1 = 300, L2 = 200, L3 = 10;
        const { A, B } = roundTrip(L1, L2, L3,  0, -(L1+L2+L3), 0,  Math.PI/2, Math.PI/2, 0);
        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("Zwischenposition in -Y: Fingerspitze bei y=-400, z=100", () => {
        const L1 = 300, L2 = 200, L3 = 10;
        // Gleiche Handorientierung wie Null-Stellung: phi=pi/2, theta=pi/2
        const { A, B } = roundTrip(L1, L2, L3,  0, -400, 100,  Math.PI/2, Math.PI/2, 0);
        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);
    });

});