const MotorPosition = require('./RobotMotorPosition.js')

/**
 * RobotBase — abstrakte Basisklasse und zugleich der Interface-Vertrag des
 * Frameworks. Enthält die roboter-unabhängige Infrastruktur (Zustand,
 * `sendCommand`, `createMotorPosition`, `calculateSpeeds`, `rotateAroundAxis`).
 *
 * Der arm-spezifische Teil sind ausschließlich die beiden Kinematik-Methoden
 * {@link RobotBase#calculateAngles3D} und
 * {@link RobotBase#calculatePositionFromMotorAngles}. Sie sind hier abstrakt
 * (werfen) und MÜSSEN von jeder konkreten Kinematik-Klasse überschrieben werden.
 *
 * Verträge (siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md):
 * - Workspace-Koordinaten: `x, y, z, phi, theta, psi` + `e` (Greifer) — 6-DOF.
 * - Motor-Zustand: 7 feste Slots `xMotor, alpha, beta, a, b, c, eMotor`.
 *
 * Konkrete Implementierungen leben in `robot/kinematics/`.
 *
 * @abstract
 */
class RobotBase{


    constructor() {
        // Umgebungsvariablen-Logik
        const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ?
            Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000;
        // Speed-Regelung-Schalter: 'legacy' (Default — exakt wie bisher) oder 'correct'.
        // Siehe doc/ToDo_6a_Speed.md.
        this.speedMode = (process.env.ROBOT_SPEED_MODE || 'legacy').toLowerCase();
        // ROBOT_USE_SPEED_CALC bleibt der interne Schalter für calculateSpeeds();
        // der Korrekt-Modus aktiviert die Berechnung automatisch.
        this.useSpeedCalc = this.speedMode === 'correct' ||
                            process.env.ROBOT_USE_SPEED_CALC === 'true' ||
                            process.env.ROBOT_USE_SPEED_CALC === '1';
        /** @type {number} Bewegungszeit des letzten Schritts in Minuten (für koordinierte Feedrate) */
        this.lastMoveTime = 0;

        /** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */
        this.speedX = 200;
        /** @type {number} Bewegungsgeschwindigkeit Y-Achse in mm/min */
        this.speedY = 200;
        /** @type {number} Bewegungsgeschwindigkeit Z-Achse in mm/min */
        this.speedZ = 200;

        /** @type {number} Zeitstempel des zuletzt gesendeten Kommandos */
        this.lastCommandSend = 0;

        if(this.lastCommandSend == 0){ this.lastCommandSend = Date.now() };
        /** @type {boolean} Animation aktiviert */
        this.doAnimate = false;

        // Plan-Koordinaten - XYZ FingerSpitze
        /** @type {number} X-Position der Fingerspitze in mm */
        this.x = 0;
        /** @type {number} Y-Position der Fingerspitze in mm */
        this.y = 0;
        /** @type {number} Z-Position der Fingerspitze in mm */
        this.z = 0;

        // Plan-Koordinaten - HandRichtung (Euler-Winkel)
        /** @type {number} Phi - Euler-Winkel (Längengrad): Rotation um Z-Achse in rad */
        this.phi = 0.0;
        /** @type {number} Theta - Euler-Winkel (Breitengrad): Neigungswinkel der Handachse in rad */
        this.theta = -Math.PI/2;
        /** @type {number} Psi - Euler-Winkel: Zusätzliche Drehung des Handgelenks in rad */
        this.psi = 0.0;

        /** @type {number} Finger-Abstands-Einstellung (Öffnungsweite) */
        this.e = 0.0;

        /** @type {number} Feedrate für Bewegungen in mm/min */
        this.feedrate = DEFAULT_FEEDRATE;

        /** @type {Object} Motor-Geschwindigkeiten in Einheiten pro Minute */
        this.motorSpeeds = {x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0};

        // Zwischen-Ergebnisse: Handgelenk-Punkt (Koordinaten des Handgelenks, nur für Tests public)
        /** @type {number} Handgelenk-Position X in mm (berechneter Zwischenwert) */
        this.pX = 0.0;
        /** @type {number} Handgelenk-Position Y in mm (berechneter Zwischenwert) */
        this.pY = 0.0;
        /** @type {number} Handgelenk-Position Z in mm (berechneter Zwischenwert) */
        this.pZ = 0.0;

        // Motor-Koordinaten - Schulter, Ellebogen, Hand-Dreher
        /** @type {number} X-Motor-Position (Schulterposition auf X-Schiene) in mm */
        this.xMotor = 0;
        /** @type {number} Alpha - Y-Motor-Winkel (Schulterposition) in rad */
        this.alpha = 0;
        /** @type {number} Beta - Z-Motor-Winkel (Unterarm-Neigung unter Y-Achse) in rad */
        this.beta = 0;

        this.xMotorChanged = false;
        this.yMotorChanged = false;
        this.zMotorChanged = false;

        // Motor-Winkel für's Handgelenk
        /** @type {number} a-Motor-Winkel: Rotation am Ellbogen in rad */
        this.a = 0;
        /** @type {number} b-Motor-Winkel: Handgelenk-Knicker-Winkel in rad */
        this.b = 0;
        /** @type {number} c-Motor-Winkel: Hand-Dreher-Rotation in rad */
        this.c = 0;

        this.aMotorChanged = false;
        this.bMotorChanged = false;
        this.cMotorChanged = false;
        this.eMotorChanged = false;

        /** @type {number} e-Motor-Wert: Finger-Abstands-Motor-Position */
        this.eMotor = 0;

        /** @type {number} Zeitstempel des letzten verarbeiteten Kommandos */
	    this.oldCommandTime = Date.now();
        /** @type {Function[]} Array von Visualisierungs-Funktionen */
        this.showFunctions = [];

        /** @type {Object[]} Gespeicherte Roboterpositionen/Punkte */
        this.savedPoints = [];
        /** @type {number} Index des aktuell angesteuerten Punktes */
        this.atPointNr = 0;
        /** @type {number} Zeitstempel des aktuellen Punktes in ms */
        this.t = 0;

        /** @type {boolean} Relative oder absolute Bewegung (true = relativ) */
        this.moveRelative = true;

        /** @type {Object[]} Array von Kommando-Empfängern */
        this.cmdReceivers = [];
    }

