const RobotBase = require('../RobotBase.js')

/**
 * Arm3SegmentLinearX — 6-DOF-Arm mit linearer X-Schiene und drei Segmenten
 * (Oberarm `l1`, Unterarm `l2`, Hand/Endeffector `l3`) plus Greifer.
 *
 * Erste konkrete Kinematik-Implementierung des Frameworks. Implementiert den
 * Interface-Vertrag von {@link RobotBase} — die gesamte generische Infrastruktur
 * (Zustand, sendCommand, Speed-Berechnung, ...) wird geerbt.
 *
 * Physikalische Struktur:
 * - X-Achse: lineare Schiene (Schulterposition `xMotor`)
 * - Schulter/Ellbogen: 2D-Arm in der Y-Z-Ebene (`alpha`, `beta`)
 * - Handgelenk: drei Winkel `a` (Ellbogen-Dreher), `b` (Knicker), `c` (Hand-Dreher)
 * - Greifer: `e`
 */
class Arm3SegmentLinearX extends RobotBase {

    /**
     * @param {number} l1 Länge des Oberarms in mm
     * @param {number} l2 Länge des Unterarms in mm
     * @param {number} l3 Länge der Hand (Endeffector) in mm
     * @param {Object} [config] Bewegungs-Config (defaultFeedrate, speedMode, …)
     */
    constructor(l1, l2, l3, config = {}) {
        super(config);

        /** @type {number} Länge des Oberarms in mm */
        this.l1 = l1;
        /** @type {number} Länge des Unterarms in mm */
        this.l2 = l2;
        /** @type {number} Länge der Hand (Endeffector) in mm */
        this.l3 = l3;
    }

    // ─────────────────────────────────────────────────────────────────────
    // Y-Konvention: Der reale Roboter steht/arbeitet in -y (robot.json:
    // Arm1 -> [0,-250,0], coordinateSystem.y = "backward"). Die interne
    // Kinematik (_ikPlusY/_fkPlusY) rechnet historisch in +y. Beide
    // öffentlichen Methoden spiegeln daher die Workspace-Pose an der x-z-Ebene
    // (y, pY, phi, psi; theta bleibt), sodass alpha=0 nach -y zeigt.
    // Siehe doc/Info_Koordinaten.md (Weg 2, Phase 1).
    // ─────────────────────────────────────────────────────────────────────

    /** Reflexion der Workspace-Pose an der x-z-Ebene (Involution: zweimal = Identität). */
    _mirrorWorkspaceY() {
        this.y   = -this.y;
        this.pY  = -this.pY;
        this.phi = -this.phi;
        this.psi = -this.psi;
    }

    calculateAngles3D(verbose) {
        // -y-Eingabe in den internen +y-Frame spiegeln, rechnen, dann die
        // Workspace-Felder zurückspiegeln (Motorwerte bleiben unberührt).
        this._mirrorWorkspaceY();
        this._ikPlusY(verbose);
        this._mirrorWorkspaceY();
    }

    calculatePositionFromMotorAngles(verbose = false) {
        this._fkPlusY(verbose);
        this._mirrorWorkspaceY();   // +y-Ergebnis -> -y Workspace
    }

