/*  Evil robot brain! Finite state machine version.  */
/* Accept comms over serial link. Non-blocking waits. */
/* Manual overrides, multi-sensors, averaging reads  */
/* And everything is in functions!                  */
/*  SIZE:  7.5k of 30,7k = 23%                        */

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

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

/* Motor controll pins. */
const int E1 = 6; //M1 Speed Control
const int E2 = 5; //M2 Speed Control
const int M1 = 8; //M1 Direction Control
const int M2 = 7; //M2 Direction Control

/* Main blue LEDs, inverted. HIGH = off */
const int ledPin = 13;    // Flashing LEDs!

/* Battery monitoring */
const int battPin = A6;
char battCount=0;
char battState=0;

/* "User" buttons with LEDs */
const int button1Pin = 4;
const int button1Led = 3;
char button1State=1;
char button1Count=10;

/* First edge detection, digital */
const int edge1Pin = 2;
char edge1State=0;
char edge1Count=5;

/* IR or Sonic distance sensors, analog */
const int dist1Pin = A0;
char dist1Value=0;

/* Motor speeds */
int m1_max=255;          // M1 is right, M2 is left.
int m2_max=231;          // m2 is slightly faster, so reduse top speed.

/* State machine controlls */
char state=state_pause;                  // Make sure the 'bot is in a known state 
char laststate=state_pause;
char setupstate=state_pause;

/* Movement and avoidance  */
long movecount=0;                    // How long to move for
char movedir=0;                      // What direction to move
unsigned long lastbump;
char avoid=0;
char bumpdetect=0;      // Objet located.  Needs defines for direction.

/* System counters. */
unsigned long movetimer=0;
unsigned long sensortimer=0;
unsigned long debugtimer=0;

/* Error modes/settings  */
char boterror=0;
char flashcount;

/* Make sure all I/O is set up and ready to use. */ 
void setup() {
pinMode(9, OUTPUT);            // Speeker 'enable'

pinMode(ledPin, OUTPUT);		// Status LED
digitalWrite(ledPin, HIGH);              // Light off.

analogWrite (E2,0);                // Motor stopped.
analogWrite (E1,0);
long lastbump=millis();                // Setup non-bump-detect.

int i;                                // Robot controlls.
for(i=5;i<=8;i++)
pinMode(i, OUTPUT);

pinMode(button1Pin,INPUT);           // 'user' button.
digitalWrite(button1Pin, HIGH);      // With pullup.
pinMode(button1Led,OUTPUT);          // And a built in LED!

Serial.begin(9600);                // Serial link
Serial.println("Robot started. Commands: asdw and er+-");  // Test link works.

}


/* Main loop. No blocking calls or commands allowed at all!! */
void loop() {

  if ((millis() - sensortimer)  >= 2) {        // Every 2mS             
    readsensors();                             // Read in all sensors
    colldetect();                              // Collision detection from sensors stuff.
    sensortimer=millis();
  }
 
  if (Serial.available() != 0) {
    serialinput();                              // Deal with serial port input 
  }

  if ((millis() - debugtimer)  >= 1000) {
    Serial.print("DEBUG");
    debugmessage();                            // Throw out debug message every so often, once a second.
    debugtimer=millis();
  }

  if(setupstate != state){          // Keep an eye out for state changes.
    Serial.println("STATE CHANGE");
    debugmessage();
  }

  // Start main state machine selection.
  switch(state) {                  
    case state_pause:
      dostate_pause();
    break; 
    case state_move:
      dostate_move();
    break;
    case state_explore:
      dostate_explore();
    break;
    case state_error:
      dostate_error(); 
    break;
    default:             // In an unknown state, force to known.
      Serial.println(") !!INAVALID STATE!! Setting state_pause.");
      Serial.print("STATE DEBUG");
      debugmessage();
      state=state_pause;
      laststate=state_pause; 
    break;
  }  // End state switch

}   // End of loop();


/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */

/* Read in sensors
Every 2-3ms, read in all sensors, each sensor has a countdown value and a 'next mode' value. On change of value, reset countdown,
set 'next mode'. On same value, decrement coutndown. when coutndown is zero(thus sensor has given same value severl times),
set 'sensor's new value'.  Analog values need an averaging somehow..
*/
void readsensors() {
  // button1State, button1Pin, button1Led 
  if (digitalRead(button1Pin) != button1State) {
    button1Count--;
    if(button1Count==0) {
      button1State=digitalRead(button1Pin);
      if(button1State==LOW){                      // When button is down, run toggle routine.
        if(state==state_pause) {
          state=state_explore;
          digitalWrite(button1Led, HIGH);
        }
        else {
          state=state_pause;
          digitalWrite(button1Led, LOW);
        }
      }
    }
  }
  else {
    button1Count=10;	
  }

  // Edge detect 1 
  if (digitalRead(edge1Pin) != edge1State) {
    edge1Count--;
    if(edge1Count==0) {
      edge1State=digitalRead(edge1Pin);
    }
  }
    else {
      edge1Count=3;	
    }

  // Battery protect, only throw an error after more than 300ms at less than 3.0v
  // LiPo batt: Max:859(4.2v) Nom:747(3.7v) Min:613(3.0v)
  if (analogRead(battPin) <= 650 ) {
    battCount--;
    if(battCount==0) {
      boterror=1;                  // If it's still a problem, toggle flag.
      laststate=state;
      state=state_error;
    }
  }
    else {
      battCount=100;        // 100 * 3 ms	
    }

  // Analong distance sensors.
  // Front IR
  dist1Value=map(analogRead(dist1Pin),100,600,1,10);
}  // End readsensors()

