#define LEFT_WHEEL_FORWARD 3
#define LEFT_WHEEL_REVERSE 2
#define RIGHT_WHEEL_FORWARD 4
#define RIGHT_WHEEL_REVERSE 5
#define HBRIDGE_ENABLE 6
#define CAMERA_SERVO 6
#define SERIAL_BAUD 115200
#define THRESHOLD_FAULTS 3
#define THRESHOLD_GM 15

#include <Servo.h>

int mode=0;

//drives the robot in the specified direction, for a dist, pausing at the end for spacing
void drive(char dir, int dist, int spacing)
{
  pinMode(LEFT_WHEEL_FORWARD,OUTPUT);
  pinMode(LEFT_WHEEL_REVERSE,OUTPUT);
  pinMode(RIGHT_WHEEL_FORWARD,OUTPUT);
  pinMode(RIGHT_WHEEL_REVERSE,OUTPUT);
  pinMode(HBRIDGE_ENABLE,OUTPUT);
  
  digitalWrite(HBRIDGE_ENABLE,1);
  switch(dir)
  {
    case 'f':
      digitalWrite(LEFT_WHEEL_FORWARD,1);
      digitalWrite(RIGHT_WHEEL_FORWARD,1);
      break;
    case 'b':
      digitalWrite(LEFT_WHEEL_REVERSE,1);
      digitalWrite(RIGHT_WHEEL_REVERSE,1);
      break;
    case 'l':
      digitalWrite(LEFT_WHEEL_FORWARD,1);
      break;
    case 'r':
      digitalWrite(RIGHT_WHEEL_FORWARD,1);
      break;
    case 'L':
      digitalWrite(LEFT_WHEEL_FORWARD,1);
      digitalWrite(RIGHT_WHEEL_REVERSE,1);
      break;
    case 'R':
      digitalWrite(RIGHT_WHEEL_FORWARD,1);
      digitalWrite(LEFT_WHEEL_REVERSE,1);
      break;
  }
  delay(dist*10); 
  digitalWrite(HBRIDGE_ENABLE,0);  
  digitalWrite(LEFT_WHEEL_FORWARD,0);
  digitalWrite(LEFT_WHEEL_REVERSE,0);
  digitalWrite(RIGHT_WHEEL_FORWARD,0);
  digitalWrite(RIGHT_WHEEL_REVERSE,0);
  delay(spacing*10); 
}

//sends a command, ignoring the result
int cmuSet(char * command)
{
  int byteCount;
   
  //send command and \r
  Serial.print(command);
  Serial.print("\r");
  delay(10);
  
  //rcv "ack\r:"
  while(Serial.available() == 0);
  while(Serial.read() != ':');
  Serial.flush();
}

//sends a command, storing the result in buffer
int cmuGet(char * command, char * buffer)
{
  char returnedByte;
  int byteCount=0;

  //send command and \r
  Serial.print(command);
  Serial.print("\r");
  delay(10);
  
  //rcv "ack\r"
  while(Serial.available() == 0);
  while(Serial.read() != 13);

  //wait for actual data
  while(Serial.available() == 0);
  while(Serial.available() > 0)
  {
    returnedByte = Serial.read();
    buffer[byteCount]=returnedByte;
    byteCount++;
    delay(1);
  } 
  Serial.flush();
}

//returns an indexed integer from a delimeted c char array
int grabInt(char string[], char delim, int index)
{  
  //declarations
  int intAcc;
  int length;
  int start;
  int found;
  int maxchars;
  int arrIndex;
  
  //initial assignments
  maxchars=strlen(string);
  start=0;
  found=0;
  arrIndex=0;
  
  //clevers  
  while(found<=index)
  {
    intAcc=0;
    length=0;
    while(start+length < maxchars && string[start+length] != delim)
    {
      intAcc*=10;
      intAcc+=string[start+length]-48;
      length++;
    }
    start=start+length+1;
    found++;
  }
  return intAcc;
}

//sets the servo position
int setServo(int pin, int pos)
{
  Servo aServo;
  aServo.attach(pin);
  aServo.write(pos);
  for(int i=0;i<25;i++)  //half second to position
  {
    aServo.refresh();
    delay(20);
  }
  aServo.detach();
}

void setup()
{
}

void loop()
{
  //replaces setup()
  if(mode==0)
  {
    digitalWrite(13, HIGH);
    
    Serial.begin(SERIAL_BAUD);
    
    //prep camera
    cmuSet("MM 0");  //disable middle mass/auto servo
    cmuSet("PM 1");  //pulsed mode on

    //wait for camera to settle
    delay(10000);

    digitalWrite(13, LOW);
    
    mode=1;
  }
 
  //DRIVE FORWARD, AVOIDING OBSTICALS
  if(mode==1)
  {
    //prep servo
    //setServo(6,130);
    
    
  }
  while(mode==1)
  {
    //prep
    char bufferFar[30]="";
    char bufferClose[30]="";
    int faults=0;
    
    //determine if at obsticals
    cmuSet("SW 1 40 40 103");  //set window to center top
    cmuGet("GM", bufferFar);     //get mean colour of window
    cmuSet("SW 40 40 80 103"); //set window to center bottom
    cmuGet("GM", bufferClose);     //get mean colour of window 
        
    for(int i=0;i<6;i++)
    {
      if(abs(grabInt(bufferClose,' ',i+1) - grabInt(bufferFar,' ',i+1)) > THRESHOLD_GM)
        faults++;
    }

    //reverse and spin or continue
    if(faults > THRESHOLD_FAULTS)
    {
      drive('b',50,50);
      drive('R',100,50);
    }
    else
    {
      drive('f',50,0);
    }

  }
  
}