const net  = require("net");
const { resolve } = require("path");
const { TelnetSocket } = require("telnet-stream");
const SenderInterface = require("./SenderInterface");

module.exports = class TelnetSenderGRBL extends SenderInterface {

    /*  urlGRBL:  URL für den GCode Empfänger (MicroController, GRBL)
     *  xAxisGrbl: Welche Achse soll ausgelesen werden? Welche Achse ist die GRBL-X-Achse
     *  yAxisGrbl: ...
     *  zAxisGrbl: ... 
     */
    constructor(urlGRBL = "grblesp.local", maxSpeedF = 5000, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z", aAxisGrbl = null, bAxisGrbl = null, cAxisGrbl = null, eAxisGrbl = null, options = {}){
        super();

        this.tSocket = null;
        this.receiver = null;
        this.connectPromise = null;
        this.connectResolver = null;
        this.connectRejecter = null;
        this.state = 'disconnected';
        this.error = null;

        this.urlGRBLstr = urlGRBL;
        this.xAxisGrbl = xAxisGrbl;
        this.yAxisGrbl = yAxisGrbl;
        this.zAxisGrbl = zAxisGrbl;
        this.aAxisGrbl = aAxisGrbl;
        this.bAxisGrbl = bAxisGrbl;
        this.cAxisGrbl = cAxisGrbl;
        this.eAxisGrbl = eAxisGrbl;
        
        this.maxSpeedF = maxSpeedF; // Speichere für Backward-Kompatibilität

        this.netModule = options.netModule || net;
        this.TelnetSocketClass = options.TelnetSocketClass || TelnetSocket;
        this.setTimeoutFn = options.setTimeoutFn || setTimeout;
        this.clearTimeoutFn = options.clearTimeoutFn || clearTimeout;
        this.setIntervalFn  = options.setIntervalFn  || setInterval;
        this.clearIntervalFn = options.clearIntervalFn || clearInterval;
        this.reconnectDelay = Number.isFinite(options.reconnectDelay) ? options.reconnectDelay : 1000;
        this.maxReconnectDelay = Number.isFinite(options.maxReconnectDelay) ? options.maxReconnectDelay : 30000;
        // Heartbeat: erkennt tote Verbindungen (z.B. nach NotAus).
        // Sendet alle heartbeatInterval ms '?' an den Controller.
        // Kommt deadTimeout ms lang keine Antwort, gilt die Verbindung als tot.
        this.heartbeatInterval = Number.isFinite(options.heartbeatInterval) ? options.heartbeatInterval : 10000;
        this.deadTimeout       = Number.isFinite(options.deadTimeout)       ? options.deadTimeout       : 20000;
        this._heartbeatTimer = null;
        this._lastDataAt     = 0;
        this._rawSocket      = null;
        this.reconnectAttempt = 0;
        this.reconnectTimer = null;
        this.shouldReconnect = true;
        this.autoConnect = options.autoConnect !== false;
        this.isTestMode = false;

        if (urlGRBL === "test.test") {
            this.tSocket = { written: "", write(txt){ this.written = txt; } };
            this.isTestMode = true;
            this.state = 'connected';
            this.shouldReconnect = false;
            return;
        }

        if (this.autoConnect) {
            this.connect();
            console.log("🤖  TelnetSenderGRBL initialized: " + urlGRBL);
        }
    }

    async connect() {
        if (this.isTestMode) {
            this.state = 'connected';
            return Promise.resolve(this);
        }

        if (this.state === 'connected' && this.tSocket) {
            return Promise.resolve(this);
        }

        if (this.connectPromise) {
            return this.connectPromise;
        }

        if (this.reconnectTimer) {
            this.clearTimeoutFn(this.reconnectTimer);
            this.reconnectTimer = null;
        }

        this.state = 'connecting';
        this.error = null;
        this.shouldReconnect = true;

        this.connectPromise = new Promise((resolve, reject) => {
            this.connectResolver = resolve;
            this.connectRejecter = reject;
            this.tryConnect();
        });

        return this.connectPromise;
    }

    tryConnect() {
        if (!this.shouldReconnect) {
            return;
        }

        this.state = 'connecting';
        this.error = null;

        const socket = this.netModule.createConnection({ port: 23, host: this.urlGRBLstr });

        socket.on('connect', () => {
            if (!this.shouldReconnect) {
                if (typeof socket.end === 'function') {
                    socket.end();
                }
                return;
            }

            this._rawSocket = socket;

            // TCP-Keepalive aktivieren: OS sendet Keepalive-Proben auf Netzwerk-Ebene.
            // Schützt als Fallback, falls der Heartbeat-Timer ausfällt.
            socket.setKeepAlive(true, 2000);

            this.tSocket = new this.TelnetSocketClass(socket);
            this.tSocket.on('close', () => {
                console.log("Telnet Closed " + this.urlGRBLstr);
                this._stopHeartbeat();
                this.tSocket = null;
                this._rawSocket = null;
                if (this.shouldReconnect) {
                    this.state = 'reconnecting';
                    this.scheduleReconnect();
                } else {
                    this.state = 'disconnected';
                }
            });

            // Zeitstempel jeder eingehenden Nachricht festhalten (für Heartbeat-Timeout).
            socket.on('data', () => { this._lastDataAt = Date.now(); });

            this.state = 'connected';
            this.error = null;
            this.reconnectAttempt = 0;
            if (this.connectResolver) {
                this.connectResolver(this);
                this.connectResolver = null;
                this.connectRejecter = null;
            }
            this.connectPromise = null;

            // Heartbeat starten: erkennt NotAus / tote Verbindungen.
            this._startHeartbeat();
        });

        socket.on('error', (error) => {
            console.log("Telnet Connection Error on " + this.urlGRBLstr + ": " + error.toString());
            this.tSocket = null;
            this.error = error.message || String(error);
            this.connectPromise = this.connectPromise || null;
            if (this.shouldReconnect) {
                this.state = 'reconnecting';
                this.scheduleReconnect();
            } else {
                this.state = 'disconnected';
                if (this.connectRejecter) {
                    this.connectRejecter(error);
                    this.connectResolver = null;
                    this.connectRejecter = null;
                    this.connectPromise = null;
                }
            }
        });
    }

    scheduleReconnect() {
        if (!this.shouldReconnect || this.reconnectTimer) {
            return;
        }

        const delay = Math.min(this.reconnectDelay * 2 ** this.reconnectAttempt, this.maxReconnectDelay);
        this.reconnectAttempt += 1;
        console.log(`Telnet reconnect attempt ${this.reconnectAttempt} in ${delay}ms for ${this.urlGRBLstr}`);

        this.reconnectTimer = this.setTimeoutFn(() => {
            this.reconnectTimer = null;
            this.tryConnect();
        }, delay);
    }

    /**
     * Startet den Heartbeat-Timer.
     *
     * Sendet alle `heartbeatInterval` ms den FluidNC-Realtime-Command '?' und
     * prüft, ob seit `deadTimeout` ms keine Daten mehr empfangen wurden.
     * Bleibt die Antwort aus (z.B. nach NotAus), wird der Socket zerstört —
     * der bestehende 'close'-Handler leitet danach den Reconnect ein.
     * @private
     */
    _startHeartbeat() {
        this._stopHeartbeat();
        this._lastDataAt = Date.now();

        this._heartbeatTimer = this.setIntervalFn(() => {
            if (!this.tSocket) {
                this._stopHeartbeat();
                return;
            }

            const silent = Date.now() - this._lastDataAt;
            if (silent >= this.deadTimeout) {
                console.log(
                    `[TelnetSenderGRBL] ${this.urlGRBLstr}: ` +
                    `keine Daten seit ${silent}ms (deadTimeout=${this.deadTimeout}ms) — ` +
                    `Verbindung wird als tot eingestuft und beendet`
                );
                this._stopHeartbeat();
                // rawSocket.destroy() löst 'close' auf tSocket aus → bestehende Reconnect-Logik
                if (this._rawSocket) this._rawSocket.destroy();
                return;
            }

            // Leichtgewichtiger Probe: FluidNC antwortet mit <Idle|…> oder <Run|…>
            try { this.tSocket.write('?'); } catch { /* Fehler kommt über 'error'-Event */ }
        }, this.heartbeatInterval);
    }

    /**
     * Stoppt den Heartbeat-Timer.
     * @private
     */
    _stopHeartbeat() {
        if (this._heartbeatTimer) {
            this.clearIntervalFn(this._heartbeatTimer);
            this._heartbeatTimer = null;
        }
    }

    send(command) {
        if (!this.tSocket || typeof this.tSocket.write !== 'function') {
            return false;
        }

        const payload = typeof command === 'string' ? command : String(command);
        if (!payload || payload.length === 0) {
            return false;
        }

        this.tSocket.write(payload + "\r\n");
        return true;
    }

    getStatus() {
        return {
            state: this.state,
            url: this.urlGRBLstr,
            error: this.error,
            isTestMode: !!this.isTestMode,
            reconnectAttempt: this.reconnectAttempt,
            reconnectTimer: !!this.reconnectTimer
        };
    }

    disconnect() {
        this._stopHeartbeat();

        if (this.isTestMode) {
            this.tSocket = null;
            this._rawSocket = null;
            this.state = 'disconnected';
            this.shouldReconnect = false;
            if (this.reconnectTimer) {
                this.clearTimeoutFn(this.reconnectTimer);
                this.reconnectTimer = null;
            }
            return;
        }

        this.shouldReconnect = false;
        if (this.reconnectTimer) {
            this.clearTimeoutFn(this.reconnectTimer);
            this.reconnectTimer = null;
        }

        if (this.connectPromise && this.connectRejecter) {
            this.connectRejecter(new Error('disconnect'));
            this.connectPromise = null;
            this.connectResolver = null;
            this.connectRejecter = null;
        }

        if (this.tSocket && typeof this.tSocket.end === 'function') {
            this.tSocket.end();
        }

        if (this.tSocket && typeof this.tSocket.destroy === 'function') {
            this.tSocket.destroy();
        }

        this.tSocket = null;
        this.state = 'disconnected';
        this.connectPromise = null;
        this.error = null;
    }

    /**
     * Emergency Stop: sendet '!' (Feed Hold) an FluidNC.
     * Best-effort — falls nicht verbunden, wird {ok:false} zurückgegeben.
     * '!' ist ein FluidNC-Realtime-Byte (kein Zeilenende nötig, sofortige Wirkung).
     */
    async emergencyStop() {
        if (!this.tSocket) return { ok: false, error: 'not connected' };
        try {
            this.tSocket.write('!');
            console.warn(`⚠️  [EmergencyStop] Feed Hold '!' → ${this.urlGRBLstr}`);
            return { ok: true };
        } catch (err) {
            return { ok: false, error: err.message };
        }
    }

    /**
     * Alarm-Unlock: sendet '$X\r\n' an FluidNC.
     * Entsperrt den Controller nach einem Power-Loss-Alarm (ALARM:1).
     * Nur aufrufen, nachdem sichergestellt wurde, dass der Roboter frei steht.
     */
    async alarmUnlock() {
        if (!this.tSocket) return { ok: false, error: 'not connected' };
        try {
            this.tSocket.write('$X\r\n');
            return { ok: true };
        } catch (err) {
            return { ok: false, error: err.message };
        }
    }

    moveTo(mOld, mNew){
        this.execCommand("G1", mOld, mNew)
    }

    /**
     * Liefert den Wert, der an einen GRBL-Port gesendet wird, wenn dieser auf eine
     * Roboter-Achse abgebildet ist — als reine Funktion der Positions-/Geschwindigkeits-
     * Quelle `pos` ({x,y,z,a,b,c,e}). Dieselben Formeln wie der Sende-Pfad, aber isoliert
     * und testbar. Wird für die koordinierte Feedrate (ToDo_6a) verwendet.
     * @returns {number|null}
     */
    portValue(grblPort, robotAxis, pos) {
        if (!robotAxis || !pos) return null;
        const D = 180 / Math.PI;
        const factorTurnLift = 1.2;
        const handOpenInMM = 1.0;
        const { x, y, z, a, b, c, e } = pos;

        switch (grblPort) {
            case 'x':
                switch (robotAxis) {
                    case 'x': return x;
                    case 'y': return y * D;
                    case 'z': return (z - y) * D;
                    case 'a': return a * D;
                    case 'b': return (b + z - y) * D;
                    case 'c': return (-b + c) * D;
                    case 'e': return -1.0 * (-1.0 * (b * D * factorTurnLift) + c * D) + e * handOpenInMM;
                }
                break;
            case 'y':
                switch (robotAxis) {
                    case 'x': return x;
                    case 'y': return y * D;
                    case 'z': return (z - y) * D;
                    case 'a': return a * D;
                    case 'b': return (b + z - y) * D;
                    case 'c': return -1.0 * (b * D * factorTurnLift) + c * D;
                    case 'e': return e * D;
                }
                break;
            case 'z':
                switch (robotAxis) {
                    case 'x': return x;
                    case 'y': return y * D;
                    case 'z': return (z - y) * D;
                    case 'a': return a * D;
                    case 'b': return b * D;
                    case 'c': return (c + b + z - y) * D;
                    case 'e': return e * D;
                }
                break;
            case 'a':
                switch (robotAxis) {
                    case 'x': return y * D;   // Quirk: a-Port auf robotAxis 'x' nutzt y (wie im Sende-Pfad)
                    case 'y': return y * D;
                    case 'z': return (z - y) * D;
                    case 'a': return a * D;
                    case 'b': return (b + z - y) * D;
                    case 'c': return (c + b + z - y) * D;
                    case 'e': return e * D;
                }
                break;
            case 'b':
                switch (robotAxis) {
                    case 'x': return x;
                    case 'y': return y * D;
                    case 'z': return (z - y) * D;
                    case 'a': return a * D;
                    case 'b': return b * D;
                    case 'c': return (c + b + z - y) * D;
                    case 'e': return e * D;
                }
                break;
        }
        return null;
    }

    /**
     * Koordinierte Feedrate für diesen Sender (Korrekt-Modus): die euklidische Strecke,
     * die dieser Sender in diesem Schritt zurücklegt, geteilt durch die Bewegungszeit.
     * So beenden alle Controller den Schritt gleichzeitig.
     * @returns {number|null} Feedrate oder null, wenn nicht bestimmbar (→ Legacy-Fallback)
     */
    computeCoordinatedFeedrate(mOld, mNew) {
        const moveTime = mNew && Number.isFinite(mNew.moveTime) ? mNew.moveTime : 0;
        if (!(moveTime > 0) || !mOld) return null;

        const ports = [
            ['x', this.xAxisGrbl],
            ['y', this.yAxisGrbl],
            ['z', this.zAxisGrbl],
            ['a', this.aAxisGrbl],
            ['b', this.bAxisGrbl],
        ];

        let sumSq = 0;
        let any = false;
        for (const [port, robotAxis] of ports) {
            if (!robotAxis) continue;
            const vNew = this.portValue(port, robotAxis, mNew);
            const vOld = this.portValue(port, robotAxis, mOld);
            if (!Number.isFinite(vNew) || !Number.isFinite(vOld)) continue;
            const d = vNew - vOld;
            sumSq += d * d;
            any = true;
        }
        if (!any) return null;

        const dist = Math.sqrt(sumSq);
        if (!(dist > 0)) return null;
        return dist / moveTime;
    }

    execCommand(strCommand = "G1", mOld, mNew ){

        // The Hand-Turn is not 1:1 to the Hand-Lift °     
        var factorTurnLift = 1.2;

        // The Hand-Open is not 1:1 to the Hand-Turn 
        var factorOpenTurn = 1.92;

        // Hand-Open in mm
        var handOpenInMM = 1.0

        var data = strCommand.toString("utf-8"); 

        if(this.xAxisGrbl == "x" && mNew.xMotorChanged && Number.isFinite(mNew.x)){
            data += " x" + (mNew.x).toFixed(2).toString();
        }
        if(this.xAxisGrbl == "y" && mNew.yMotorChanged && Number.isFinite(mNew.y)){
            data += " x" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.xAxisGrbl == "z" && mNew.zMotorChanged && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " x" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
        }
        if(this.xAxisGrbl == "a" && mNew.aMotorChanged && Number.isFinite(mNew.a)){
            // This is the case for the Ellbow, when the Motor is connected to the X-Port of the FluidNC
            data += " x" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.xAxisGrbl == "b" && (mNew.bMotorChanged || mNew.cMotorChanged) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " x" + (mNew.b * 180 / Math.PI+(mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
        }
        if(this.xAxisGrbl == "c"  && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.b) && Number.isFinite(mNew.c)){
            // Runs correctly, substracts the "b" axis, uses the "c" axis to send to the x-Motor wich is in this case the hand-twist
            data += " x" + ((-1)*mNew.b * 180 / Math.PI + (mNew.c * 180 / Math.PI) ).toFixed(2).toString();
        }
        if(this.xAxisGrbl == "e" && mNew.eMotorChanged && Number.isFinite(mNew.b) && Number.isFinite(mNew.c) && Number.isFinite(mNew.e)){
            //This is the case for the Hand, when the Open-Close Motor is connected to the X-Port of FluidNC
            var handUpDown = mNew.b * 180 * factorTurnLift / Math.PI ;
            var handTurn =   mNew.c*180/Math.PI  ;
            data += " x" + ((-1.0*(-1.0*(handUpDown) + handTurn) + mNew.e*handOpenInMM)).toFixed(2).toString();
        }

        

        if(this.yAxisGrbl == "x" && mNew.xMotorChanged && Number.isFinite(mNew.x)){
            data += " y" + (mNew.x ).toFixed(2).toString();
        }
        if(this.yAxisGrbl == "y" && mNew.yMotorChanged && Number.isFinite(mNew.y)){
            data += " y" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.yAxisGrbl == "z" && mNew.zMotorChanged && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " y" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
        }
        if(this.yAxisGrbl == "a" && mNew.aMotorChanged && Number.isFinite(mNew.a)){
            data += " y" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.yAxisGrbl == "b" && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " y" + (mNew.b * 180 / Math.PI+(mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
        }
        if(this.yAxisGrbl == "c" && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.b) && Number.isFinite(mNew.c)){
            // This is the case if the hand-rotation-turner is connected to the FluidNC-Y
            var handUpDown = (mNew.b * 180 / Math.PI ) * factorTurnLift;
            var handTurn =  ( mNew.c*180/Math.PI ) ;
            data += " y" + (-1.0*(handUpDown) + handTurn).toFixed(2).toString();
        }
        if(this.yAxisGrbl == "e"  && (mNew.eMotorChanged) && Number.isFinite(mNew.e)){
            data += " y" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
        }


        if(this.zAxisGrbl == "x" && mNew.xMotorChanged && Number.isFinite(mNew.x)){
            data += " z" + (mNew.x).toFixed(2).toString();
        }
        if(this.zAxisGrbl == "y" && mNew.yMotorChanged && Number.isFinite(mNew.y)){
            data += " z" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.zAxisGrbl == "z" && mNew.zMotorChanged && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " z" + ((mNew.z * 180 / Math.PI)  - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
        }
        if(this.zAxisGrbl == "a" && mNew.aMotorChanged && Number.isFinite(mNew.a)){
            data += " z" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.zAxisGrbl == "b"  && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.b)){
            // This is the case of the Hand, when the Up-Down-Motor is connected to the FluidNC-Z
            data += " z" +  ( mNew.b * 180 / Math.PI ).toFixed(2).toString();
        }
        if(this.zAxisGrbl == "c"  && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.c) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " z" + (mNew.c * 180 / Math.PI + (mNew.b * 180 / Math.PI) + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
        }
        if(this.zAxisGrbl == "e" && (mNew.eMotorChanged) && Number.isFinite(mNew.e)){
        
            data += " z" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
        }


             
        if(this.aAxisGrbl == "x" && mNew.xMotorChanged && Number.isFinite(mNew.y)){
            data += " a" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.aAxisGrbl == "y" && mNew.yMotorChanged && Number.isFinite(mNew.y)){
            data += " a" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.aAxisGrbl == "z" && mNew.zMotorChanged && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " a" + ((mNew.z * 180 / Math.PI)  - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
        }
        if(this.aAxisGrbl == "a" && mNew.aMotorChanged && Number.isFinite(mNew.a)){
            data += " a" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.aAxisGrbl == "b" && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " a" +  (mNew.b * 180 / Math.PI+(mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
        }
        if(this.aAxisGrbl == "c" && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.c) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " a" + (mNew.c * 180 / Math.PI + (mNew.b * 180 / Math.PI) + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
        }
        if(this.aAxisGrbl == "e" && mNew.eMotorChanged && Number.isFinite(mNew.e)){
            // ToDo Mai 2024
            data += " a" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
        }
    
        if(this.bAxisGrbl == "x" && mNew.xMotorChanged && Number.isFinite(mNew.x)){
            data += " b" + (mNew.x).toFixed(2).toString();
        }
        if(this.bAxisGrbl == "y" && mNew.yMotorChanged && Number.isFinite(mNew.y)){
            data += " b" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.bAxisGrbl == "z" && mNew.zMotorChanged && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " b" + ((mNew.z * 180 / Math.PI)  - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
        }
        if(this.bAxisGrbl == "a" && mNew.aMotorChanged && Number.isFinite(mNew.a)){
            data += " b" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.bAxisGrbl == "b" && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.b)){
            data += " b" + (mNew.b * 180 / Math.PI).toFixed(2).toString();
        }
        if(this.bAxisGrbl == "c" && (mNew.bMotorChanged || mNew.cMotorChanged || mNew.zMotorChanged) && Number.isFinite(mNew.c) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)){
            data += " b" + (mNew.c * 180 / Math.PI + (mNew.b * 180 / Math.PI) + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
        }
        if(this.bAxisGrbl == "e" && mNew.eMotorChanged && Number.isFinite(mNew.e)){
            data += " b" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
        }
        
        if(this.tSocket && data.length > 3){
            
            if(strCommand == "G1" && mNew){
                const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ?
                    Number(process.env.ROBOT_DEFAULT_FEEDRATE) : this.maxSpeedF;

                let feedrate;
                if (mNew.speedMode === 'correct') {
                    // Korrekt-Modus: koordinierte Feedrate; Fallback auf Legacy-Wert,
                    // wenn (noch) nicht bestimmbar (z. B. erster Schritt ohne moveTime).
                    const coordinated = this.computeCoordinatedFeedrate(mOld, mNew);
                    feedrate = (coordinated != null && Number.isFinite(coordinated) && coordinated > 0)
                        ? coordinated
                        : (mNew.feedrate !== undefined ? mNew.feedrate : DEFAULT_FEEDRATE);
                } else {
                    // Legacy-Pfad — unverändert: kartesische Feedrate an alle Achsen
                    feedrate = mNew.feedrate !== undefined ? mNew.feedrate : DEFAULT_FEEDRATE;
                }
                data += " f"+(feedrate.toFixed(2).toString())
            }

            if(data.indexOf("G90") == -1 && data.indexOf("G1 ") !== -1){
                data = "G90 " + data;
            }
            
            console.log("Driver send to 🤖  " + this.urlGRBLstr + " the message: " + data)
            
            
            this.tSocket.write( data + "\r\n"); 
        }
    } 
}