#include <SendOnlySoftwareSerial.h>
#include <ArduinoInit.h>


boolean forwards = true, black = true, targetFound = false;
unsigned int sensor0, sensor1, sensor2, sensor3;
unsigned int floorThreshold = 20000;
unsigned int targetThreshold = 18000;
unsigned int navThreshold = 38000;
unsigned int homecolorFunc();
boolean currentcolor;
boolean homecolor;


void setup()
{
  configArduino ();
  outputHigh (4);

  attachInterrupt (0, bumper_int, LOW);
  attachInterrupt (1, bumper_int, LOW);
  floorSensor();

  //calls sensor to determine homecolor
  sensor0 = readADC(0);
  if (sensor0 >= floorThreshold) homecolor = 0; //0=black 1=white
  else homecolor = 1;
}

void loop()
{

  sensor0 = readADC(0); //floor
  Serial.print("Floor: ");
  Serial.println(sensor0);
  sensor1 = readADC(1); //nav
  Serial.print("Nav: ");
  Serial.println(sensor1);
  sensor2 = readADC(2); //right
  Serial.print("Right: ");
  Serial.println(sensor2);
  sensor3 = readADC(3); //left
  Serial.print("Left: ");
  Serial.println(sensor3);

  floorSensor();
  if (homecolor == currentcolor)
  {
    navLight();
  }
  else
  {
    if (!targetFound)
    {
      targetLight();
    }
  }

  pause(200);
}


void targetLight ()
{
  if (sensor2 < targetThreshold)
  {
    targetFound = true;
    right();
    pause(200); //TODO Determine this
    halt();
    pause(100);
    forward();
  }
  else if (sensor3 < targetThreshold)
  {
    targetFound = true;
    left();
    pause(400); //TODO Determine this
    halt();
    pause(100);
    forward();
  }
}

void navLight()
{
  if (sensor1 > navThreshold)
  {
    right();
  }
  else
  {
    forward();
  }
}

void floorSensor()
{
  if (sensor0 >= floorThreshold)
  {
    currentcolor = 0; //0=black
  }
  else
  {
    currentcolor = 1;
  }
}



void bumper_int () // This function will be automatically called upon an interrupt
{ // 1 means free, 0 means pressed
  pause (5);                                              // Pause 5 msec to allow for bumper switch contact bounce
  if (readInput (2) == 1 && readInput (3) == 1)
    return;                                               // No bumper hit, return
  halt();                                                 //Turn both motors off, retain the last speed setting
  pause (100);                                            //Always allow time (100ms) for the motors to stop
  while (readInput (2) == 0 || readInput (3) == 0)        //While a bumper is hit
  {
    if (targetFound)
    {
      targetFound = false;
    }
    if (readInput(2) == 0)
    {
      reverse();
      pause(100);
      halt();
      pause(100);
      left();
      pause(200);
      halt();
      pause(100);
    }
    else if (readInput(3) == 0)
    {
      reverse();
      pause(100);
      halt();
      pause(100);
      right();
      pause(200);
      halt();
      pause(100);
    }
  }
  forward();
  return;
}

void reverse()
{
  motors('1', 'a', 70);
  motors('2', 'a', 70);
}
void left()
{
  motors('1', 'b', 25);
  motors('2', 'a', 25);
}
void right()
{
  motors('1', 'a', 25);
  motors('2', 'b', 25);
}
void halt()
{
  motors('1', 'o', 0);
  motors('2', 'o', 0);
}
void forward()
{
  motors('1', 'b', 70);
  motors('2', 'b', 70);
}