#include <Servo.h> // variable speed servo library.
#include <SoftwareSerial.h> // TX RX software library for bluetooth.

const byte BLUETOOTH_TX_PIN = 3; // bluetooth Tx to pin 3.
const byte BLUETOOTH_RX_PIN = 2; // bluetooth Rx to pin 2.

const byte LEFT_SERVO_PIN = 6;   // left servo to pin 9.
const byte RIGHT_SERVO_PIN = 7; // right servo Rx to pin 10.

const byte OPEN_BUTTON_PIN = A1; // open switch to pin 6.
const byte CLOSE_BUTTON_PIN = A2; // close switch to pin 7.

const byte STATUS_LED_PIN = 11; // close switch to pin 11.

// Settings //

byte leftServoAngle = 180;  // initial servo angle degrees 0-180.
byte rightServoAngle = 0; // initial servo angle degrees 0-180.

byte slowSpeed = 50; // slow speed 0-255.
byte fastSpeed = 255; // fast speed 0-255.

byte maxServoAngle = 180; // max servo angle degrees 0-180.
byte midServoAngle = 80;  // mid servo angle degrees 0-180.
byte minServoAngle = 0;   // min servo angle degrees 0-180.

byte openDelay = 0; // open delay between left and right servos in ms.
byte closeDelay = 0; // close delay between left and right servos in ms
byte blinkDelay = 200;

byte servoSteps = 2; // servo inching degrees 0-180.

const int BUTTON_LONG_PRESS = 500; // switch long press in ms.

// End Settings //

SoftwareSerial bluetooth(BLUETOOTH_TX_PIN, BLUETOOTH_RX_PIN);

Servo leftServo;
Servo rightServo;

byte ledPinState = 0;

void setup()
{
  Serial.begin(9600); // start serial.

  bluetooth.begin(9600); // start bluetooth serial.

  pinMode(OPEN_BUTTON_PIN, INPUT_PULLUP);
  pinMode(CLOSE_BUTTON_PIN, INPUT_PULLUP);

  pinMode(STATUS_LED_PIN, OUTPUT);

  leftServo.attach(LEFT_SERVO_PIN);
  rightServo.attach(RIGHT_SERVO_PIN);

  leftServo.writeMicroseconds(10000);
  leftServo.write(leftServoAngle);
  rightServo.write(rightServoAngle);

  Serial.print("LEFT SERVO ANGLE = ");
  Serial.println(leftServoAngle);
  Serial.print("RIGHT SERVO ANGLE = ");
  Serial.println(rightServoAngle);

  delay(1000);
}