    // Berechnet aus XYZ die Motor-Winkel für den GCode (interne +y-Mathematik)
    _ikPlusY(verbose){
        while(this.phi >  Math.PI){this.phi -= 2*Math.PI}
        while(this.phi < -Math.PI){this.phi += 2*Math.PI}
        while(this.theta >  Math.PI){this.theta -= 2*Math.PI}
        while(this.theta < -Math.PI){this.theta += 2*Math.PI}

        // Handgelenk-Punkt ausrechnen:
        this.pX = this.x + this.l3*Math.sin(this.theta)*Math.cos(this.phi);
        this.pY = this.y + this.l3*Math.sin(this.theta)*Math.sin(this.phi);
        this.pZ = this.z + this.l3*Math.cos(this.theta);

        var pX = this.pX;
        var pY = this.pY;
        var pZ = this.pZ;

        this.xMotor = pX;
        // Ziel-Punkt ausrechnen ==> 2D Rechnung Arm
        var r = Math.sqrt(pY * pY + pZ * pZ);

        if (r > (this.l1 + this.l2)) { return; }
        if (r == 0) { return; }

        var gamma = Math.atan2(pZ, pY);
        var delta = Math.acos((this.l1 * this.l1 + this.l2 * this.l2 - r * r) / (2 * this.l1 * this.l2));
        this.alpha = Math.acos((this.l1 * this.l1 + r * r - this.l2 * this.l2) / (2 * r * this.l1)) + gamma;
        this.beta =  -Math.PI + (this.alpha + delta);
        // Ende <== 2D Rechnung Arm

        // Richtung der Hand ausgerechnet
        //          Arm = (0, cos(beta), sin(beta))      Punkt = (sin(theta)cos(phi), sin(theta)sin(phi), cos(theta))
        //
        // Unterarm muss gedreht werden. Aus der Y-Z-Ebene raus. Hin in die Ebene n x r
        //          wobei n =  Unterarm x (P-O) ist
        var nX = Math.cos(this.beta)*Math.cos(this.theta)  - Math.sin(this.theta)*Math.sin(this.phi)*Math.sin(this.beta);
        var nY = Math.sin(this.beta)*Math.sin(this.theta)*Math.cos(this.phi);
        var nZ = -1.0*Math.sin(this.theta)*Math.cos(this.phi)*Math.cos(this.beta);
        var nBetrag = Math.sqrt(nX*nX + nY*nY + nZ*nZ);

        if(verbose) console.log("Richtung: > ", nX/nBetrag, nY/nBetrag, nZ/nBetrag);

        var cosA = (nX)/nBetrag;
        this.a = Math.acos(cosA)
        if(Math.cos(this.phi) > 0){this.a = -this.a}
        if(Math.sin(this.theta) < 0) {this.a = -this.a}


        // Handgelenk-Knick-Winkel: b=0 = gerade (Phase 2). cosB ist cos des internen
        // Winkels zwischen Unterarm und Hand; acos liefert [0,π], also b = π−acos(cosB) ∈ [0,π].
        var cosB = Math.cos(this.beta)*Math.sin(this.theta)*Math.sin(this.phi) + Math.sin(this.beta)*Math.cos(this.theta);
        this.b = Math.PI - Math.acos(cosB);


        // Winkel zwischen  n und z muss rumgedreht werden.
        var cosC = - nZ  / nBetrag;
        this.c = Math.acos(cosC);
        this.c += this.psi;

        // a um 180° drehen
        this.a += Math.PI;
        while(this.a > Math.PI){this.a -= 2*Math.PI}
        while(this.a < -Math.PI){this.a += 2*Math.PI}

        this.eMotor = this.gripperMotorFromOpening(this.e);
    }

    /**
     * Greifer-Kopplung dieses Arms: die Finger-Sehne läuft durchs Handgelenk, daher
     * ziehen Knick (`b`) und Dreh (`c`) am Greifer mit. `eMotor` kompensiert das, damit
     * die Finger-Öffnung `e` (mm, ab Null-Position eines Fingers) unabhängig von der
     * Handstellung bleibt. Einzige Quelle für diese Kopplung (auch via G92/M92 genutzt).
     */
    gripperMotorFromOpening(e) {
        return e - this.b - this.c;
    }

    _fkPlusY(verbose = false) {

        const vecBizeps   = {x: this.xMotor, y: this.l1 * Math.cos(this.alpha), z: this.l1 * Math.sin(this.alpha)}
        const vecUnterarm = {x: 0, y: Math.cos(this.beta), z: Math.sin(this.beta)}

        // der Handgelenk-Punkt
        this.pX = vecBizeps.x + this.l2 * vecUnterarm.x;
        this.pY = vecBizeps.y + this.l2 * vecUnterarm.y;
        this.pZ = vecBizeps.z + this.l2 * vecUnterarm.z;

        // n: Die Handgelenk-Unterarm-Knick-Achse. X-Achse wird um den Unterarm gedreht.
        const n = { x: -Math.cos(this.a), y: vecUnterarm.z * Math.sin(this.a), z: -vecUnterarm.y * Math.sin(this.a) };

        if(verbose) console.log("n inverse:", n.x, n.y, n.z);

        // b=0 = gerade (Phase 2): interne Rotation um π−b (bei b=0 → π → Hand gestreckt).
        const vHand = this.rotateAroundAxis(vecUnterarm, n, Math.PI - this.b);

        this.x = this.pX - this.l3 * vHand.x;
        this.y = this.pY - this.l3 * vHand.y;
        this.z = this.pZ - this.l3 * vHand.z;

        this.theta = Math.atan2(Math.sqrt(vHand.x*vHand.x + vHand.y*vHand.y),vHand.z);
        this.phi   = Math.atan2(vHand.y, vHand.x);
        this.psi   = this.c - Math.acos(-n.z);

        while(this.phi >  Math.PI){this.phi -= 2*Math.PI}
        while(this.phi < -Math.PI){this.phi += 2*Math.PI}
        while(this.theta >  Math.PI){this.theta -= 2*Math.PI}
        while(this.theta < -Math.PI){this.theta += 2*Math.PI}
    }
}


module.exports = Arm3SegmentLinearX