#include <ESP32Servo.h>
#include <math.h>

const int servoPin1 = 18;//eye LR
const int servoPin2 = 19;// neck U-D
const int servoPin3 = 21;// neck LR
const int servoPin4= 22;// eye U-D
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

void setup() {
  //  attach servo pin with a min/max timings of 500 microseconds and 2400 microseconds
  servo1.attach(servoPin1, 500, 2400);
  servo2.attach(servoPin2, 500, 2400);
  servo3.attach(servoPin3, 500, 2400);
  servo4.attach(servoPin4, 500, 2400);
  
}

//  initialize position of 0 degrees
float pos1 = 0;
float pos2=110;
int pos3=60; 
int x=40;
int y=-24;
int z=67;
float a1, a2;

void idle(){
servo2.write(110);
servo3.write(60);
    //  for loop that increases the angle by 1 from 0 to 180 with a delay of 10 ms per angle
  for (pos1 = 45; pos1 <= 135; pos1 +=0.25 ) {
    servo1.write(pos1);
    delay(5);
  }

  //  for loop that decreases the angle by 1 from 180 to 0 with a delay of 10 ms per angle
  for (pos1 = 135; pos1 >= 45; pos1 -= 8) {
    servo1.write(pos1);
    delay(10);
  }
  
  pos2+=4;
  servo2.write(pos2);
  if(pos2==150){
    pos2=110;
    if(pos3==60){
      pos3=120;
    }
    else{
      pos3=60;
    }
      servo3.write(pos3);
    }
    
    
  }

void nonidle(){

int b1=(asin(abs(y/z))*180/(4*atan(1)));
 a1=90+ (asin(x/(z*cos(b1)))*180/(4*atan(1)));
 servo1.write(a1);
 int b2=(asin(abs(x/z))*180/(4*atan(1)));
 a2=90+ (asin(y/(z*cos(b2)))*180/(4*atan(1)));
 servo4.write(a2);
 /*servo4.write(a2);
 if(x>300){
  servo3.write(300);
  servo1.write(a1);
 }
 else if(x<-300){
  servo3.write(-300);
  servo1.write(a1);
  }
 else 
 servo1.write(a1);


   if(y>300){
  servo3.write(300);
  servo1.write(a2);
 }
 else if(y<-300){
  servo3.write(-300);
  servo1.write(a2);
  }
  else 
  servo1.write(a2);
  
 
 
  
    
 */

}

void loop() {
 nonidle();
    
}