//--------------------------------------------------------------------------
// Library include stuff:
#include <AFMotor.h>  // needed for the motorshield.
#include <Servo.h>  // motorshield needs this for running servos.

//--------------------------------------------------------------------------

#define trigPin 15 // for distance sensor.
#define echoPin 14 // for distance sensor.

long g_collideDist = 40; // in cm, how far before collision mode kicks in?
long g_cm = 300; // the current ping reading, just ana initial default.
boolean g_checkLeft = false; // used for collision detection
boolean g_checkRight = false; // used for collision detection
boolean g_checkCenter = false; // used for collision detection
int g_leftDist = 0;  // used for collision detection
int g_rightDist = 0;  // used for collision detection

// motor stuff:
AF_DCMotor g_motor1(1, MOTOR12_8KHZ); //  front right
AF_DCMotor g_motor2(2, MOTOR12_8KHZ); //  front left
AF_DCMotor g_motor3(3, MOTOR12_8KHZ); //  back left
AF_DCMotor g_motor4(4, MOTOR12_8KHZ); //  back right
byte g_motorSpeed = 200; // max is 255

// Mode (state) stuff:
byte g_mode = 1; // 1 = drive, 2 = proximity check, 3 = turning
byte g_driveDir = -1; // 0 = stop, 1 = straight, 2 = right, 3 = left

// servo stuff:
Servo g_servo2;
int g_servo2pin = 9; // servo2 uses digital pin 9, even though the docs say pin 10...
int g_servoCenter = 98; // in degrees.  98 is straight ahead based on how I mounted the servo (it must be
    // a little inaccurate).  full left is 180, full right is 0.
    // Servo is flipped upside down, fyi
int g_servoRight = 135; // how far right?  Max is 180, full right.
int g_servoLeft = 45; // how far left?  Min is 0, full left.

// time stuff
unsigned long g_time;  // global time tracker
unsigned long g_ptime;  // global previous time tracker
unsigned long g_elapsed = 0;  // helps keeping track of framerate.
unsigned long g_interval = 33; // in ms (about 30fps).  This slows down the processing allowing the
    // multiple systems to play nicely together.  Make this number smaller and it won't have enough
    // time for each state to run properly:  The servo won't do a full sweep when scanning.
unsigned long g_turnlength = 750; // in ms, how long to turn for.  Since we don't know how far
    // it turns, we just guess on how long it should.  The type of terrain turing on has a great
    // effect on what this should be.  This value works well for my hardwood floors.
unsigned long g_turnUntil; // used to track how long to turn.

//--------------------------------------------------------------------------
// setup \ loop :

void setup() {
  // Attach & center the servo:
    Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  g_servo2.attach(g_servo2pin);
  g_servo2.write(g_servoCenter);  // center it by default
  // Setup our framerate stuff
  g_time = millis();
  g_ptime = g_time;
}

void loop() {
  // Main program entry point.
 
  // Need to keep the aiming and the piging at a fixed rate, or the closer the object gets
  // to the ping the faster it will aim, and do bad stuff.
  g_ptime = g_time;
  g_time = millis();
  g_elapsed += g_time - g_ptime;
  // Wait for the proper amount of time to pass before we do anything:
  if(g_elapsed < g_interval){
    return;
  }
  else{
    g_elapsed = 0;
  }

  //  Do stuff, based on the current mode:
  if(g_mode == 1){
    g_cm = ping();
    // sometimes the sensor returns a 0 value that can foul up things :(
    if(g_cm > g_collideDist || g_cm == 0){
      // If there is nothing in our way, drive!
      if(g_driveDir != 1){
        g_driveDir = 1;
        move(g_driveDir);
      }
    }
    else{
      // if we're too close to something, stop the motors and switch to 'proximity check' mode:
      if(g_driveDir != 0){
        g_driveDir = 0;
        move(g_driveDir);
      }
      // initialize the proximity check stuff:
      g_checkLeft = false;
      g_checkRight = false;
      g_checkCenter = false;
      g_leftDist = 0;
      g_rightDist = 0; 
      // switch to mode 2:
      g_mode = 2;   
    }
  }

  else if(g_mode == 2){
    // Do proximity check!
    boolean checked = proximityCheck(); 
    if(checked){
      // Define which direction to turn.
      // If the right distance is greater than the left distance, turn that direction,
      // otherwise go the other way:
      if(g_rightDist >= g_leftDist){
        g_driveDir = 2;
      }
      else{
        g_driveDir = 3;
      }
      // now that we've set which direction to turn, start timing how long the turn
      // has taken, and switch to mode 3.
      g_turnUntil = millis() + g_turnlength;
      g_mode = 3;
    }
  }

  else if(g_mode == 3){
    // We must be turning!  The logic for switching to other modes is in that function.
    turn();
  }
}

