#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <Easing.h>



Servo servo1 ; // base
Servo servo2 ; // nack
Servo servo3 ; // twist
Servo servo4 ; // mouth

Servo servo5 ; // brow1
Servo servo6 ; // brow2



int analogInputPin1 = 0;    // select the input pin for the potentiometer
int analogInputPin2 = 1;    // select the input pin for the potentiometer

int analogInputValue1 = 0 ;
int analogInputValue2 = 0 ;

LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display


int value , analogValue ;
String command ;

String inputString = "";
boolean stringComplete = false;

void updateServoStatus(int servo,int value) {


  switch (servo) {

      case 97:

        lcd.setCursor(0,1);
        lcd.print("   ");
        lcd.setCursor(0,1);

        break;

      case  98:

        lcd.setCursor(4,1);
        lcd.print("   ");
        lcd.setCursor(4,1);

        break;

      case 99:

        lcd.setCursor(9,1);
        lcd.print("   ");
        lcd.setCursor(9,1);

        break;

      case 100:

        lcd.setCursor(13,1);
        lcd.print("   ");
        lcd.setCursor(13,1);

        break;
  }

  lcd.print(value) ;


}


void setup() {





  lcd.init();                      // initialize the lcd
  lcd.backlight();
  lcd.print("ready ...");




  servo1.attach(8);
  servo2.attach(9);
  servo3.attach(10);
  servo4.attach(11);
  servo5.attach(12) ;
  servo6.attach(13) ;


  servo1.write(90) ;
  servo2.write(90) ;
  servo3.write(90);
  servo4.write(90);
  servo5.write(90);
  servo6.write(90);



 // servoTest(servo1,90) ;
 // servoTest(servo2,90) ;
  //servoTest(servo3,90) ;
 // servoTest(servo4,90) ;
 // servoTest(servo5,90) ;
 //  servoTest(servo6,90) ;

  /*
  servoTest(servo5) ;
  servoTest(servo6) ;

  servoTest(servo5) ;
  servoTest(servo6) ;

  servo5.write(90) ;
  servo6.write(90) ;
*/

  /// update display

  updateServoStatus(97,90);
  updateServoStatus(98,90);
  updateServoStatus(99,90);
  updateServoStatus(100,90);

  /// init finished

  Serial.begin (57600);  // start serial communication !



}



void servoTest(Servo servo , int originalPose) {

  int startDeg = 40 ;
  int stopDeg = 140 ;


  for(int pos = startDeg ; pos < stopDeg ; pos += 1)  // goes from 0 degrees to 180 degrees
  {                                  // in steps of 1 degree
    servo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 15ms for the servo to reach the position
  }
  for(int pos = stopDeg; pos>=startDeg; pos-=1)     // goes from 180 degrees to 0 degrees
  {
    servo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 15ms for the servo to reach the position
  }

  servo.write(originalPose) ;

}


void readAnalogData() {

  /// handle analog Potentiometer input

  analogInputValue1 = analogRead(analogInputPin1);    // read the value from the sensor
  Serial.println(analogInputValue1 -700 )  ;

  analogInputValue2 = analogRead(analogInputPin2);    // read the value from the sensor
  analogInputValue2 = analogInputValue2 - 700 ;



  analogValue = map(analogInputValue2, 700, 1023, 0, 179);
  Serial.println(analogValue)  ;
  Serial.println("-") ;

 //// servo1.write(val);
}


int reverseServo(int val) {

  ///return map(val,0, 180, 180, 0); // goes the other way
  return val ;
}

int mapServo(int val) {

  int v = map(val,0, 180, 0, 180); // goes the other
  return constrain(v,0,180) ;
}





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() {


  /// recieve serial communication messges

//  lcd.setCursor(12,0);
//  lcd.print("*") ;


  if (stringComplete) {


    /// read number

    String valueString = inputString.substring(1,inputString.length()-1) ;
    value = valueString.toInt() ;


    lcd.setCursor(12,0);
    lcd.print("    ") ;
    lcd.setCursor(12,0);
    lcd.print(inputString[0]); // command !



    if (inputString[0] == 'a') { // 'a' - motor1

      servo1.write(value) ;
      updateServoStatus(inputString[0],value) ;

    }

    if (inputString[0] == 'b') { // 'b' - motor 2

      servo2.write(reverseServo(value)) ;
      updateServoStatus(inputString[0],reverseServo(value)) ;

    }


    if (inputString[0] == 'c') { // 'c' - motor 3

      servo3.write(reverseServo(value)) ;
      updateServoStatus(inputString[0],reverseServo(value)) ;

    }



    if (inputString[0] == 'd') { // 'd' - motor 4

      servo4.write(reverseServo(value)) ;
      updateServoStatus(inputString[0],reverseServo(value)) ;

    }



    if (inputString[0] == 'e') { // 'e' - motor 5

      servo5.write(reverseServo(value)) ;
    //  updateServoStatus(inputString[0],reverseServo(value)) ;

    }




    if (inputString[0] == 'f') { // 'd' - motor 4

      servo6.write(reverseServo(value)) ;
      //updateServoStatus(inputString[0],reverseServo(value)) ;

    }


    inputString = "";
    stringComplete = false;

  }

}