#include <Servo.h>
#include <SoftwareSerial.h>

SoftwareSerial mySerial(10, 11); // RX, TX

String voice;

#define servo1_pin 2
#define servo2_pin 3
#define servo3_pin 4
#define servo4_pin 5
#define servo5_pin 6

Servo Servo1;
Servo Servo2;
Servo Servo3;
Servo Servo4;
Servo Servo5;

/*
            Open~Closed~Med
   Servo1 = 130 ~ 20     55
   Servo2 = 32  ~ 143    87
   Servo3 = 50  ~ 150    100
   Servo4 = 55  ~ 156    105
   Servo5 = 53  ~ 155    104

*/

void setup() {
  Serial.begin(9600);
  mySerial.begin(9600);
  Servo1.attach(servo1_pin);
  Servo2.attach(servo2_pin);
  Servo3.attach(servo3_pin);
  Servo4.attach(servo4_pin);
  Servo5.attach(servo5_pin);
  startPosition();
}

void loop() {
  while (mySerial.available()) { //Check if there is an available byte to read
    delay(10); //Delay added to make thing stable
    char c = mySerial.read(); //Conduct a serial read
    if (c == '#') {
      Serial.println(voice);
      break; //Exit the loop when the # is detected after the word
    }
    voice += c; //Shorthand for voice = voice + c
  }
  if (voice.length() > 0) {
    if (voice == "*not sure") {
      notsure();
      delay(5000);
    }
    else if (voice == "*hello") {
      hello();
      delay(5000);
    }
    else if (voice == "*you") {
      you();
      delay(5000);
    }
    else if (voice == "*good job") {
      goodjob();
      delay(5000);
    }
    else if (voice == "*I love you") {
      iloveyou();
      delay(5000);
    }
    voice = "";
  }







}

void startPosition() {
  Servo1.write(55);
  delay(15);
  Servo2.write(87);
  delay(15);
  Servo3.write(100);
  delay(15);
  Servo4.write(105);
  delay(15);
  Servo5.write(104);
  delay(15);
  Serial.print("Waiting...");
}

//#1 Hello
void hello() {
  Servo1.write(130);
  delay(15);
  Servo2.write(32);
  delay(15);
  Servo3.write(50);
  delay(15);
  Servo4.write(55);
  delay(15);
  Servo5.write(53);
  delay(15);
  Serial.print("Hello");
}

//#2 You
void you() {
  Servo1.write(20);
  delay(15);
  Servo2.write(32);
  delay(15);
  Servo3.write(150);
  delay(15);
  Servo4.write(156);
  delay(15);
  Servo5.write(155);
  delay(15);
  Serial.print("YOU");
}

//#3 Good Job
void goodjob() {
  Servo1.write(130);
  delay(15);
  Servo2.write(143);
  delay(15);
  Servo3.write(150);
  delay(15);
  Servo4.write(156);
  delay(15);
  Servo5.write(155);
  delay(15);
  Serial.print("Good Job");
}

//#4 I Love You
void iloveyou() {
  Servo1.write(130);//o
  delay(15);
  Servo2.write(32);//o
  delay(15);
  Servo3.write(150);
  delay(15);
  Servo4.write(156);
  delay(15);
  Servo5.write(53);//o
  delay(15);
  Serial.print("I Love You");
}

//#5 Not Sure
void notsure() {
  Servo1.write(130);//o
  delay(15);
  Servo2.write(143);
  delay(15);
  Servo3.write(150);
  delay(15);
  Servo4.write(156);
  delay(15);
  Servo5.write(53);//o
  delay(15);
  Serial.print("Not Sure");
}