/*  Evil robot brain! Finite state machine version.  */
/* Accept comms over serial link. Non-blocking waits. */
/* LiPo batt: Max:859(4.2v) Nom:747(3.7v) Min:613(3.0v)  */

/* FSM  states */
#define state_pause        0
#define state_move         1
#define state_explore      2
#define state_error        255  


/* Directions */
#define dir_forward    0
#define dir_left       1
#define dir_right      2
#define dir_back       3


int E1 = 6; //M1 Speed Control
int E2 = 5; //M2 Speed Control
int M1 = 8; //M1 Direction Control
int M2 = 7; //M2 Direction Control
const int bumpPin = 12;     // the number of the pushbutton pin
const int ledPin = 13;    // Flashing LEDs!
const int m1_max=255;          // Motor speeds. M1 is right, M2 is left.
const int m2_max=250;          // m2 is slightly faster, so reduse top speed.
unsigned long lastbump;
char state=state_pause;                  // Make sure the 'bot is in a known state 
char laststate=state_pause;
char setupstate=state_pause;
long movecount=0;                    // How long to move for
char movedir=0;                      // What direction to move
char boterror=0;
char avoid=0;
long debugtimer=0;
long movetimer=0;
int flashcount;

void setup()
{
pinMode(9, OUTPUT);            // Speeker 'enable'
pinMode(ledPin, OUTPUT);		// Status LED
pinMode(bumpPin, INPUT);	// Bump sensor
analogWrite (E2,m2_max);
analogWrite (E1,m1_max);
long lastbump=millis();                // Setup non-bump-detect.

int i;
for(i=5;i<=8;i++)
pinMode(i, OUTPUT);
Serial.begin(9600);                // Serial link
Serial.println("Robot started. Commands: asdw and e");  // Test link.

}
void loop() {

// Read in sensors


// Battery protect
//   Serial.println(analogRead(A0), DEC);
if (analogRead(A0) <= 650 ) {  // If batt is lower than 3.0v
 delay(50);
 if (analogRead(A0) <= 650 ) {      // Delay then try again. Just to be sure.
   boterror=1;                  // If it's still a problem, toggle flag.
   laststate=state;
   state=state_error;
 }
}


// Deal with serial port input
if (Serial.available() != 0) {
  char val = Serial.read();
  Serial.println(val, BYTE); 
switch(val) // Perform an action depending on the command
{
  case 'w'://Move Forward
    state=state_move;
    movedir=dir_forward;
    movecount=10000;
  break;
  case 's'://Move Backwards
    state=state_move;
    movedir=dir_back;
    movecount=10000;
  break;
  case 'a'://Turn Left
    state=state_move;
    movedir=dir_left;
    movecount=10000;
  break;
  case 'd'://Turn Right
    state=state_move;
    movedir=dir_right;
    movecount=10000;
  break;
  case 'e': // Explore
    state=state_explore;
  break;
  default:
    state=state_pause;
  break;
  }
}

// Throw out debug message every so often.
if ((millis() - debugtimer)  >= 1000) {
  Serial.print("Debug(");
  Serial.print(millis(),HEX);
  Serial.print("): Batt(");
//  Serial.print(analogRead(A0)*0.00489,DEC);
  Serial.print(analogRead(A0),DEC);
  Serial.print(") State(");
  Serial.print(state,DEC);
  Serial.print(") Prev(");
  Serial.print(laststate,DEC);  
  Serial.print(") Move(");
  Serial.print(movecount,DEC);
  Serial.print(") Dir(");
  Serial.print(movedir,DEC);
  Serial.print(") Avoid(");
  Serial.print(avoid,DEC);  
  Serial.println(")");
  
  debugtimer=millis();
}

if(setupstate != state){          // Keep an eye out for state changes.
  Serial.print("DEBUG(");
  Serial.print(millis(),HEX);
  Serial.print(") State(");
  Serial.print(state,DEC);
  Serial.print(") Prev(");
  Serial.print(laststate,DEC);
  Serial.println(") State change.");
}

// Start main state machine selection.
switch(state) {                  

// Pause state.
case state_pause:
  if(setupstate != state) {
    if ( movecount != 0 ) {       // Debug, we got here because of a sensor trigger.
  Serial.print("DEBUG(");
  Serial.print(millis(),HEX);
  Serial.print(") Move(");
  Serial.print(movecount,DEC);
  Serial.print(") Dir(");
  Serial.print(movedir,DEC);
  Serial.println(")  Incomplete Move");
    }
  setupstate=state;
  }
break;
  
// Normal travel  
case state_move:
  if(setupstate != state) {
    switch(movedir) {            // Send the robot off in the right direction!
      case dir_forward:
        digitalWrite(M1,LOW);
        digitalWrite(M2,LOW);
      break;
      case dir_left:
        digitalWrite(M1,HIGH);
        digitalWrite(M2,LOW);
      break;
      case dir_right:
        digitalWrite(M1,LOW);
        digitalWrite(M2,HIGH);
      break;
      case dir_back:
        digitalWrite(M1,HIGH);
        digitalWrite(M2,HIGH);
      break;
    }
    lastbump=millis();            // 'reset' bump timer.
    setupstate=state;
  }
    
  // Might move this into main tree later.
  if (digitalRead(bumpPin) == HIGH) {		// Have we hit somthing?
    state=laststate;    	        	// We're done moving
  }
  // Stuck detect.  No bumps for 30 seconds
  if ((millis() - lastbump)  >= 30000) {
    boterror=2;                  // So you know this triggerd it
    laststate=state;
    state=state_error;
    break;
  }
  
  if ((millis() - movetimer)  >= 1) {
  movecount--;            // This will be driven by the wheel decoders later
  movetimer=millis();      // For now, it's just a milisecond timer.
  }

  if (movecount==0) { 
    state=laststate;
  }    
break;


// Robot explore; roomba mode
case state_explore:
    digitalWrite(ledPin, HIGH);
    setupstate=state;
    
    if ( movecount != 0 ) {          // A move stopped before time!
      if (movedir==dir_forward){
      avoid=2;                       // We hit somthing going forward, reverse then turn   
      }
      else {                         // Hit somthing doing avoid, so turn more
        avoid=1;
      }
    }

  switch(avoid){
  case 2:        // Reverse robot for 1 second
    laststate=state;
    state=state_move;
    movedir=dir_back;
    movecount=1000;
    avoid=1;            // Next avoid mode
  break;
  case 1:              // Turn to avoid
    laststate=state;
    state=state_move;
    movedir=dir_left;
    movecount=random(100,1000);
    avoid=0;          // Done avoiding
  break;             
  default:              // We are not avoiding
    digitalWrite(ledPin, LOW);
    laststate=state;
    state=state_move;
    movedir=dir_forward;
    movecount=30000;
  break;
  }
break;


// Handle an errror, mostly pause and wait
case state_error:
  if(setupstate != state){
    analogWrite (E2,0);        // We've had an error! Kill the 'bot
    analogWrite (E1,0);        // Switch off motors
    digitalWrite(9,HIGH);        // Enable speeker
    setupstate=state;
  }
    digitalWrite(13, HIGH);
    tone(10,800);              // Beep and let the world know!
    delay(500);
    noTone(10);
  
    flashcount=boterror;  // Flash the error code
    while(flashcount>0){
      digitalWrite(13, LOW);
      delay(100);
      digitalWrite(13, HIGH);
      delay(300);
      flashcount--;
    }
break;


default:                        // In an unknown state, force to known.
  Serial.print("DEBUG(");
  Serial.print(millis(),HEX);
  Serial.print(") State(");
  Serial.print(state,DEC);
  Serial.print(") Prev(");
  Serial.print(laststate,DEC);
  Serial.println(") !!INAVALID STATE!! Setting state_pause.");
  state=state_pause;
  laststate=state_pause;
break;

}  // End state switch
}   // End of program