/* ----------------------------------------------------------------------------------------------------------------------- */

// Collision, bump or object detection. Both hit somthing and from what direction.
void colldetect() {
  if(dist1Value >= 5 ) {          // Front center distance.
    bumpdetect=1;
  }
  else if(edge1State == 1) {      // Front center edge
    bumpdetect=1;
  }
  else {
    bumpdetect=0;                  // We hit 'nutt'n!
  }
} // end colldetect()

/* ----------------------------------------------------------------------------------------------------------------------- */

void serialinput() {
    char val = Serial.read();
  Serial.println(val, BYTE); 
switch(val) // Perform an action depending on the command
{
  case 'w'://Move Forward
    state=state_move;
    laststate=state_pause;
    movedir=dir_forward;
    movecount=10000;
  break;
  case 's'://Move Backwards
    state=state_move;
    laststate=state_pause;
    movedir=dir_back;
    movecount=10000;
  break;
  case 'a'://Turn Left
    state=state_move;
    laststate=state_pause;
    movedir=dir_left;
    movecount=10000;
  break;
  case 'd'://Turn Right
    state=state_move;
    laststate=state_pause;
    movedir=dir_right;
    movecount=10000;
  break;
  case 'e': // Explore
    state=state_explore;
  break;
  case 'r':  // Reset settings.
    lastbump=0;
    state=state_pause;
    laststate=state_pause;
    setupstate=state_pause;
    movecount=0;
    movedir=0;
    boterror=0;
    avoid=0;
    analogWrite (E2,0);
    analogWrite (E1,0);  
  break;
  case '+':
    m2_max++;
    analogWrite (E2,m2_max);
  break;
  case '-':
    m2_max--;
    analogWrite (E2,m2_max);
  break;
  default:
    state=state_pause;
  break;
  }
} // end serialinput();

/* ----------------------------------------------------------------------------------------------------------------------- */

void debugmessage() {
    Serial.print("(");
  Serial.print(millis(),HEX);
  Serial.print("): Batt(");
//  Serial.print(analogRead(A0)*0.00489,DEC);
  Serial.print(analogRead(battPin),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.print(") Bump(");
  Serial.print(bumpdetect,DEC);
  Serial.print(") Error(");
  Serial.print(boterror,DEC);
  Serial.print(") Speed(");
  Serial.print(m1_max,DEC);
  Serial.print(":");
  Serial.print(m2_max,DEC);

  Serial.print(") Inputs(");
  Serial.print(button1State,DEC);
  Serial.print(":"); 
  Serial.print(edge1State,DEC);
  Serial.print(":"); 
  Serial.print(dist1Value,DEC);
 
  Serial.println(")");
}  // end debugmessage();

/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */

/* Pause state. */
void dostate_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");
      analogWrite (E2,0);       // Stop motors
      analogWrite (E1,0);
      digitalWrite(ledPin, HIGH);
    }
  setupstate=state;
  }
} // end dostate_pause();

/* ----------------------------------------------------------------------------------------------------------------------- */

/* Normal travel  */
void dostate_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;
    analogWrite (E2,m2_max);
    analogWrite (E1,m1_max);
    digitalWrite(ledPin, LOW);
  }
    
  // Basic 'stop and throw an error' code, actual avoidance is done elseware.
  if (bumpdetect >= 1) {		// Have we hit somthing?
    if ((millis() - lastbump)  >= 100) {        // And motors have been mmoving for a bit
      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;
    return;
  }
  
  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;
  }
} // end dostate_move();

/* ----------------------------------------------------------------------------------------------------------------------- */

void dostate_explore() {
  // Robot explore; roomba mode
//    if(setupstate != state) {
//      movedir=dir_forward;
      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
    laststate=state;
    state=state_move;
    movedir=dir_forward;
    movecount=30000;
  break;
  }
}  // End dostate_explore();

/* ----------------------------------------------------------------------------------------------------------------------- */

// Handle an errror, mostly pause and wait
void dostate_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--;
    }
} // End dostate_error();

/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------------------------------------------------- */