#include <Servo.h>

/* 
 * My First Robot
 *
 * This robot drive around and avoids objects with the Ping sonar.
 *
 */
 
int eyes_red    = 5;
int eyes_blue   = 6;
int mouth[]       = {2,3,4};

char mood = '|';
int min_word_segment = 100; // Min LED light time
int max_word_segment = 200; // Max LED light time
int max_word_expand  = 600; // Maximum time it takes to expand a word
boolean words_done = false;
int curr_word = 0;
int words_len = 0;
int word_index = 0;
int words[50];


byte cmd = 0;
boolean new_cmd = false; 

Servo eyebrow_left;
Servo eyebrow_right;


void setup(){
  Serial.begin(9600);
  eyebrow_right.attach(9);
  eyebrow_left.attach(7);
  
  eyebrow_right.write(90);
  eyebrow_left.write(90);
}

void loop(){
  Servo::refresh();
  
  if(words_done == true && word_index < words_len){
    Serial.println("speaking");
    //Serial.println(words[word_index]);
    
    speak_word(words[word_index]);
    word_index++;
  }
  
  // Reset eyebrows
  if(words_len > 0 && word_index >= words_len){
    delay(1000); // leave the mood for about 1 second
    eyebrow_right.write(90);
    eyebrow_left.write(90);
    
    digitalWrite(eyes_blue, LOW);
    digitalWrite(eyes_red, LOW);
    
    // Reset everything
    words_len = 0;
    word_index = 0;
    curr_word = 0;
    words_done = false;
  }
  
  // Receive serial commands
  int value;
  if (words_done == false && Serial.available() > 0) {
    value = Serial.read();
  
    // No more words, start reading
    if((char)value == '\r'){
      Serial.println("Speak");
      words_done = true;
    }
    // Mood = Angry
    else if(words_len == 0 && (char)value == '>'){
       Serial.println("angry");
       eyebrow_right.write(70);
       eyebrow_left.write(115);
       digitalWrite(eyes_red, HIGH);
    }
    // Mood = Sad
    else if(words_len == 0 && (char)value == '('){
       Serial.println("sad");
       eyebrow_right.write(140);
       eyebrow_left.write(50);
       digitalWrite(eyes_blue, HIGH);
    }
    // Mood = Neutral
    else if(words_len == 0 && (char)value == '|'){
       eyebrow_right.write(90);
       eyebrow_left.write(90);
       digitalWrite(eyes_blue, HIGH);
    }
    // Add word time
    else if(words_done == false && value > 0){
      // Word time might be longer than 255, so we split the number into 255 segments
      // ending with the \n character
      if((char)value == '\n'){ 
        Serial.println(curr_word);
        words[words_len] = curr_word;
        words_len++;
        curr_word = 0;
      }
      else{
        curr_word += value;
      }
    }
  }  
}


// Say a word
void speak_word(int time){
  time = time / 3;
  int segments[] = {0, 0, 0};
  int pause = 0;
  int inc = 0;
  
  // The word is too short
  if(time < min_word_segment){
    return;
  }
  
 
  // The word wont fill all segments
  if(time < min_word_segment * 3){
    segments[0] = min_word_segment;
    
    // Fits in at least 2
    if(time >= min_word_segment * 2){
      segments[1] = time - min_word_segment;
      pause = time - segments[1];
    }
    
    talk(segments, pause);
  }
  // The word fits
  else {
    inc = time / 3; // LED increment time
    
    // Too long, add pause
    if(inc > max_word_segment){
      inc = max_word_segment;
      pause = time - (max_word_segment * 3);
    }
    
    segments[0] = inc;
    segments[1] = inc;
    segments[2] = inc;
    talk(segments, pause);
  }
}

// Light up the mouth and talk
void talk(int *segments, int pause){
  
  Serial.print(segments[0]);
  Serial.print(" : ");
  Serial.println(pause);
  
  // Light 'em up
  for(int i = 0; i < 3; i++){
    if(segments[i] > 0){
      digitalWrite(mouth[i], HIGH);
      delay(segments[i]);
    }
  }
  
  if(pause > 0){
    delay(pause);
  }
  
  // Take 'em down
  //Serial.println("out");
  for(int i = 2; i >= 0; i--){
    digitalWrite(mouth[i], LOW);
    if(segments[i] > 0){
      delay(segments[i]);
    }
  }
  
  delay(20);
  
}

void loop_test(){
 //eyebrows.write(90); // Angry
 //eyebrows.write(20); // Sad
 //eyebrows.write(40); // Neutral
 //Servo::refresh(); 
  
  // Light 'em up
  for(int i = 0; i < 3; i++){
    digitalWrite(mouth[i], HIGH);
    delay(100);
  }
  
  delay(100);
  
  // reset
  for(int i = 3; i > -1; i--){
    digitalWrite(mouth[i], LOW);
    delay(100);
  }
  delay(100);
}