const Robot = require('../robot/kinematics/Arm3SegmentLinearX');

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

    const geometries = [
        { L1: 300, L2: 200, L3: 10 },
        { L1: 240, L2: 260, L3: 10 },
        { L1: 200, L2: 300, L3: 10 },
        { L1: 200, L2: 270, L3: 30 },
        { L1: 300, L2: 300, L3: 40 },
        { L1: 220, L2: 200, L3: 40 },
    ];

    const poses = [
        // stabil mittig
        { x: 100, y: 100, z: 0, phi: 0.3, theta: 1.0, psi: 0 },
        { x: 100, y: 200, z: 0, phi: 2, theta: 0.2, psi: 0 },
        { x: 100, y: 100, z: 30, phi: 0, theta: 0.2, psi: 0 },
        { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: 0 },
        { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: .3 },
        { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: .9 },
        { x: -100, y: 200, z: -40, phi: 0.1, theta: 1.2, psi: .9 },
        { x: -100, y: 200, z: -240, phi: 0.1, theta: 1.2, psi: .9 },
        { x: -100, y: 200, z: -240, phi: -0.5, theta: 1.2, psi: -0.9 },
        { x: -100, y: 200, z: -240, phi: -0.5, theta: Math.PI - 0.3, psi: .9 },
    ];

    geometries.forEach(({ L1, L2, L3 }, gIndex) => {

        poses.forEach((pose, pIndex) => {

            test(`Roundtrip G${gIndex} P${pIndex}`, () => {

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

                // Input setzen
                A.x = pose.x;
                A.y = pose.y;
                A.z = pose.z;

                A.phi   = pose.phi;
                A.theta = pose.theta;
                A.psi   = pose.psi;
                A.e     = 0;

                A.calculateAngles3D();

                const motor = {
                    xMotor: A.xMotor,
                    alpha:  A.alpha,
                    beta:   A.beta,
                    a:      A.a,
                    b:      A.b,
                    c:      A.c
                };

                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;

                B.calculatePositionFromMotorAngles();

                const EPS = 0.02; // 10^-2 mm → realistisch bei trig

                try {
                    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);
                    expect(B.psi).toBeCloseTo(A.psi, EPS);
                } catch (err) {
                    console.log("❌ Fehler bei:");
                    console.log("Geometrie:", { L1, L2, L3 });
                    console.log("Pose:", pose);

                    console.log("Soll (A):", {
                        x: A.x, y: A.y, z: A.z,
                        pY: A.pY, pZ: A.pZ
                    });

                    console.log("Ist (B):", {
                        x: B.x, y: B.y, z: B.z,
                        pY: B.pY, pZ: B.pZ
                    });

                    console.log("Motor:", motor);

                    throw err;
                }
            });

        });

    });

});