// HeadBotBase - From Box Head / DRHead Type of Animated Bot
// Stephen W Nolen / Protowrxs 2015
// v1.00 - Public 10/27/2015

//** Original code from Box Head
//** serial commands are in the main loop case - add or remove as needed

//** This ASSumes a few things - you are using RGB LEDs for eyes and you have at least ONE servo to rotate the neck
//** You will have to adjust and/or flip around the Left/Right and Up/Down limits on the servos to match your  needs



// Pin Use Defaults
// 0  RX from VB.net
// 1  TX to VB.net
// 2  
// 3  
// 4  Neck Rotate
// 5  Neck Up/Down
// 6  Jaw Servo
// 7  R Left Eye
// 8  G Left Eye
// 9  B Left Eye
// 10 R Right Eye
// 11 G Right Eye
// 12 B Right Eye
// 13
// A0
// A1 B Mouth       //** You should change this up if you are not using it or however the heck you want to configure things
// A2 G Mouth
// A3 R Mouth
// A4

// ** Includes area
#include <Servo.h>

Servo NeckRotate;
Servo NeckUpDown;
Servo Jaw;

long substeptimer;
int  substeps = 12;                      // this is how many increments we are going to move a servo for each walk step or "frame"

int MyTilt;
int MyRotate;

byte Blue = 0;                        // Just values to use for changing eye color LEDs
byte Red = 1;
byte Green = 2;
byte eyeColor = Red;                 // Start out normal with blue eyes
byte mouthColor = Green;              // start out with green mouth to test
byte currentMouth;   // default to this

byte currentLeftEye;
byte currentRightEye;

//** Adjust the head center/positions as needed for your configuration
byte HeadCenterLimit= 90;
byte HeadLeftLimit = HeadCenterLimit - 30;
byte HeadRightLimit=  HeadCenterLimit + 30;

//** Adjust the jaw center/positions as needed for your configuration
byte JawOpenLimit = 110;
byte JawClosedLimit = 59;
byte JawCurrentValue = JawClosedLimit;
byte JawHalfOpen = 85;

long TalkingTimer;
byte isTalking = 0;
long DelayToTalk = 200;             //** The delay from the command until the time sound may be heard from computer due ot the delay of eSpeak
long DelayToTalkTimer;              //** used with delaytotalk to wait to move jaw

//** For tilt options - adjust as needed
byte HeadMiddleLimit = 90;
byte HeadUpLimit = HeadMiddleLimit - 30;
byte HeadDownLimit= HeadMiddleLimit + 30;

float CurrentRotate;                  // Current neck rotate position - used as we do step movement to desired positions
float CurrentTilt;                    // Current tilt position for tracking
float NeckRotateInc;
float NeckTiltInc;

byte CurrentLook;                    // Current looking position for the head
byte LookingForward=0;               // general position tracking variables to easily know where looking or where to look
byte LookingDown  = 1;
byte LookingUp    = 2;
byte LookingLeft  = 3;
byte LookingRight = 4;
byte LookingMiddle= 5;

byte LookingCustom= 9;

int headrequest;

int LoopDelay = 50;                  // 

boolean MouthOpen = false;

boolean isawake;                // Used to blink eyes or not, etc

long motionTimer;                 //Used to determine when to make minor movements
int nextMotion;                   //Next time random motion will occur

long BoredTimer;
byte data = 0;
byte facedata;

// give eye pins a name so we can move things around if needed
int LeftR = 9;
int LeftG = 8;
int LeftB = 7;

int RightR = 12;
int RightG = 11;
int RightB = 10;

int eyesOpen = 1;
long blinkTimer;
long nextBlink;

//** This is the option to use a RGB led for the mouth instead of a jaw servo - not fully functional though
int MouthR = 13;
int MouthG = A0;
int MouthB = A1;

long generalTimer;
long nextMoveTime;


