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

/**
 * Arm3SegmentRotaryBase — Kinematik für den **Joy-IT „Grab-It" (Robot02)**.
 *
 * Strukturell wie {@link Arm3SegmentLinearX} (drei Arm-Segmente: Oberarm `l1`,
 * Unterarm `l2`, Hand `l3`), aber statt der linearen X-Schiene sitzt der Arm auf
 * einer **drehbaren Basis** (Yaw). Dazu kommt eine Handgelenk-Rotation.
 *
 * ───────────────────────────────────────────────────────────────────────────
 * Physische Spezifikation (recherchiert aus dem Joy-IT-Datenblatt / Handbuch):
 *   - 6 PWM-Servos (20 kg, Metallgetriebe), 360° mech. / 180° Arbeitsbereich
 *   - 5 Bewegungsachsen + Greifer
 *   - Reichweite ca. 300 mm ab Mitte Drehteller, Bauhöhe bis 420 mm
 *
 * Gelenkkette (Basis → Greifer):
 *   q1  Basis-Drehung   (Yaw um die vertikale Z-Achse)   → this.xMotor
 *   q2  Schulter        (Pitch)                           → this.alpha
 *   q3  Ellbogen        (Pitch, relativ zum Oberarm)      → this.beta
 *   q4  Handgelenk      (Pitch, relativ zum Unterarm)     → this.a
 *   q5  Handgelenk-Roll (Drehung um die Hand-/Anflugachse)→ this.b
 *   –   (ungenutzt)                                        → this.c  (= 0)
 *   q6  Greifer         (Öffnungsweite)                   → this.eMotor
 *
 * Damit reichen die 7 Motor-Slots des Frameworks aus; ein Slot (`c`) bleibt frei.
 *
 * ───────────────────────────────────────────────────────────────────────────
 * 5-DOF-Einschränkung (wichtig!):
 * Ein 5-Achs-Arm kann **nicht** jede beliebige 6-DOF-Pose (x,y,z,phi,theta,psi)
 * erreichen. Da die Basis-Drehung den Azimut bestimmt, liegt der gesamte Arm in
 * **einer** vertikalen Ebene durch die Basisachse. Folge:
 *   - `phi` (Hand-Azimut) ist **nicht frei** — er ist an die Position gekoppelt
 *     und wird in der Inversen aus `atan2(y, x)` abgeleitet. Ein als Eingabe
 *     gesetztes `this.phi` wird in {@link calculateAngles3D} **ignoriert** und in
 *     {@link calculatePositionFromMotorAngles} als Ergebnis überschrieben.
 *   - Frei wählbar sind: Position `x,y,z`, Hand-Neigung `theta` (Polarwinkel von
 *     +Z, gleiche Konvention wie Arm3SegmentLinearX) und Hand-Rotation `psi`.
 *
 * ───────────────────────────────────────────────────────────────────────────
 * Geometrie-Modell / Annahmen:
 *   - Schultergelenk sitzt auf der Basis-Drehachse, Höhe `baseHeight` über der
 *     Grundplatte (radialer Versatz = 0 — vereinfachende Annahme).
 *   - Winkel `alpha`, `beta`, `a` werden intern als Elevation gegen die
 *     Horizontale geführt; `alpha` ist absolut, `beta`/`a` relativ zum
 *     vorhergehenden Segment.
 *   - Es wird genau **ein** Ellbogen-Zweig gewählt (`beta` ∈ [0, π]).
 *   - Gelenk-Endanschläge werden **nicht** geprüft (wie bei Arm3SegmentLinearX).
 *
 * ⚠️ Die Default-Längen sind **Schätzwerte**, abgeleitet aus den
 * veröffentlichten Maßen (Σ Segmente ≈ Reichweite 300 mm, `baseHeight` ≈
 * Höhe 420 mm − Reichweite). Für den Echtbetrieb am physischen Arm sollten sie
 * **kalibriert** und per `ROBOT_KINEMATICS_PARAMS` gesetzt werden.
 */
class Arm3SegmentRotaryBase extends RobotBase {

    /**
     * @param {number} [l1=105] Länge des Oberarms (Schulter→Ellbogen) in mm
     * @param {number} [l2=98]  Länge des Unterarms (Ellbogen→Handgelenk) in mm
     * @param {number} [l3=100] Länge der Hand (Handgelenk→Greiferspitze) in mm
     * @param {Object} [params] zusätzliche Parameter
     * @param {number} [params.baseHeight=110] Höhe der Schulterachse über der Basis in mm
     */
    constructor(l1 = 105, l2 = 98, l3 = 100, params = {}) {
        super();

        /** @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;
        /** @type {number} Höhe der Schulter-Drehachse über der Grundplatte in mm */
        this.baseHeight = (params && params.baseHeight != null) ? params.baseHeight : 110;

        // Neutrale Default-Pose: Hand horizontal nach vorn (theta = π/2 = Polarwinkel).
        this.theta = Math.PI / 2;
    }

