#include <Servo.h> 

// For the stepper motor
#define DIR_PIN  2    // The direction pin controls the direction of stepper motor rotation.
#define STEP_PIN 3    // Each pulse on the STEP pin moves the stepper motor one angular unit.
#define STEPS    20//800   // Number of steps per   

// For the servos
#define LEFT_WING_SERVO  9
#define RIGHT_WING_SERVO 10
#define DELAY            1000 // milliseconds

Servo leftWing;
Servo rightWing;  

// Servo Positions
int leftWingPos = 0;
int rightWingPos = 0;

// Min/Max Angles
const int leftMinAngle = 13;
const int leftMaxAngle = 135;
//const int rightMinAngle = 10;
//const int rightMaxAngle = rightMinAngle + (leftMaxAngle - leftMinAngle);

// Delays
const int msInterDelay = 500;
const int msSpinDelay = 50;  // 15

int pos = 0;

// For the stepper
const long usDelay = 1000000000;
long dir = HIGH;

/* For Serial Comms */
String inputString = "";         // a string to hold incoming data
boolean stringComplete = false;  // whether the string is complete

boolean threatened = false;
unsigned long prev = 0;

// Initialize and put everything in the base configuration
void setup() {
  /* STEPPER */
  pinMode(DIR_PIN, OUTPUT); 
  pinMode(STEP_PIN, OUTPUT);
  digitalWrite(DIR_PIN, dir);

  /* SERVO */
  leftWing.attach(LEFT_WING_SERVO);
  rightWing.attach(RIGHT_WING_SERVO);

  // Put wings in folded configuration
  leftWing.write(leftMaxAngle);
  rightWing.write(0);

  /* Serial Comms */
  Serial.begin(9600);
  inputString.reserve(200);
}

// Actuate the wings
void wingControl(char state) {
  const int ps = 3;
  switch (state) {
    case 'e':
      Serial.print("EXTEND\n");
      for (pos = leftMinAngle; pos <= leftMaxAngle; pos += ps) {
        leftWing.write(pos);
        rightWing.write(pos);
        delay(msSpinDelay);
      }
      break;
    case 'r':
      for (pos = leftMaxAngle; pos >= leftMinAngle; pos -= ps) {                                
        leftWing.write(pos);
        rightWing.write(pos); // 20
        delay(msSpinDelay);
      }
      break;
    default:
      break;
  }
}

// Actuate the neck
void neckControl(int angle) {

  // Set up direction of motion
  int default_dir = LOW;
  if (angle < 0) {
    digitalWrite(DIR_PIN, default_dir);
  } else {
    digitalWrite(DIR_PIN, !default_dir);
  }
  
  for (int i = 0; i < abs(angle); i++) {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(usDelay);
    
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(usDelay);
  }
}

void test_stepper() {

  digitalWrite(DIR_PIN, HIGH);
  
  for (int i = 0; i < 100; i++) {
    //Serial.print("HERE");
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(usDelay);
    
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(usDelay);
  }
}//*/

void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

void loop() {
  //test_stepper();
  serialEvent();
  if (stringComplete) {
    if (inputString.equals("threatened\r\n") || inputString.equals("threatened")) {
      if (!threatened) {
          Serial.print("THREATENED MODE\n");
          threatened = true;
          wingControl('e'); // EXTEND WINGS
          prev = millis();
      } else {
        unsigned long curr = millis();
        if (curr - prev > DELAY)  {
          Serial.print("RESETTING\n");
          threatened = false;
          wingControl('r'); // RETRACT WINGS
          //prev = curr;
        }
      }
    }//*/
    Serial.print(inputString.toInt());
    Serial.print("\n");
    neckControl(inputString.toInt());
    inputString = "";       // clear the string
    stringComplete = false; // reset the conditional
  }
}