void loop()
{
  if (digitalRead(OPEN_BUTTON_PIN) == LOW)
  {
    Serial.println("OPEN");
    Serial.println("");
    unsigned long startOpen = millis();

    while (digitalRead(OPEN_BUTTON_PIN) == LOW)
    {
      Serial.println("OPEN");
      Serial.println("");

      if (millis() - startOpen < BUTTON_LONG_PRESS)
      {
        Serial.println("SHORT PRESS (OPEN)");
        Serial.println("");

        digitalWrite(STATUS_LED_PIN, LOW);

        rightServoAngle += servoSteps;
        leftServoAngle -= servoSteps;

        if (rightServoAngle >= maxServoAngle) {
          rightServoAngle = maxServoAngle;

        }

        if (leftServoAngle >= maxServoAngle) {
          leftServoAngle = maxServoAngle;
        }

        rightServo.attach(RIGHT_SERVO_PIN);
        leftServo.attach(LEFT_SERVO_PIN);

        rightServo.write(rightServoAngle);
        delay(openDelay);
        leftServo.write(leftServoAngle);

        digitalWrite(STATUS_LED_PIN, LOW);
      }

      else if (millis() - startOpen >= BUTTON_LONG_PRESS)
      {
        Serial.println("LONG PRESS (OPEN)");
        Serial.println("");

        rightServoAngle = maxServoAngle;
        leftServoAngle = minServoAngle;

        rightServo.attach(RIGHT_SERVO_PIN);
        leftServo.attach(LEFT_SERVO_PIN);

        rightServo.write(rightServoAngle);
        delay(openDelay);
        leftServo.write(leftServoAngle);


        STATUS_LED_PINBlink();

        delay(1000);
      }
    }
  }

  if (digitalRead(CLOSE_BUTTON_PIN) == LOW)
  {
    Serial.println("CLOSE");
    Serial.println("");
    unsigned long startClose = millis();

    while (digitalRead(CLOSE_BUTTON_PIN) == LOW)
    {
      Serial.println("CLOSE");
      Serial.println("");

      if (millis() - startClose < BUTTON_LONG_PRESS)
      {
        Serial.println("SHORT PRESS (CLOSE)");
        Serial.println("");

        digitalWrite(STATUS_LED_PIN, LOW);

        rightServoAngle -= servoSteps;
        leftServoAngle += servoSteps;

        if (rightServoAngle <= minServoAngle) {
          rightServoAngle = minServoAngle;

        }

        if (leftServoAngle <= minServoAngle) {
          leftServoAngle = minServoAngle;
        }

        rightServo.attach(RIGHT_SERVO_PIN);
        leftServo.attach(LEFT_SERVO_PIN);

        leftServo.write(leftServoAngle);
        delay(closeDelay);
        rightServo.write(rightServoAngle);

        digitalWrite(STATUS_LED_PIN, LOW);
      }

      else if (millis() - startClose >= BUTTON_LONG_PRESS)
      {
        Serial.println("LONG PRESS (CLOSE)");
        Serial.println("");

        leftServoAngle = maxServoAngle;
        rightServoAngle = minServoAngle;

        leftServo.attach(LEFT_SERVO_PIN);
        rightServo.attach(RIGHT_SERVO_PIN);

        leftServo.write(leftServoAngle);
        delay(closeDelay);
        rightServo.write(rightServoAngle);

        STATUS_LED_PINBlink();

        delay(1000);
      }
    }
  }

  while (bluetooth.available() > 0 ) // receive number from bluetooth
  { 
    int bluetoothValue = bluetooth.read();// save the received number to servopos

    Serial.println(bluetoothValue); // serial print servopos current number received from bluetooth

    int Open;
    int Close;
    int HalfOpen;

    if (bluetoothValue >= 160)
    {
      Open = 1;
      Close = 0;
      HalfOpen = 0;

      digitalWrite(STATUS_LED_PIN, HIGH);

      //rightServo.write(maxServoAngle, fastSpeed, true); // roate the servo the angle received from the android app
      delay(openDelay);
      //leftServo.write(maxServoAngle, fastSpeed, true);

      STATUS_LED_PINBlink();
    }

    if (bluetoothValue == 80) while (Open == 1)
    {
        HalfOpen = 1;
        Open = 0;
        Close = 0;

        digitalWrite(STATUS_LED_PIN, HIGH);

        //leftServo.write(midServoAngle, fastSpeed, true); // roate the servo the angle received from the android app
        delay(closeDelay);
        //rightServo.write(midServoAngle, fastSpeed, true);

        STATUS_LED_PINBlink();
      }

    if (bluetoothValue == 80) while (Close == 1)
    {
        HalfOpen = 1;
        Open = 0;
        Close = 0;

        digitalWrite(STATUS_LED_PIN, HIGH);

        //rightServo.write(midServoAngle, fastSpeed, true); // roate the servo the angle received from the android app
        delay(openDelay);
        //leftServo.write(midServoAngle, fastSpeed, true);

        STATUS_LED_PINBlink();
      }

    else if (bluetoothValue == 0)
    {
      Open = 0;
      Close = 1;
      HalfOpen = 0;

      digitalWrite(STATUS_LED_PIN, HIGH);

      //leftServo.write(minServoAngle, fastSpeed, true); // roate the servo the angle received from the android app
      delay(closeDelay);
      //rightServo.write(minServoAngle, fastSpeed, true);

      STATUS_LED_PINBlink();
    }
  }
}

void statusLedBlink() 
{
  ledPinState = (millis() / (blinkDelay / 2)) %2;
  digitalWrite(STATUS_LED_PIN, ledPinState);
}

void moveServoForward()

 for (int pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees 
    leftServo.write(pos);              // tell servo to go to position in variable 'pos'
    rightServo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(500);                       // waits delayTime ms for the servo to reach the position
  }

void moveServoBack()
  for (int pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    leftServo.write(pos);              // tell servo to go to position in variable 'pos'
    rightServo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(delayTime);                       // waits delayTime ms for the servo to reach the position
  }