    createMotorPosition(){
        this.motorPosition =  new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);

        // Setze Changed-Flags basierend auf Änderungen seit der letzten Position
        this.motorPosition.xMotorChanged = this.motorPositionOld ? this.xMotor !== this.motorPositionOld.x : true;
        this.motorPosition.yMotorChanged = this.motorPositionOld ? this.alpha !== this.motorPositionOld.y : true;
        this.motorPosition.zMotorChanged = this.motorPositionOld ? this.beta !== this.motorPositionOld.z : true;
        this.motorPosition.aMotorChanged = this.motorPositionOld ? this.a !== this.motorPositionOld.a : true;
        this.motorPosition.bMotorChanged = this.motorPositionOld ? this.b !== this.motorPositionOld.b : true;
        this.motorPosition.cMotorChanged = this.motorPositionOld ? this.c !== this.motorPositionOld.c : true;
        this.motorPosition.eMotorChanged = this.motorPositionOld ? this.eMotor !== this.motorPositionOld.e : true;

        // Setze Handgelenk-Koordinaten (für Speed-Berechnung)
        this.motorPosition.pX = this.pX;
        this.motorPosition.pY = this.pY;
        this.motorPosition.pZ = this.pZ;

        // Setze Geschwindigkeiten
        this.motorPosition.speeds = {...this.motorSpeeds};
        this.motorPosition.feedrate = this.feedrate || 200;

