/*  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!   Bitwise motor controll and sensors.  And main loop profiling.  */
/*  SIZE:  6.5k of 30.7k max = 21%                        */

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

/* Directions */
#define dir_none       0
#define dir_forward    4
#define dir_forwleft   5
#define dir_forwright  6
#define dir_back       8
#define dir_backleft   9  
#define dir_backright  10
#define dir_spinleft   13
#define dir_spinright  14

/* Collisions */
#define coll_frontleft    1
#define coll_frontright   2     // 3 = front square collision
#define coll_rearleft     4
#define coll_rearright    8     // 12  = reverse square collision
#define coll_analog       16

/* Motor controll pins. */
const byte LeftSpeedPin = 6; //M1 Speed Control
const byte RightSpeedPin = 5; //M2 Speed Control
const byte LeftDirPin = 8; //M1 Direction Control
const byte RightDirPin = 7; //M2 Direction Control

/* Motor speeds */
byte MRight_max=255;
byte MLeft_max=255;          // Left slightly faster, so reduse top speed.

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

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

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

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

/* Second/Left edge detection, digital */
const byte edge2Pin = 9;
byte edge2State=0;
byte edge2Count=5;

/* Third/Back edge detection, digital */
const byte edge3Pin = 10;
byte edge3State=0;
byte edge3Count=5;

/* IR or Sonic distance sensors, analog */
const byte dist1Pin = A0;
byte dist1Value=0;
const byte dist2Pin = A1;
byte dist2Value=0;

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

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

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

/* Profiling counters  */
unsigned int profilecount=0;
unsigned int profilestore=0;

/* Error modes/settings  */
byte boterror=0;
byte 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 (LeftSpeedPin,0);                // Motor stopped.
analogWrite (RightSpeedPin,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!

pinMode(edge1Pin,INPUT);
pinMode(edge2Pin,INPUT);
pinMode(edge3Pin,INPUT);
digitalWrite(edge3Pin, HIGH);

Serial.begin(19200);                // 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)  >= 1) {        // Every 1mS             
    readsensors();                             // Read in all sensors
    colldetect();                              // Collision detection from sensors stuff.
    sensortimer=millis();
    // Once every mS lets save the profile count.
    profilestore=profilecount;
    profilecount=0;
  }

  // How many times around the main loop.
  profilecount++;
  
  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();                     // Reset this in debugmessage()
  }

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

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

}   // End of loop();


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

