#include <ESP32Servo.h>

uint8_t speedEyes = 100;
uint8_t speedBeak = 100;
uint8_t speedNeck = 100;
uint8_t speedWings = 100;

unsigned long previousTimeEyes = 0;
unsigned long previousTimeBeak = 0;
unsigned long previousTimeNeck = 0;
unsigned long previousTimeWings = 0;

uint8_t currentPosEyes = 0;           //aktueller Servowinkel
uint8_t aimPosEyes = 180;             //Zielwinkel
uint8_t currentPosBeak = 0;
uint8_t aimPosBeak = 180;
uint8_t currentPosNeck = 0;
uint8_t aimPosNeck = 180;
uint8_t currentPosWings = 0;
uint8_t aimPosWings = 180;

unsigned long previousTimePrint = 0;

Servo s1;
Servo s2;
Servo s3;
Servo s4;

void setup(){

  Serial.begin(115200);
  s1.attach(2);
  s2.attach(4);
  s3.attach(5);
  s4.attach(18);

}

void moveEyes(){

  unsigned long currentTime = millis();
  
  if(currentTime - previousTimeEyes >= 1000/speedEyes){    
    previousTimeEyes = currentTime;
    
    if(currentPosEyes != aimPosEyes){
      if(currentPosEyes < aimPosEyes){
        currentPosEyes += 1;
      } else {
        currentPosEyes -= 1;
      }
    } else {
      aimPosEyes = random (0,180);
      speedEyes = random (30,200);
    }  
  }
}

void moveBeak(){

  unsigned long currentTime = millis();
  
  if(currentTime - previousTimeBeak >= 1000/speedBeak){    
    previousTimeBeak = currentTime;
    
    if(currentPosBeak != aimPosBeak){
      if(currentPosBeak < aimPosBeak){
        currentPosBeak += 1;
      } else {
        currentPosBeak -= 1;
      }
    } else {
      aimPosBeak = random (0,180);
      speedBeak = random (30,200);
    }  
  }
}

void moveNeck(){

  unsigned long currentTime = millis();
  
  if(currentTime - previousTimeNeck >= 1000/speedNeck){    
    previousTimeNeck = currentTime;
    
    if(currentPosNeck != aimPosNeck){
      if(currentPosNeck < aimPosNeck){
        currentPosNeck += 1;
      } else {
        currentPosNeck -= 1;
      }
    } else {
      aimPosNeck = random (0,180);
      speedNeck = random (30,200);
    }  
  }
}

void moveWings(){

  unsigned long currentTime = millis();
  
  if(currentTime - previousTimeWings >= 1000/speedWings){    
    previousTimeWings = currentTime;
    
    if(currentPosWings != aimPosWings){
      if(currentPosWings < aimPosWings){
        currentPosWings += 1;
      } else {
        currentPosWings -= 1;
      }
    } else {
      aimPosWings = random (0,180);
      speedWings = random (30,200);
    }  
  }
}


void printSeconds(){

  unsigned long currentTime = millis();
  
  if(currentTime - previousTimePrint >= 1000){
     previousTimePrint = currentTime;
     Serial.println(millis()/1000);
 
  }
  
}



void loop(){

  moveEyes();
  moveBeak();
  moveNeck();
  moveWings();
  
  printSeconds();

  s1.write(currentPosEyes);
  s2.write(currentPosBeak);
  s3.write(currentPosNeck);
  s4.write(currentPosWings);

}