/*
Rover - 1.0
Buhatkj - Oct/2008
*/
#include <ServoTimer1.h>

//mimimum safe distance in front of us in inches
int minSafeDist = 7;

//PING and pan servo head
unsigned long echo = 0;
int ultraSoundSignal = 12; // Ultrasound signal pin
unsigned long ultrasoundValue = 0;
int servoPin = 10;
ServoTimer1 servo1;
int SERVOCENTER = 97;
int SERVOLEFT = 67;
int SERVORIGHT = 127;

//motor 1 = right
int m1a = 4;
int m1b = 3;
int m1e = 2;

//motor 2 = left
int m2a = 7;
int m2b = 6;
int m2e = 5;

//pan ping distance vars all in inches
int leftDist = 0;
int rightDist = 0;
int centerDist = 0;

//for debugging
int ledPin = 13;                // LED connected to digital pin 13

//this is the cheapest, crappiest circular buffer evar
int actionBuffer[10];

void initBuffer(){
 for(int i = 0; i < 10; i++){
   actionBuffer[i] = 0;
 }
}

void pushBuffer(int x){
 for(int i = 0; i < 9; i++){
   actionBuffer[i+1] = actionBuffer[i];
 }
 actionBuffer[0] = x;
}

int oscillating(){
  int osc = 0;
  for(int i = 0; i < 5; i++){
   if ((actionBuffer[i] == 1)||(actionBuffer[i] == 2)){
     osc++;
   }
  }
  return osc;
}

void setup()                    // run once, when the sketch starts
{
  //init all pins
  pinMode(ledPin, OUTPUT);      // sets the digital pin as output
  
  pinMode(ultraSoundSignal,OUTPUT);
  pinMode(servoPin, OUTPUT);      // sets the digital pin as output
  servo1.attach(servoPin);
  servo1.write(97); //center our servo
  
  pinMode(m1a, OUTPUT);      // sets the digital pin as output
  pinMode(m1b, OUTPUT);      // sets the digital pin as output
  pinMode(m1e, OUTPUT);      // sets the digital pin as output
  
  pinMode(m2a, OUTPUT);      // sets the digital pin as output
  pinMode(m2b, OUTPUT);      // sets the digital pin as output
  pinMode(m2e, OUTPUT);      // sets the digital pin as output
  
  //enable the motors
  digitalWrite(m1e, HIGH); // on
  digitalWrite(m2e, HIGH); // on
  
  initBuffer();
  
  Serial.begin(9600);
  Serial.println("starting...");
  
  //SelfTest();
  //Serial.println("done self test...");
}

unsigned long ping(){
   pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output
   digitalWrite(ultraSoundSignal, LOW); // Send low pulse
   delayMicroseconds(2); // Wait for 2 microseconds
   digitalWrite(ultraSoundSignal, HIGH); // Send high pulse
   delayMicroseconds(5); // Wait for 5 microseconds
   digitalWrite(ultraSoundSignal, LOW); // Holdoff
   pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input
   digitalWrite(ultraSoundSignal, HIGH); // Turn on pullup resistor
   echo = pulseIn(ultraSoundSignal, HIGH); //Listen for echo
   ultrasoundValue = (echo / 58.138) * .39; //convert to CM then to inches
   Serial.println(ultrasoundValue);
   return ultrasoundValue;
 }
 
 void AllStop(){
    digitalWrite(m2e, LOW); // off
    digitalWrite(m1e, LOW); // off
 }
 
 void AllForward(){
   digitalWrite(m2e, HIGH); 
   digitalWrite(m2a, HIGH); 
   digitalWrite(m2b, LOW); 
   digitalWrite(m1e, HIGH); 
   digitalWrite(m1a, HIGH); 
   digitalWrite(m1b, LOW); 
 }
 
 void AllReverse(){
   digitalWrite(m2e, HIGH); 
   digitalWrite(m2a, LOW); 
   digitalWrite(m2b, HIGH); 
   digitalWrite(m1e, HIGH); 
   digitalWrite(m1a, LOW); 
   digitalWrite(m1b, HIGH);
 }
 
 void SpinRight(){
   digitalWrite(m2e, HIGH); 
   digitalWrite(m2a, LOW); 
   digitalWrite(m2b, HIGH); 
   digitalWrite(m1e, HIGH); 
   digitalWrite(m1a, HIGH); 
   digitalWrite(m1b, LOW);
 }
 
 void SpinLeft(){
   digitalWrite(m2e, HIGH); 
   digitalWrite(m2a, HIGH); 
   digitalWrite(m2b, LOW); 
   digitalWrite(m1e, HIGH); 
   digitalWrite(m1a, LOW); 
   digitalWrite(m1b, HIGH);
 }
 
 void LookAround(){
  //look around
  servo1.write(SERVOLEFT); //left servo
  delay(175);                  // waits for a second
  leftDist = ping();
  servo1.write(SERVOCENTER); //center our servo
  delay(175);                  // waits for a second
  centerDist = ping();
  servo1.write(SERVORIGHT); //right servo
  delay(175);                  // waits for a second
  rightDist = ping();
  servo1.write(SERVOCENTER); //center our servo
  delay(175);                  // waits for a second
  Serial.println(leftDist);
  Serial.println(centerDist);
  Serial.println(rightDist);
 }
 
 void LookAhead(){
   //servo1.write(SERVOCENTER); //center our servo
   //delay(175);                  // waits for a second
   centerDist = ping();
 }
 
 void loop()                     // run over and over again
{
  digitalWrite(ledPin, HIGH);   // sets the LED on
   LookAhead();
   //LookAround();
     
   if(centerDist >= minSafeDist){
     AllForward();
     pushBuffer(0);
     delay(100);
   }else{
     //stop
     AllStop();
     delay(100); 
     
     //have a look around
     LookAround();
     if((oscillating() > 2)||(rightDist == leftDist)){
       //best to turn around
       SpinLeft();
       initBuffer();
       pushBuffer(3);
       delay(2000);
     //go in the direction that read farthest
     }else if(leftDist > rightDist){
       //best to turn left
       SpinLeft();
       pushBuffer(1);
       delay(500);
     }else if(rightDist >leftDist){
       //best to turn right
       SpinRight();
       pushBuffer(2);
       delay(500); 
     }else{
       //this should NEVER happen
       //best to turn around
       SpinLeft();
       initBuffer();
       pushBuffer(3);
       delay(2000);
     }
   }
  delay(100);  
  digitalWrite(ledPin, LOW);    // sets the LED off
}