/* Read in sensors
Every 1ms, 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=30;	
  }

  // Edge detect 1 
  if (digitalRead(edge1Pin)==HIGH) {
    edge1Count--;
    if(edge1Count==0) {
      edge1State=HIGH;
    }
  }
    else {
      edge1Count=9;
      edge1State=LOW;	
    }

  // Edge detect 2 
  if (digitalRead(edge2Pin) == HIGH) {
    edge2Count--;
    if(edge2Count==0) {
      edge2State=HIGH;
    }
  }
    else {
      edge2Count=9;
      edge2State=LOW;	
    }
    
  // Edge detect 3 
  if (digitalRead(edge3Pin) == HIGH) {
    edge3Count--;
    if(edge3Count==0) {
      edge3State=HIGH;
    }
  }
    else {
      edge3Count=9;
      edge3State=LOW;	
    }
  
  // 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)
  battState=map(constrain(analogRead(battPin),613,900),613,900,1,10);
  if (battState <= 1 ) {
    battCount--;
    if(battCount==0) {
      boterror=1;                  // If it's still a problem, toggle flag.
      laststate=state;
      state=state_error;
    }
  }
    else {
      battCount=500;        // 300 ms	
    }

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

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

// Collision, bump or object detection. Both hit somthing and from what direction.
// Needs to include OR for left + right at same time, ie, front square collision.
void colldetect() {
  if(dist1Value >= 5 ) {          // Right front distance.
    bumpdetect=+coll_frontright+coll_analog;
  }
  else if(dist2Value >= 5 ) {          // Left front distance.
    bumpdetect=+coll_frontleft+coll_analog;
  }
  else if(edge1State == HIGH) {      //  Right front edge
    bumpdetect=+coll_frontright;
  }
  else if(edge2State == HIGH) {      //  Left front edge
    bumpdetect=+coll_frontleft;
  }  
  else if(edge3State == HIGH) {      //  Both rear edges
    bumpdetect=+coll_rearleft+coll_rearleft;
  }  
  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;
    setupstate=state_pause;
    laststate=state_pause;
    movedir=dir_forward;
    movecount=10000;
  break;
  case 's'://Move Backwards
    state=state_move;
    setupstate=state_pause;
    laststate=state_pause;
    movedir=dir_back;
    movecount=10000;
  break;
  case 'a'://Turn Left
    state=state_move;
    setupstate=state_pause;
    laststate=state_pause;
    if(movedir==dir_forwleft) {
      movedir=movedir + 8;
    }
    else {
      movedir=dir_forwleft;
    }
    movecount=10000;
  break;
  case 'd'://Turn Right
    state=state_move;
    setupstate=state_pause;
    laststate=state_pause;
    if(movedir==dir_forwright) {
      movedir=movedir + 8;
    }
    else {
      movedir=dir_forwright;
    }
    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;
    changedir(movedir); 
  break;
  case '+':
    MLeft_max++;
    analogWrite (LeftSpeedPin,MLeft_max);
  break;
  case '-':
    MLeft_max--;
    analogWrite (LeftSpeedPin,MLeft_max);
  break;
  default:
    state=state_pause;
  break;
  }
} // end serialinput();

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

// Control motors. Send the robot off in the right Speed and direction. Or stop it.
// All done bitwise. Blaim Issac. 0:Off, 1:Left 2:Right 4:Forward 8:Reverse 16+: 4 bits of speed. 
// To impliment: movedir -> moveprev 
void changedir(byte dir) {

  // LEDs on if theres somthing non-zero.
  if(dir) {
    digitalWrite(ledPin, LOW);
  }
  
  // Save current max speeds.
  byte MRight_speed=MRight_max;
  byte MLeft_speed=MLeft_max;

  // If speed control bytes are there, change max speeds.
  byte dirspeed=dir&B11110000;
  if(dirspeed) {
    MRight_speed=MRight_max/2;
    MLeft_speed=MLeft_max/2;
  }
  
  // Isolate track direction bytes, the two LSBs
  byte dirtrack=(dir&B00000011)^B00000011;
  
  // Motor direction controll. Off and Spin are exceptions.
  dir=dir&B00001100;  
  switch(dir) {            
    case B00000000:        // 0: Off
      analogWrite (LeftSpeedPin,0);        // Motors both off.
      analogWrite (RightSpeedPin,0);
      digitalWrite(ledPin, HIGH);          // LEDs off too.
      return;
    break;
    case B00000100:    // 3: Forward 
      digitalWrite(RightDirPin,LOW);
      digitalWrite(LeftDirPin,LOW); 
    break;
    case B00001000:   // 8: Reverse 
      digitalWrite(RightDirPin,HIGH);
      digitalWrite(LeftDirPin,HIGH);
    break;
    case B00001100:  // 11: Spin -- Anoying exception to clean logic
      analogWrite (LeftSpeedPin,MLeft_speed);              // Set speeds here.    
      analogWrite (RightSpeedPin,MRight_speed);            // We'll need to bail early.
      if(dirtrack&B00000001) {            // Spin Left
        digitalWrite(RightDirPin,HIGH);
        digitalWrite(LeftDirPin,LOW);
      }
      else {                              // Maybe right?
        digitalWrite(RightDirPin,LOW);
        digitalWrite(LeftDirPin,HIGH);
      }
      return;        // Leave before we get to speed and select logic.      
    break;
  } 
  
  //  What track has power, switch on/off as required.
  if(dirtrack&B00000001) {    // Left track logic
    analogWrite (LeftSpeedPin,MLeft_speed);    
  }
  else {
    analogWrite (LeftSpeedPin,0);
  }
  
  if(dirtrack&B00000010) {    // Right track logic
    analogWrite (RightSpeedPin,MRight_speed); 
  }
  else {
     analogWrite (RightSpeedPin,0);
  }

}  // end changedir();

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

void debugmessage() {
    Serial.print("(");
  Serial.print(millis(),HEX);

  Serial.print(")(");
  Serial.print(profilestore,DEC);
  
  Serial.print("): Batt(");
//  Serial.print(analogRead(A0)*0.00489,DEC);
  Serial.print(battState,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(MRight_max,DEC);
  Serial.print(":");
  Serial.print(MLeft_max,DEC);
  Serial.print(") Inputs(B1:");
  Serial.print(button1State,DEC);
  Serial.print(" E1:"); 
  Serial.print(edge1State,DEC);
  Serial.print(" E2:"); 
  Serial.print(edge2State,DEC); 
  Serial.print(" E3:"); 
  Serial.print(edge3State,DEC);   
  Serial.print(" D1:"); 
  Serial.print(dist1Value,DEC);
  Serial.print(" D2:"); 
  Serial.print(dist2Value,DEC); 
  Serial.println(")");

  debugtimer=millis();
}  // end debugmessage();

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

/* Pause state. */
byte dostate_pause() {
  if(setupstate != state) {
    if ( movecount != 0 ) {       // Debug, we got here because of a sensor trigger.
      Serial.print("STOP :");
      debugmessage();
      movedir=0;
      movecount=0;
      changedir(movedir);
    }
  setupstate=state;
  }
return state_pause;
} // end dostate_pause();

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

