// mxengineer
// Pin Connections:
// Wing Left Servo Signal -> Pin 3
// Wing Right Servo Signal -> Pin 5
// Button -> Pin 2
// Servo VCC -> External 5V Power Supply
// Servo GND -> Common GND with Arduino

#include <Servo.h>

Servo wingLeft;
Servo wingRight;

const int pinWingLeft = 3;
const int pinWingRight = 5;
const int buttonPin = 2;

const int ANGLE_REST = 180;
const int ANGLE_MAX_UP = 30;
const int ANGLE_FLAP_UP = 50;
const int ANGLE_FLAP_DOWN = 160;

bool isFlying = false;
bool lastButtonState = HIGH;
bool isTakingOff = false;
bool isFlapping = false;

unsigned long prevMillis = 0;
int flapStep = 0;
int takeOffStep = 0;

void writeRightWing(int angle) {
  wingRight.write(180 - angle);
}

void setup() {
  wingLeft.attach(pinWingLeft);
  wingRight.attach(pinWingRight);
  goToRest();
  pinMode(buttonPin, INPUT_PULLUP);
}

void loop() {
  bool currentButtonState = digitalRead(buttonPin);
  
  if (currentButtonState == LOW && lastButtonState == HIGH) {
    delay(50);
    
    if (isFlying) {
      isFlying = false;
      isTakingOff = false;
      isFlapping = false;
      goToRest();
      delay(200);
      wingLeft.detach();
      wingRight.detach();
    } 
    else {
      isFlying = true;
      isTakingOff = true;
      isFlapping = false;
      
      if (!wingLeft.attached()) wingLeft.attach(pinWingLeft);
      if (!wingRight.attached()) wingRight.attach(pinWingRight);
      
      takeOffStep = 0;
      prevMillis = millis();
    }
  }
  lastButtonState = currentButtonState;
  
  if (isFlying) {
    if (isTakingOff) {
      takeOff();
    } 
    else if (isFlapping) {
      flapWings();
    }
  }
}

void takeOff() {
  unsigned long currentMillis = millis();
  
  switch (takeOffStep) {
    case 0:
      if (currentMillis - prevMillis >= 20) {
        static int angle = ANGLE_REST;
        angle = angle - 5;
        if (angle <= ANGLE_MAX_UP) {
          angle = ANGLE_MAX_UP;
          wingLeft.write(angle);
          writeRightWing(angle);
          takeOffStep = 1;
          prevMillis = currentMillis;
        } else {
          wingLeft.write(angle);
          writeRightWing(angle);
          prevMillis = currentMillis;
        }
      }
      break;
      
    case 1:
      if (currentMillis - prevMillis >= 500) {
        takeOffStep = 2;
        prevMillis = currentMillis;
      }
      break;
      
    case 2:
      isTakingOff = false;
      isFlapping = true;
      flapStep = 0;
      prevMillis = currentMillis;
      break;
  }
}

void flapWings() {
  unsigned long currentMillis = millis();
  
  if (currentMillis - prevMillis >= 180) {
    prevMillis = currentMillis;
    
    if (flapStep == 0) {
      wingLeft.write(ANGLE_FLAP_UP);
      writeRightWing(ANGLE_FLAP_UP);
      flapStep = 1;
    } 
    else {
      wingLeft.write(ANGLE_FLAP_DOWN);
      writeRightWing(ANGLE_FLAP_DOWN);
      flapStep = 0;
    }
  }
}

void goToRest() {
  wingLeft.write(ANGLE_REST);
  writeRightWing(ANGLE_REST);
  delay(200);
}