//--------------------------------------------------------------------------
// Helper functions:

void turn(){
  // Executed when g_mode == 3
  // Used to turn the robot.
 
  if (millis() < g_turnUntil){
    // keep'on turning...
    move(g_driveDir);
  } 
  else{
    // If we're done turning, see if there is anything still in our way:
    g_cm = ping();
    // sometimes the sensor returns a 0 value that can foul up things :(
    if(g_cm > g_collideDist || g_cm == 0){
      // If there is nothing in our way go back to mode 1:
      g_mode = 1;
    }
    else{
      // If stuff is still in our way, turn more in the same direction until
      // not blocked:
      g_turnUntil = millis() + g_turnlength;
    }
  }
}

boolean proximityCheck(){
  // Used when mode == 2:  Swing the ping left and right taking readings to
  // find which way is safe to go.  Returns false when in the middle of the check
  // operation, returns true when entire check process is complete.
 
  // Current angle of the servo:
  float angle = g_servo2.read();
 
  // check left first: ( full left is 0 deg)
  if(g_checkLeft == false){
    if(angle > g_servoLeft){
      g_servo2.write(angle-10) ;
      return false;   
    }
    else{
      g_checkLeft = true;
      g_leftDist = ping();
      return false;  
    }
  }

  // check right second: ( full right is 180 deg)
  if(g_checkLeft == true && g_checkRight == false){
    if(angle < g_servoRight){
      g_servo2.write(angle+10) ;
      return false;     
    }   
    else{
      g_checkRight = true;
      g_rightDist = ping();
      return false;
    }
  }

  // Go to center position third:
  if(g_checkLeft == true && g_checkRight == true && g_checkCenter == false){
    if(angle != g_servoCenter){
      g_servo2.write(angle-10) ;
      return false;
    }      
    else{
      g_checkCenter = true;
      return false;
    }
  }

  // Finally if we've checked left, right, and got back to center, return true
  // to tell the later code that the full check is complete:
  if(g_checkLeft == true && g_checkRight == true && g_checkCenter == true){
    return true;
  }
  else{
    return false;
  }
}

void move(byte mode){
  // This function moves the wheels.  Can be called to when mode == 1 (driving) or 3 (turning).
  if(mode == 0){
    // all stop!
    g_motor1.setSpeed(0);
    g_motor2.setSpeed(0);
    g_motor3.setSpeed(0);
    g_motor4.setSpeed(0);
  }
  else{
    // full speed!
    g_motor1.setSpeed(g_motorSpeed);
    g_motor2.setSpeed(g_motorSpeed);
    g_motor3.setSpeed(g_motorSpeed);
    g_motor4.setSpeed(g_motorSpeed);
   
    // full speed, but which direction?
    if(mode == 1){
      // go straight
      g_motor1.run(FORWARD);
      g_motor2.run(FORWARD);
      g_motor3.run(FORWARD);
      g_motor4.run(FORWARD);
    }
    else if(mode == 2){
      // turn right
      g_motor1.run(BACKWARD);
      g_motor2.run(FORWARD);
      g_motor3.run(FORWARD);
      g_motor4.run(BACKWARD); 
    }
    else if (mode == 3){
      // turn left
      g_motor1.run(FORWARD);
      g_motor2.run(BACKWARD);
      g_motor3.run(BACKWARD);
      g_motor4.run(FORWARD);
    }
  }
}

float ping(){
  // Returns the distance in cm from the ping sensor.  Pulled from
  // Arduino docs.
 
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
/*  pinMode(g_pingPin, OUTPUT);
  digitalWrite(g_pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(g_pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(g_pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(g_pingPin, INPUT);
  long duration = pulseIn(g_pingPin, HIGH); 
  float cm = microsecondsToCentimeters(duration);
  return cm;
  */
 
  int duration, distance;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
   float cm = distance;
  Serial.print(cm);
  return cm;
}

/*
long microsecondsToCentimeters(long microseconds){
  // Simple conversion setp, kept as a function simply to illustrate
  // how it works.  Pulled from Arduino docs.
 
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  //return microseconds / 29 / 2;
  return microseconds / 58;
}
*/