/* Normal travel  */
byte dostate_move() { 
  if(setupstate != state) {
    if(movedir==dir_none) {
      return state_pause;
    }
    changedir(movedir);
    lastbump=millis();            // 'reset' bump timer.
    setupstate=state;
  }

  if ((millis() - movetimer)  >= 1) {
    movecount--;            // This will be driven by the wheel decoders later
    movetimer=millis();      // For now, it's just a milisecond timer.
  }
  
  // Somthing stopped us, return.
  if (bumpdetect >= 1) {		// Have we hit somthing?
    if ((millis() - lastbump)  >= 100) {        // And motors have been mmoving for a bit
      return laststate;    	        	// We're done moving
    }
  }
  
  // Stuck detect.  No bumps for too long. Throw an error!
  if ((millis() - lastbump)  >= 30000) {
    boterror=2;                  // So you know this triggerd it
    laststate=state;
    return state_error;
  }

  // Moved the required distance, we're done.
  if (movecount==0) { 
    return laststate;
  }
  
return state_move;
} // end dostate_move();

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


// Has avoid only complete when forward is sevel counts, has to allow continuing in same direction.
// Needs to use analog distance sensors to  turn before collision, no need to reverse.
// Needs 'turn until sensors clear' mode. And know diffrence beeween front and left/right collisions.
// 'avoid' must maintain direction state to allow above, including reversing and left/right planned turns. Done bitwise.
// ie bit one is avoud mode, bit 2 is left vs. right. Bit 3 is 'allready tried'. 
// Edge avoid: reverse until sensors clear plus a bit, turn in direction. 

byte dostate_explore() {
//    if(setupstate != state) {
//      movedir=dir_forward;
      setupstate=state;
//    }
    
    if ( movecount != 0 ) {          // A move stopped before time!
      if (movedir==dir_forward){
        avoid=2;
      }
      else {                         // Hit somthing doing avoid, so turn more
        avoid=1;
      }
    }

  switch(avoid){
  case 2:        // Reverse robot for 1 second
    movedir=dir_back;
    movecount=1000;
    avoid=1;            // Next avoid mode
    laststate=state;
    return state_move;
  break;
  case 1:              // Turn to avoid
    if(bumpdetect==1) {    // Right sensor     
      movedir=dir_spinleft;
    }
    else {
      movedir=dir_spinright;
    }
    movecount=random(100,1000);
    avoid=0;          // Done avoiding
    laststate=state;
    return state_move;
  break;             
  default:              // We are not avoiding
    movedir=dir_forward;
    movecount=30000;
    laststate=state;
    return state_move;
  break;
  }

return state_explore;
}  // End dostate_explore();

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

// Handle an errror, mostly pause and wait
byte dostate_error() {
    if(setupstate != state){    // We've had an error! Kill the 'bot
    movedir=0;
    changedir(movedir);    // 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--;
    }
return state_error;
} // End dostate_error();

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