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

describe("Robot Gcode receive", () => {

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

    afterAll(() => {
        jest.restoreAllMocks();
    });

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

                GCode.receiveGCode(A,`G90 G1 x${pose.x} y${pose.y} z${pose.z} a${pose.phi} b${pose.theta} c${pose.psi} f200`);
                
                var epsilon = 0.02

                expect(A.x).toBeCloseTo(pose.x, epsilon);
                expect(A.y).toBeCloseTo(pose.y, epsilon);
                expect(A.z).toBeCloseTo(pose.z, epsilon);
                expect(A.phi).toBeCloseTo(pose.phi, epsilon);
                expect(A.theta).toBeCloseTo(pose.theta, epsilon);
                expect(A.psi).toBeCloseTo(pose.psi, epsilon);
            });
        });
    });
});