    /**
     * Inverse Kinematik: Workspace (x, y, z, theta, psi, e) → Gelenkwinkel.
     * `phi` wird ignoriert und aus der Position abgeleitet (5-DOF, siehe Klassendoc).
     */
    calculateAngles3D(verbose) {
        // Winkel normalisieren (Konsistenz mit Arm3SegmentLinearX)
        while (this.theta >  Math.PI) { this.theta -= 2 * Math.PI }
        while (this.theta < -Math.PI) { this.theta += 2 * Math.PI }

        // 1) Basis-Drehung aus dem Azimut der Zielposition (phi ist gekoppelt)
        const q1 = Math.atan2(this.y, this.x);
        this.xMotor = q1;
        this.phi = q1;

        // 2) Hand-Vektor abziehen → Handgelenk-Punkt
        //    theta = Polarwinkel von +Z. Hand-Radial = l3·sin(theta), Hand-Z = l3·cos(theta).
        const st = Math.sqrt(this.x * this.x + this.y * this.y); // radialer Abstand der Spitze
        const zt = this.z;

        const sw = st - this.l3 * Math.sin(this.theta); // radial: Handgelenk
        const zw = zt - this.l3 * Math.cos(this.theta); // Höhe:   Handgelenk

        // Handgelenk-Punkt in 3D (für Speed-Berechnung)
        this.pX = sw * Math.cos(q1);
        this.pY = sw * Math.sin(q1);
        this.pZ = zw;

        // 3) 2-Gelenk-IK in der Armebene: von Schulter (0, baseHeight) zum Handgelenk (sw, zw)
        const dx = sw;
        const dz = zw - this.baseHeight;
        const R2 = dx * dx + dz * dz;
        const R = Math.sqrt(R2);

        if (R > (this.l1 + this.l2)) { return; } // unerreichbar — Slots unverändert lassen
        if (R === 0) { return; }

        // Ellbogen-Relativwinkel (ein Zweig: qe ∈ [0, π])
        let cosE = (R2 - this.l1 * this.l1 - this.l2 * this.l2) / (2 * this.l1 * this.l2);
        cosE = Math.max(-1, Math.min(1, cosE));
        const qe = Math.atan2(Math.sqrt(1 - cosE * cosE), cosE);

        // Absoluter Oberarmwinkel (Elevation gegen Horizontale)
        const A2 = Math.atan2(dz, dx) - Math.atan2(this.l2 * Math.sin(qe), this.l1 + this.l2 * Math.cos(qe));
        const A3 = A2 + qe;                       // absoluter Unterarmwinkel
        const A4 = Math.PI / 2 - this.theta;      // absolute Hand-Elevation aus Polarwinkel

        // 4) Motor-Slots schreiben
        this.alpha  = A2;          // Schulter (absolut)
        this.beta   = qe;          // Ellbogen (relativ)  = A3 − A2
        this.a      = A4 - A3;     // Handgelenk-Pitch (relativ)
        this.b      = this.psi;    // Handgelenk-Roll
        this.c      = 0;           // ungenutzt
        this.eMotor = this.e;      // Greifer

        // a auf [-π, π] normalisieren
        while (this.a >  Math.PI) { this.a -= 2 * Math.PI }
        while (this.a < -Math.PI) { this.a += 2 * Math.PI }

        if (verbose) {
            console.log("IK GrabIt: q1=", q1.toFixed(3), "alpha=", this.alpha.toFixed(3),
                "beta=", this.beta.toFixed(3), "a=", this.a.toFixed(3), "b=", this.b.toFixed(3));
        }
    }

    /**
     * Vorwärts-Kinematik: Gelenkwinkel → Workspace (x, y, z, phi, theta, psi, e).
     * Exakte Umkehrung von {@link calculateAngles3D}.
     */
    calculatePositionFromMotorAngles(verbose = false) {
        const q1 = this.xMotor;          // Basis-Yaw
        const A2 = this.alpha;           // Oberarm absolut
        const A3 = this.alpha + this.beta;       // Unterarm absolut
        const A4 = this.alpha + this.beta + this.a; // Hand absolut (Elevation)

        // Handgelenk-Punkt in der Armebene
        const sw = this.l1 * Math.cos(A2) + this.l2 * Math.cos(A3);
        const zw = this.baseHeight + this.l1 * Math.sin(A2) + this.l2 * Math.sin(A3);

        // Greiferspitze in der Armebene (Hand-Elevation A4)
        const st = sw + this.l3 * Math.cos(A4);
        const zt = zw + this.l3 * Math.sin(A4);

        // In den 3D-Raum drehen
        this.pX = sw * Math.cos(q1);
        this.pY = sw * Math.sin(q1);
        this.pZ = zw;

        this.x = st * Math.cos(q1);
        this.y = st * Math.sin(q1);
        this.z = zt;

        this.phi   = q1;                    // Azimut = Basis-Drehung
        this.theta = Math.PI / 2 - A4;      // Polarwinkel von +Z
        this.psi   = this.b;                // Hand-Roll

        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 }

        if (verbose) {
            console.log("FK GrabIt: x=", this.x.toFixed(2), "y=", this.y.toFixed(2),
                "z=", this.z.toFixed(2), "phi=", this.phi.toFixed(3), "theta=", this.theta.toFixed(3));
        }
    }
}


module.exports = Arm3SegmentRotaryBase