        // Speed-Regelung: Modus und (vorläufige) Bewegungszeit für die Sender
        this.motorPosition.speedMode = this.speedMode;
        this.motorPosition.moveTime = this.lastMoveTime || 0;
    }

    /**
     * Vorwärts-Kinematik: Workspace-Koordinaten → Motorwinkel.
     *
     * Liest `this.x/y/z/phi/theta/psi/e` und schreibt das Ergebnis auf die
     * Motor-Slots `this.xMotor/alpha/beta/a/b/c/eMotor` (sowie die Zwischenwerte
     * `this.pX/pY/pZ` des Handgelenk-Punkts).
     *
     * ABSTRAKT — muss von der konkreten Kinematik-Klasse überschrieben werden.
     *
     * @abstract
     * @param {boolean} [verbose] Debug-Ausgaben aktivieren
     */
    calculateAngles3D(verbose){
        throw new Error('calculateAngles3D() not implemented — RobotBase ist abstrakt; eine konkrete Kinematik-Klasse muss diese Methode überschreiben.');
    }

    // Berechnet die Motor-Geschwindigkeiten basierend auf Feedrate und Positionsänderung
    calculateSpeeds(oldPos, newPos) {
        if (!this.useSpeedCalc) return; // Neue Logik nur aktivieren, wenn Flag gesetzt
        if (!oldPos || !newPos || this.feedrate <= 0) return;

        this.lastMoveTime = 0; // wird unten gesetzt, sobald eine Bewegung erkannt wird

        // 1. Berechne xyz-Distanz (primär)
        const dx = newPos.x - oldPos.x;
        const dy = newPos.y - oldPos.y;
        const dz = newPos.z - oldPos.z;
        const xyz_dist = Math.sqrt(dx*dx + dy*dy + dz*dz);

        if (xyz_dist > 0.001) {
            const time = xyz_dist / this.feedrate;
            this.lastMoveTime = time;
            this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
            this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
            this.motorSpeeds.z = (this.beta - oldPos.z) / time;
            this.motorSpeeds.a = (this.a - oldPos.a) / time;
            this.motorSpeeds.b = (this.b - oldPos.b) / time;
            this.motorSpeeds.c = (this.c - oldPos.c) / time;
            this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
            return;
        }

        // 2. Berechne Handgelenk-Punkt-Distanz (falls xyz = 0)
        const dpx = newPos.pX - oldPos.pX;
        const dpy = newPos.pY - oldPos.pY;
        const dpz = newPos.pZ - oldPos.pZ;
        const handgelenk_dist = Math.sqrt(dpx*dpx + dpy*dpy + dpz*dpz);

        if (handgelenk_dist > 0.001) {
            const time = handgelenk_dist / this.feedrate;
            this.lastMoveTime = time;
            this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
            this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
            this.motorSpeeds.z = (this.beta - oldPos.z) / time;
            this.motorSpeeds.a = (this.a - oldPos.a) / time;
            this.motorSpeeds.b = (this.b - oldPos.b) / time;
            this.motorSpeeds.c = (this.c - oldPos.c) / time;
            this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
            return;
        }

        // 3. Berechne Finger-Distanz (falls Handgelenk = 0)
        const de = Math.abs(this.eMotor - oldPos.e);
        if (de > 0.001) {
            const time = de / this.feedrate;
            this.lastMoveTime = time;
            this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
            this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
            this.motorSpeeds.z = (this.beta - oldPos.z) / time;
            this.motorSpeeds.a = (this.a - oldPos.a) / time;
            this.motorSpeeds.b = (this.b - oldPos.b) / time;
            this.motorSpeeds.c = (this.c - oldPos.c) / time;
            this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
            return;
        }

        // 4. Keine Bewegung erkannt → motorSpeeds bleiben AUF DEFAULT


        // ToDo: Aus motorSpeed mit einzelnenen Werten muss noch die feedrate berechnet werden.
        // hier bin ich unsicher, ob das nicht in den Sender rein sollte, da es eventuell
        // abhngig vom FluidNC und dessen speed interpretation ist.
    }

    /**
     * Geschützte Hilfsmethode (Rodrigues-Rotation): dreht den Vektor `v` um die
     * (normierte) Achse `n` um den Winkel `angle`. Für Kinematik-Implementierungen.
     */
    rotateAroundAxis(v, n, angle) {
        const cos = Math.cos(angle);
        const sin = Math.sin(angle);

        const dot = v.x*n.x + v.y*n.y + v.z*n.z;

        const cross = {
            x: n.y*v.z - n.z*v.y,
            y: n.z*v.x - n.x*v.z,
            z: n.x*v.y - n.y*v.x
        };

        return {
            x: v.x*cos + cross.x*sin + n.x*dot*(1 - cos),
            y: v.y*cos + cross.y*sin + n.y*dot*(1 - cos),
            z: v.z*cos + cross.z*sin + n.z*dot*(1 - cos)
        };
    }

    /**
     * Rückwärts-Kinematik: Motorwinkel → Workspace-Koordinaten.
     *
     * Liest die Motor-Slots `this.xMotor/alpha/beta/a/b/c` und schreibt das
     * Ergebnis auf `this.x/y/z/phi/theta/psi` (sowie `this.pX/pY/pZ`).
     *
     * ABSTRAKT — muss von der konkreten Kinematik-Klasse überschrieben werden.
     *
     * @abstract
     * @param {boolean} [verbose] Debug-Ausgaben aktivieren
     */
    calculatePositionFromMotorAngles(verbose = false) {
        throw new Error('calculatePositionFromMotorAngles() not implemented — RobotBase ist abstrakt; eine konkrete Kinematik-Klasse muss diese Methode überschreiben.');
    }

    sendCommand(cmd="G1"){
        const isFirstCall = !this.motorPosition;

        if (isFirstCall) {
            this.motorPositionOld = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);
        } else {
            this.motorPositionOld = this.motorPosition;
        }

        this.createMotorPosition()

        // Für den ersten Aufruf setze alle Changed-Flags auf true
        if (isFirstCall) {
            this.motorPosition.xMotorChanged = true;
            this.motorPosition.yMotorChanged = true;
            this.motorPosition.zMotorChanged = true;
            this.motorPosition.aMotorChanged = true;
            this.motorPosition.bMotorChanged = true;
            this.motorPosition.cMotorChanged = true;
            this.motorPosition.eMotorChanged = true;
        }

        // Berechne Geschwindigkeiten
        this.calculateSpeeds(this.motorPositionOld, this.motorPosition);
        this.motorPosition.speeds = {...this.motorSpeeds};
        // moveTime nach der Berechnung aktualisieren (createMotorPosition lief davor)
        this.motorPosition.moveTime = this.lastMoveTime || 0;

        console.log("Robot.sendCommand: ", cmd.toString(), " Motor-Pos: x=", this.motorPosition.x.toFixed(3), "yMotor=",this.motorPosition.y.toFixed(3), "zMotor=",this.motorPosition.z.toFixed(3), "aM=", this.motorPosition.a.toFixed(3), "bM=", this.motorPosition.b.toFixed(3), "cM=", this.motorPosition.c.toFixed(3), " e=", this.motorPosition.e.toFixed(3));

        this.cmdReceivers.forEach(receiver => {
                receiver.execCommand(cmd,this.motorPositionOld, this.motorPosition);
        });
    }
}



module.exports = RobotBase // Export abstrakte Basisklasse / Interface