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

describe("Robot G92", () => {

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

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

    test("ReadPosition -> calculatePositionFromMotorAngles()", () => {

        // === 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();

        var strGCode = `M92 X${A.xMotor} Y${A.alpha} Z${A.beta} A${A.a} B${A.b} C${A.c}`


        const T = new Robot(L1, L2, L3)
        console.log("GCode: " + strGCode);
        GCode.receiveGCode(T, strGCode);

        
        const EPS = 0.01;  // 1/1000 mm Genauigkeit
        expect(T.xMotor).toBeCloseTo(A.xMotor, EPS);
        expect(T.alpha).toBeCloseTo(A.alpha, EPS);
        expect(T.beta).toBeCloseTo(A.beta, EPS);
        expect(T.x).toBeCloseTo(A.x, EPS);
        expect(T.y).toBeCloseTo(A.y, EPS);
        expect(T.z).toBeCloseTo(A.z, EPS);


    });

    // G92 y2.8 z131.0
    test("ReadPosition -> G92() test ob nur bestimmte Achsen gesendet werden", () => {

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

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

        var telnetSender1 = new TenetSender(urlGRBL = "test.test",   maxSpeedF = 2300, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z");
        var telnetSender2 = new TenetSender(urlGRBL = "test.test", maxSpeedF = 5000, xAxisGrbl = "a", yAxisGrbl = null, zAxisGrbl = null);
        var telnetSender3 = new TenetSender(urlGRBL = "test.test",   maxSpeedF = 5000, xAxisGrbl = "c", yAxisGrbl = "e", zAxisGrbl = "b");

        robot.cmdReceivers.push(telnetSender1);
        robot.cmdReceivers.push(telnetSender2);
        robot.cmdReceivers.push(telnetSender3);

        
        GCode.receiveGCode(robot,"M92 y0.049 z2.286");

        //const strExpect1 = 'G92 x0.00 y2.81 z128.17\r\n';
        const strExpect1 = 'G92 y2.81 z128.17\r\n'; // x soll eben nicht gesendet werden.
        const strExpect2 = 'G92 y2.81 z128.17\r\n';
        
        allowedValues = [strExpect1, strExpect2];
        expect(allowedValues).toContain(telnetSender1.tSocket.written);
        expect(telnetSender2.tSocket.written.length).toBe(0); // kein GCode für Ellbow

        // ("Wenn nur G92 x3 gegeben wird, dann wird trotzdem  auch y und z gesendet. schlecht."  );
    });
    // G92 y2.8 z131.0
    test("ReadPosition -> G92() test ob nur bestimmte Achsen gesendet werden", () => {

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

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

        var telnetSender1 = new TenetSender(urlGRBL = "test.test",   maxSpeedF = 2300, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z");
        var telnetSender2 = new TenetSender(urlGRBL = "test.test", maxSpeedF = 5000, xAxisGrbl = "a", yAxisGrbl = null, zAxisGrbl = null);
        var telnetSender3 = new TenetSender(urlGRBL = "test.test",   maxSpeedF = 5000, xAxisGrbl = "c", yAxisGrbl = "e", zAxisGrbl = "b");

        robot.cmdReceivers.push(telnetSender1);
        robot.cmdReceivers.push(telnetSender2);
        robot.cmdReceivers.push(telnetSender3);

        
        GCode.receiveGCode(robot,"M92 a0.20");

        
        expect(telnetSender1.tSocket.written.length).toBe(0); // kein GCode für XYZ
        expect(telnetSender2.tSocket.written).toBe('G92 x11.46\r\n');
        expect(telnetSender3.tSocket.written.length).toBe(0); // kein GCode für XYZ

        // ("Wenn nur G92 x3 gegeben wird, dann wird trotzdem  auch y und z gesendet. schlecht."  );
    });
});