void setup()
{
  randomSeed(analogRead(0));

   // Default the Current values to avoid ackward start ups
  CurrentTilt = HeadMiddleLimit;
  CurrentRotate= HeadCenterLimit;

  NeckRotate.attach(4);     //** hook up the neck
  NeckRotate.write(CurrentRotate);
  
  Jaw.attach(6) ;           //** hook up the jaw - if this is ever used in EvilAnna
  Jaw.write(JawCurrentValue);

  NeckUpDown.attach(5);
  NeckUpDown.write(CurrentTilt);
  
  changeEyeColor();         //** initial condiguration

  //** setup all the pin modes as needed
  pinMode(MouthR, OUTPUT);
  pinMode(MouthG, OUTPUT);
  pinMode(MouthB, OUTPUT);

  currentMouth = MouthR;    //** Just using RED mouth for now but you could change this
  
  pinMode(LeftR, OUTPUT);     
  pinMode(LeftG, OUTPUT);     
  pinMode(LeftB, OUTPUT);     

  pinMode(RightR, OUTPUT);
  pinMode(RightG, OUTPUT);
  pinMode(RightB, OUTPUT);
  
  //** Write out the initial settings as needed
  digitalWrite(LeftR, HIGH);
  digitalWrite(LeftB, HIGH);
  digitalWrite(LeftG, HIGH);

  digitalWrite(RightR,HIGH);  
  digitalWrite(RightG,HIGH);  
  digitalWrite(RightB,HIGH);  
  
  digitalWrite(MouthR, HIGH);
  digitalWrite(MouthG, HIGH);
  digitalWrite(MouthB, HIGH);

  
  //** Init the VB Voice / Control port on hardware  
  Serial.begin(9600); // Open serial monitor at 115200 baud to see ping results.

  LookUp();
  
  BoredTimer = millis();

  //** Coolest to default to facetracking I think
  CurrentLook = LookingForward;
  headrequest = LookingForward;

  motionTimer = millis();     //** Start up motion random timer
  nextMotion = random(200,1000); //** when the next random movement will be
}

void loop()
{

  //** If we are in talking mode then we call the Talking() sub to move the jaw
  if ((isTalking == 1) && (getET(DelayToTalkTimer) > DelayToTalk))
  {
    Talking2();
  }
  
  if (getET(blinkTimer) > nextBlink)
  {
    if (isawake) 
    {
     BlinkEyes();
     blinkTimer = millis();
    }
    else
     {
     CloseEyes();
     }
  }

  //** Generate a little random head movement when it is awake to keep things interesting
  if (isawake)
  {
    if (getET(motionTimer) > nextMotion)  
    {
      randomHeadMove();
    }
  }
  
  headrequest = CurrentLook;

  //** If we have any inbound serial commands we need to process them
  if (Serial.available() > 0) 
  {
    data = Serial.read();	 // read the incoming byte:
    switch(data)
    {
        case 'r' : eyeColor = Red; changeEyeColor(); break;   //** Simple eye color changes if you are using RGB LEDs
        case 'b' : eyeColor = Blue; changeEyeColor();break;
        case 'g' : eyeColor = Green; changeEyeColor();break;
      	case 'U' : headrequest = LookingUp;  break;           //** Updated headrequest var to move th ehead around
        case 'T' : isTalking = 1; DelayToTalkTimer = millis();break;    //** This starts the random jaw animation on the servo on pin 5 if you are using it
        case 't' : isTalking = 0; CloseJaw(); break;                    //** Shuts down the talking code
        case 'G' : isawake = false; break;                  //** Says goodbye to the bot and turned off isawake
        case 'L' : headrequest = LookingLeft; break;        //** Just normal movements for the Pan and/or Tilt servos
        case 'R' : headrequest = LookingRight ; break;
        case 'D' : headrequest = LookingDown ;  break;
        case 'F' : headrequest = LookingForward; isawake = true;break;
        case 'M' : headrequest = LookingMiddle; break;
        case 'H' : WiggleHead(); break;                     //** This gives that acknowledgement that he/she/it heard something
        case '1' : mouthColor = Red; changeMouthColor(); break;            //** Commands to change the MOUTH RGB LED to different colors
        case '2' : mouthColor = Green; changeMouthColor(); break;
        case '3' : mouthColor = Blue; changeMouthColor(); break;
        case 'J' : OpenJaw(); break;                        //** open / close commands 
        case 'j' : CloseJaw(); break;
        case '?': ;break;
        default  : break;
    }

    if (data != 'S')
    {
      Serial.print(headrequest);
    }
  }

  
// ** Depending on what we have set the driverequest var to above we execute it here
// ** This could/should be in a sub to clean up the main loop but easier to debug right here

  switch (headrequest){
    case 0: 
      substeptimer = millis();
      LookForward();   	  
      break;
    case 1:
      substeptimer = millis();
      LookDown();
      break;
    case 2: 
      substeptimer = millis();
      LookUp();  
      break;
    case 3:
      substeptimer = millis();
      LookLeft();  
      break;
    case 4:
      substeptimer = millis();
      LookRight();
      break;
    case 5: 
      substeptimer = millis();
      LookMiddle();
    case 6: 
      substeptimer = millis();
      break;
    case 7:
      break;
    case 8:
      break;
    case 9:
      LookCustom();
      break;
      
  }  // End Current Step

  delay(LoopDelay);
}


