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

class Robot{


    constructor(l1, l2, l3) {
        this.speedX = 200;  // mm/min 
        this.speedY = 200;   
        this.speedZ = 200;
    
        this.lastCommandSend = 0;

        if(this.lastCommandSend == 0){ this.lastCommandSend = Date.now() };
        this.doAnimate = false;

        this.l1 = l1;   // Oberarm
        this.l2 = l2;   // Unterarm
        this.l3 = l3;   // Hand-Länge

        // Plan-Koordinaten - XYZ FingerSpitze
        this.x = 0;
        this.y = 0;
        this.z = 0;

        // Plan-Koordinaten - HandRichtung
        this.phi = 0.0;   // Euler-Winkel zwischen X-Achse  - Laengengrad
        this.theta = -Math.PI/2; // Euler-Winkel zwischen Z-Achse und P - Breitengrad
        this.psi = 0.0;   // Euler-Winkel: Drehung des Handgelenks abweichend vom Breitengrad

        this.e = 0.0 // Finger-Distance

        // Zwischen-Ergebnisse: Hand Punkt (nur für Tests sind die Public)
        this.pX = 0.0;
        this.pY = 0.0;
        this.pZ = 0.0;

        // Motor-Koordinaten - Schulter, Ellebogen, Hand-Dreher
        this.xMotor = 0;
        this.alpha = 0; // =Y Motor
        this.beta = 0;  // =Z Motor = Winkel die der Unterarm unter der Y-Achse ist. 

        // Motor-Winkel fuer's Handgelenk
        this.a = 0;     // aMotor am Ellebogen
        this.b = 0;     // bMotor Handgelenk-Knicker
        this.c = 0;     // cMotor Hand-Dreher

        this.eMotor = 0; // eMotor Finger-Distanz


	    this.oldCommandTime = Date.now();
        this.showFunctions = [];

        this.savedPoints = [];
        this.atPointNr = 0;
        this.t = 0;         // TimeStamp of this Point

        this.moveRelative = true;

        this.pythonSender = null;
        this.cmdReceivers = [];
    }

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

    // Berechnet aus XYZ die Motor-Winkel für den GCode
    calculateAngles3D(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.asin(pZ / r);
        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 ist zwischen Arm und Punkt-O  
        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.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.e - this.b - this.c;
    }
    
    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)
        };
    }

    calculatePositionFromMotorAngles(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);

        const vHand = this.rotateAroundAxis(vecUnterarm, n, 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}
    }
    
    sendCommand(){
        if(this.motorPosition == null){this.createMotorPosition() }
        this.motorPositionOld = this.motorPosition;
        this.createMotorPosition()

        console.log("Robot.sendCommand: 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.moveTo(this.motorPositionOld, this.motorPosition);
        });
    }
}



module.exports = Robot // Export class