#include <Servo.h>
#define stepPin 20
#define dirPin 21

Servo servo1;
Servo servo2;
Servo servo3;

const int kar1 = 60.0000;
const int kar2 = 50.0000;
int corx;
int corz;
int cory;
int corxsq;
int corysq;
int distance;
int angler;
int angled;
int ra;
int ra2;
int angler2;
int angled2;
int sra;
int ra3;
int angler3;
int angled3;
int angle1;
int angle2;
int pos1;
int pos2;
int fusiona1;
int fusiona2;
int posz;
int fusionaz;
int poszs;
int x = 0;
int y = 3;
int length;
int ra4;
int angler4;
int angled4;
int angled4m;
int angled4sq;
int angled4sqrt;

void setup() {

Serial.begin(9600);
//=======================================
corx = 80.0000;
corz = 40.0000;
cory = 20.0000;
//=======================================
servo1.attach(44);
servo2.attach(45);
servo3.attach(46);
pinMode(stepPin,OUTPUT); 
pinMode(dirPin,OUTPUT);

}

void loop() {


 float length = sqrt(sq(corx)+sq(cory));               //length real    
 float distance = sqrt(sq(length)+sq(corz)); 
 float ra4 = cory/length;                              //angle alpha''
 float angler4 = asin(ra4);
 float angled4 = angler4*180/3.1416;
 float angled4sq = sq(angled4);
 float angled4sqrt = sqrt(angled4sq);
 float angled4m = angled4sqrt + 1;
 Serial.println(angled4);
 Serial.println(angled4m);

 if (distance < 110){

 Serial.print("distance: ");                          //length c
 Serial.println(distance,4);
 float ra = ((sq(distance)-(sq(kar1)+sq(kar2)))/(-2*kar1*kar2));
 Serial.println(ra);
 float angler = acos(ra);                                           //angle sigma
 float angled = angler*180/3.1416;
 Serial.println(angled);
 float ra2 = corz/distance/1;
 float angler2 = asin(ra2);                                         //angle alpha'
 float angled2 = angler2*180/3.1416;
 Serial.println(angled2);
float sra = asin(ra);                                                //angle alpha
 float ra3 = ((sq(kar2)-(sq(kar1)+sq(distance)))/(-2*kar1*distance));
 float angler3 = acos(ra3);
 float angled3 = angler3*180/3.1416;
 Serial.println(angled3);

 angle1 = angled2+angled3;                                            //cyan servo
 angle2 = angled;                                                     //blue servo

 Serial.println(angle1);
 Serial.println(angle2);

if (cory > 0 && corx > 0){
  posz = angled4m;
}

if (corx < 0 && cory > 0){
  posz = angled4m + 90;
}

if (cory < 0 && corx > 0){
  posz = angled4m + 270;
}

if (cory < 0 && corx < 0){
  posz = angled4m + 180;
}

 servo1.write(posz);
 Serial.print("servo0 angle: ");
  Serial.print(posz);

pos1 = 180-angle1; 
  Serial.print("    servo1 angle: ");
  Serial.print(pos1);
  servo2.write(pos1);
pos2 = 180-angle2;
  Serial.print("    servo2 angle:");
  Serial.println(pos2);
  servo3.write(pos2);

fusiona1 = map(pos1,0,180,-90,90);
fusiona2 = map(pos2,0,180,90,-90);
fusionaz = map(posz,0,180,-90,90);
Serial.print("fusion servoZ angle: ");
 Serial.print(fusionaz);
Serial.print("    fusion servo1 angle: ");
 Serial.print(fusiona1);
 Serial.print("    fusion servo2 angle: ");
 Serial.println(fusiona2);

//=========================================================================
poszs = map(posz,0,360,2,202);

poszs *= 1;                                //rotation multiplier

if (x < poszs){digitalWrite(dirPin,HIGH); // Enables the motor to move in a particular direction
  for(x=0; x < poszs; x++) {
    digitalWrite(stepPin,HIGH); 
    delayMicroseconds(2500);    // by changing this time delay between the steps we can change the rotation speed
    digitalWrite(stepPin,LOW); 
    delayMicroseconds(2500); 
  }}
  
if (y > poszs && y<403) {digitalWrite(dirPin,LOW); // Enables the motor to move in a particular direction
  for(y=3; y > poszs && y<403; y++) {
    digitalWrite(stepPin,HIGH); 
    delayMicroseconds(2500);    // by changing this time delay between the steps we can change the rotation speed
    digitalWrite(stepPin,LOW); 
    delayMicroseconds(2500); 
  }}

 }
 else Serial.println("=====invalid value=====");

delay(200);
}