//library for servo
#include <Servo.h>
//declaration of digital pins for stepper motor 1
const int pinStep = 5;
const int pinDir = 4;
//declaration of digital pins for stepper motor 1
int trigPin = 11;      // trig pin датчика HC-SR04
int echoPin = 10;     // Echo pin датчика HC-SR04

//declaration of pins for ultrasonics distance sensor
//int trigPin = 2;     
//int echoPin = 3; 
long duration, distance;
int i;
const int move_delay = 3;
int steps_rotate_360;

const float Pi = 3.14159;
float Arad;
int A;
float XY;
float FullRotate;
Servo servo;
int A_90=90;
int A_turn=90;
int t=0;
int t1;
int var;
//int distance = 30;
//float duration;


void setup()
{
    Serial.begin(9600); 
    servo.attach(9);

    pinMode(pinStep, OUTPUT);
    pinMode(pinDir, OUTPUT);
    
    digitalWrite(pinStep, HIGH);
    digitalWrite(pinDir, HIGH);
 
    pinMode(trigPin, OUTPUT);         // set trig pin as output
    pinMode(echoPin, INPUT);    
    
    }


  

void loop()
{ 

if (Serial.available() > 0) {
    String bufString = Serial.readString();       // читаем как строку
    byte dividerIndex = bufString.indexOf(';');   // ищем индекс разделителя
    String buf_1 = bufString.substring(0, dividerIndex);    // создаём строку с первым числом
    String buf_2 = bufString.substring(dividerIndex + 1);   // создаём строку со вторым числом
    float val_1 = buf_1.toFloat();    // преобразуем во флоат
    float val_2 = buf_2.toFloat();    // ...
    Serial.println(val_1);  // проверка
    Serial.println(val_2);  // ...
 float X=val_1;
float Y=val_2;
     // ...
    if(X==0){
        XY=Y;
        A=90;
            }
    else{
         Arad=atan(Y/X);
         A=Arad*180/Pi;
         XY = X/cos(Arad);
            }
    if (A<0) {A=A+180;}
    FullRotate = XY/16;
    steps_rotate_360 =FullRotate*200;


  Serial.print("angle of XY");
  Serial.print("\t");
  Serial.print(A);
  Serial.print("\t");
  Serial.print("distance to point");
  Serial.print("\t");
  Serial.print(XY);        
  Serial.print("\t");
  Serial.print("steps to get to coordinate");
  Serial.print("\t");
  Serial.print(steps_rotate_360);
  Serial.print("\t");




  servo.write(90);
 
  if(A<90) var=1;

  else if (A>90) 
  {
  var=2;
  }

  else {var=3;}

switch (var) {
  //------------------------------------Case 1--------------------------------
  case 1:
   do{
   // for (int i = 0;  < 10; i++){
        servo.write(A_90); 
        for(int u = 0; u < 10; u++)
          { digitalWrite(pinStep, HIGH);
            delay(move_delay);
            digitalWrite(pinStep, LOW);
            delay(move_delay);
          }
        A_90=A_90-3;
       delay(500);
      }  
    while (A_90>A);
  break;


  //------------------------------------Case 2--------------------------------
  case 2:
   do{
    //  for (int i = 0; i < 10; i++){
        servo.write(A_90); 
    //digitalWrite(pinDir, HIGH);
        for(int u = 0; u < 10; u++)
          { digitalWrite(pinStep, HIGH);
            delay(move_delay);
            digitalWrite(pinStep, LOW);
            delay(move_delay);
          }
        A_90=A_90+3;
       delay(500);
      }   
    while (A_90<A);
    break;

  //--------------------------------Going to point----------------------------
 
}





 do{
//********************************************************************************************
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH); 
  distance = duration / 58.2;   
  delay(10);
Serial.print(t);

   if (distance > 19)
  { 
  if(t==0) {t1=2;}
  else if (t>0) {t1=1;}
 
switch(t1){

case 1:
    servo.write(A_turn);
 
    for(int u = 0; u < 10; u++)
          { digitalWrite(pinStep, HIGH);
            delay(move_delay);
            digitalWrite(pinStep, LOW);
            delay(move_delay);
          }
     A_turn=A_turn-5;
     t=t-1;
    delay(500);
 break;
 

 case 2:

    servo.write(90);
     for(int i = 0; i < 100; i++)
          { digitalWrite(pinStep, HIGH);
            delay(move_delay);
            digitalWrite(pinStep, LOW);
            delay(move_delay);
          }
     steps_rotate_360=steps_rotate_360-100;  
 break;
}
  }
   else if (distance < 19)  
   {Serial.print("obsticles");
    servo.write(A_turn); 
    for(int u = 0; u < 10; u++)
          { digitalWrite(pinStep, HIGH);
            delay(move_delay);
            digitalWrite(pinStep, LOW);
            delay(move_delay);
          }
     A_turn=A_turn+5;
     t++;
     delay(500);
    }   
 
 //******************************************************************************************** }

   }
  while(steps_rotate_360>0);


















    }



}