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(){

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

        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;

    }

    calculatePositionFromMotorAngles(){
        
    }

    sendCommand(){
        if(this.motorPosition == null){this.createMotorPosition() }
        this.motorPositionOld = this.motorPosition;
        this.createMotorPosition()

        console.log("Robot.sendCommand: neue RobotMotorPosition: 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