void LookForward()
{
  TiltNeck(HeadMiddleLimit);
  RotateNeck(HeadCenterLimit);
  CurrentLook = LookingForward;
}
  

void LookUp()
{
  TiltNeck(HeadUpLimit);
  CurrentLook = LookingUp;

}

void LookDown()
{
  TiltNeck(HeadDownLimit);
  CurrentLook = LookingDown;
}

void LookCenter()
{
  NeckUpDown.write(90);
  CurrentTilt = HeadMiddleLimit;
  }


void LookLeft()
{
   RotateNeck(HeadLeftLimit);
   CurrentLook = LookingLeft;
}

void LookRight()
{
   RotateNeck(HeadRightLimit);
   CurrentLook = LookingRight;
}

void LookMiddle()
{
  TiltNeck(HeadMiddleLimit);
  CurrentLook = LookingMiddle;
}


void LookUpCenter()
{
  LookCenter();
  LookUp();
}

void LookDownCenter()
{
  LookDown();
  LookCenter();
}

void LookCustom()
{
  RotateNeck(CurrentRotate);
  TiltNeck(CurrentTilt);
  CurrentLook = LookingCustom;  
}


// ** returns the ET in millis for timer variable passed
long getET(long mytimer){
  long result;
  result = (millis() - mytimer);
  return result;
}

//** This moves to the position by the substeps provided
void RotateNeck(byte MoveToPosition){
  if (getET(substeptimer) < LoopDelay){
     NeckRotateInc = (MoveToPosition - CurrentRotate) / (substeps);
  }

  CurrentRotate = CurrentRotate + NeckRotateInc;

  NeckRotate.write(CurrentRotate);
}

//** This moves to the position by the substeps provided
void TiltNeck(byte MoveToPosition){
  if (getET(substeptimer) < LoopDelay){
     NeckTiltInc = (MoveToPosition - CurrentTilt) / (substeps);
  }

  CurrentTilt = CurrentTilt + NeckTiltInc;

  NeckUpDown.write(CurrentTilt);
}


// Directly position the rotation and update the current values
void RotateNeckPosition(byte MoveToPosition)
{
  //** Keep us in the master bounds just in case
  //** This is more likely to occur from Face Tracking
  MoveToPosition = constrain(MoveToPosition, HeadLeftLimit, HeadRightLimit);
  NeckRotate.write(MoveToPosition);
  CurrentRotate = MoveToPosition;
}

void TiltNeckPosition(byte MoveToPosition)
{
  MoveToPosition = constrain(MoveToPosition, HeadUpLimit, HeadDownLimit);
  NeckUpDown.write(MoveToPosition);
  CurrentTilt = MoveToPosition;
}

