float A_motor = 0;
float B_motor = 0;
float C_motor = 0;
float D_motor = 0;

int A_out = 0;
int B_out = 0;
int C_out = 0;
int D_out = 0;

float absAngle = 0;

void setup() {
  Serial.begin(115200);


  Serial.print("abszolut szog: ");Serial.print(absAngleCalc(4,2),5); Serial.print("  Vektorhossz: "); Serial.println(vektorLength(4,2),5);
  float absAngle = absAngleCalc(4,2);
  motorRateCalc(absAngle);
}

void loop() {
   
  
 
 
}

float vektorLength(float x, float y){
    float forceVal = sqrt(x*x+y*y);
    return forceVal;
}

void intFullScale(){
    for(int i = 0;i< 360; i++){
    Serial.print(i); Serial.print(" ");
    motorRateCalc(i);
    delay(30);
  }
}



float absAngleCalc(float x, float y){
  float absAngle = 0;
  //1. lépés abszolut szög meghatározása x tengelyhez képest:

  absAngle = atan(y/x)*(180/PI);
  if(x < 0){
    absAngle = absAngle + 180;
  }
  if(absAngle < 0){
    absAngle = absAngle + 360;
  }
  if(absAngle == 360){
    absAngle = 0;
  }
  //motorRateCalc(absAngle);
  return absAngle;
}





void motorRateCalc(float absAngle){
  if((315 <= absAngle && absAngle < 360)||(0 <= absAngle && absAngle < 45)){
    
    if(315 <= absAngle && absAngle < 360){
        absAngle = (absAngle - 360);
    }
      absAngle = absAngle + 45;
      Serial.print(absAngle); 
      B_motor = (absAngle / 90);
      C_motor = 1-B_motor;
      Serial.print(" B_motor: "); Serial.print(B_motor,4); Serial.print("% ");
     Serial.print(" C motor: "); Serial.print(C_motor,4); Serial.println("% ");

  }
  else if((45 <= absAngle && absAngle < 135)){
    absAngle = absAngle - 45;
      Serial.print(absAngle); 
      A_motor = (absAngle / 90);
      B_motor = 1-A_motor;
      Serial.print(" A_motor: "); Serial.print(A_motor); Serial.print("% ");
      Serial.print(" B_motor: "); Serial.print(B_motor); Serial.println("% ");
      
  }
  else if((135 <= absAngle && absAngle < 225)){
     absAngle = absAngle - 135;
      Serial.print(absAngle); 
      D_motor = (absAngle / 90);
      A_motor = 1-D_motor;
      Serial.print(" D_motor: "); Serial.print(D_motor); Serial.print("% ");
      Serial.print(" A_motor: "); Serial.print(A_motor); Serial.println("% ");
  }
  else if((225 <= absAngle && absAngle < 315)){
      absAngle = absAngle - 225;
      Serial.print(absAngle); 
      C_motor = (absAngle / 90);
      D_motor = 1-C_motor;
      Serial.print(" C_motor: "); Serial.print(C_motor); Serial.print("% ");
      Serial.print(" D_motor: "); Serial.print(D_motor); Serial.println("% ");
      
  }
}