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

class Robot{

    constructor(l1, l2, l3) {
        this.speedX = 100;  // mm/min 
        this.speedY = 100;   
        this.speedZ = 100;
    
        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.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: Handgelenk Punkt (nur für Tests sind die Public)
        this.Hz = 0.0;
        this.Hr = 0.0;

        this.OH = 0.0;
        this.epsilon = 0.0;

        this.r = 0.0; // Abstand in der XY-Ebene
        this.alpha = 0.0;
        this.beta = 0.0;    

        // Motor-Koordinaten in Grad
        this.aMotor = 0; // Dreher
        this.bMotor = 0; // Schulter
        this.cMotor = 0; // Ellebogen 
        this.dMotor = 0; // Handgelenk Knick
        this.eMotor = 0; // handgelenk Drehung
        this.fMotor = 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.phi = Math.atan2(this.y, this.x);
        this.r = Math.sqrt(this.x*this.x + this.y*this.y);

        this.Hr = this.r - this.l3*Math.cos(this.theta);
        this.Hz = this.z + this.l3*Math.sin(this.theta);

        this.epsilon = Math.atan2(this.Hz, this.Hr);
        this.OH = Math.sqrt(this.Hr*this.Hr + this.Hz*this.Hz);

        // Winkel für die Oberarm und Unterarm Dreiecksberechnung
        this.alpha = Math.acos((this.l1*this.l1 + this.OH*this.OH - this.l2*this.l2) / (2*this.l1*this.OH));
        this.beta  = Math.acos((this.l1*this.l1 + this.l2*this.l2 - this.OH*this.OH) / (2*this.l1*this.l2));        
        
        this.eMotor = this.e;
    }

    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