//** This just gives a nod when talking - Best to use this if you have tilt options!!!
void WiggleHeadOLD()
{
  NeckUpDown.write(CurrentTilt + 3);
  delay(100);
  NeckUpDown.write(CurrentTilt);
}

//** This just gives a nod when talking
void WiggleHead()
{
  NeckRotate.write(CurrentRotate + 3);
  delay(100);
  NeckRotate.write(CurrentRotate);
}


void BlinkEyes()
{
  
  digitalWrite(currentLeftEye,HIGH);
  digitalWrite(currentRightEye,HIGH);
  
  delay(random(100,300));               // wait for a second
  
  digitalWrite(currentLeftEye, LOW);   // turn the LED on (HIGH is the voltage level)
  digitalWrite(currentRightEye, LOW);   // turn the LED on (HIGH is the voltage level)
  
  nextBlink = random(2000,5000);  //** reset timer to next blink here
  blinkTimer = millis();
  
}

void CloseEyes()
{

  digitalWrite(LeftB,HIGH);
  digitalWrite(RightB,HIGH);
  digitalWrite(LeftR,HIGH);
  digitalWrite(RightR,HIGH);
  digitalWrite(LeftG,HIGH);
  digitalWrite(RightG,HIGH);
  
}

void OpenJaw()
{
  Jaw.write(JawOpenLimit);
  JawCurrentValue = JawOpenLimit;
  digitalWrite(currentMouth,LOW);
}

void CloseJaw()
{
  Jaw.write(JawClosedLimit);
  JawCurrentValue = JawClosedLimit;
  digitalWrite(currentMouth,HIGH);
}

void HalfJaw()
{
  Jaw.write(JawHalfOpen);
  JawCurrentValue = JawHalfOpen;
  digitalWrite(currentMouth,LOW);
}

void Talking()
{
  long TalkingDelay = random(50,300);
  if (getET(TalkingTimer) > TalkingDelay)
  {
    if (JawCurrentValue == JawOpenLimit)
    {
      HalfJaw();
      TalkingTimer = millis();
    }
    else
    {
      OpenJaw();
      TalkingTimer = millis();
    }
  }
}


void Talking2()
{
  long TalkingDelay = random(50,100);
  if (getET(TalkingTimer) > TalkingDelay)
  {
    int talkJaw = random(JawClosedLimit, JawOpenLimit);
    Jaw.write(talkJaw);
    TalkingTimer = millis();
    if (talkJaw > JawHalfOpen)
    {
      digitalWrite(currentMouth,LOW);
    }
    else
    {
      digitalWrite(currentMouth,HIGH);
    }
  }
}

void randomHeadMove()
{
  int randomOffset = random(-1,1);
  CurrentRotate = CurrentRotate + randomOffset  ;
  nextMotion = random(1000,3000);
  motionTimer = millis();
}

void changeEyeColor()
{
  digitalWrite(LeftB,HIGH);
  digitalWrite(RightB,HIGH);
  digitalWrite(LeftR,HIGH);
  digitalWrite(RightR,HIGH);
  digitalWrite(LeftG,HIGH);
  digitalWrite(RightG,HIGH);

  switch (eyeColor){
    case 0: // Blue
      currentLeftEye = LeftB;
      currentRightEye= RightB;
      break;
    case 1: // Red
      currentLeftEye = LeftR;
      currentRightEye= RightR;
      break;
    case 2: // Green
      currentLeftEye = LeftG;
      currentRightEye= RightG;
      break;
  }
  
}


void changeMouthColor()
{
  digitalWrite(MouthR,HIGH);
  digitalWrite(MouthB,HIGH);
  digitalWrite(MouthG,HIGH);

  switch (mouthColor){
    case 0: // Blue
      currentMouth = MouthB;
      break;
    case 1: // Red
      currentMouth = MouthR;
      break;
    case 2: // Green
      currentMouth = MouthG;
      break;
  }
}