/*
 * KU Corobot Example: Track line and read color
 * 
 * Follow black track and stop on every color patch to read the color.  The
 * color code is displayed on the 3-color LEDs as binary.
 */
#include <Wire.h>
#include <Adafruit_TCS34725.h>
#include <Corobot.h>

#include "colors98.h"

#define CRUISE_SPEED    50

enum State {
  SEEK_COLOR,  // Track line and look for a color patch

  STOP_WAIT,   // Found a color patch and ordered motors to stop;
               // wait a bit for complete stop

  IGNORE_COLOR // Track line and ignore any color patch
};

long until;
State state;

void setup() {
  Corobot.init();
  Corobot.calibrateColor(
    RAW_BLACK,
    RAW_WHITE,
    RAW_RED,
    RAW_GREEN,
    RAW_BLUE,
    RAW_YELLOW,
    RAW_MAGENTA);
  Corobot.waitSwitchPress();
  state = SEEK_COLOR;
}

void track() {  
  bool lt = Corobot.getLeftEye();
  bool rt = Corobot.getRightEye();
  bool ct = Corobot.getCenterEye();

  if (lt && rt)
    Corobot.setMotors(FORWARD,CRUISE_SPEED,FORWARD,CRUISE_SPEED);
  if (!lt && rt)
    Corobot.setMotors(FORWARD,0,FORWARD,CRUISE_SPEED);
  if (lt && !rt)
    Corobot.setMotors(FORWARD,CRUISE_SPEED,FORWARD,0);

  // Always turn right at any intersection
  if (!lt && !rt) 
    Corobot.setMotors(FORWARD,CRUISE_SPEED,FORWARD,0);
}

void do_seek_color() {
  // Am I on a color patch?
  if (Corobot.getCenterEye()) {
    // On a color patch; stop motors and wait doing nothing for one second
    Corobot.setMotors(FORWARD,0,FORWARD,0);
    state = STOP_WAIT;
    until = millis() + 1000;
  }
  else {
    // Not on a color patch; keep tracking
    track();
  }
}

void do_stop_wait() {
  // Stay waiting until the 'until' time is reached
  if (millis() < until) return;

  // The robot should stop completely, ready for color checking
  Color c = Corobot.getColor();
  Corobot.setLedValue((int)c);
  
  // Continue tracking but ignore color patch for one second
  state = IGNORE_COLOR;
  until = millis() + 1000;
}

void do_ignore_color() {
  if (millis() < until) {
    // The 'until' time is not yet reached, track line without caring about
    // any color patch underneath
    track();
  }
  else {
    // It's time to start seeking color again
    state = SEEK_COLOR;
  } 
}

void loop() {
  if (state == SEEK_COLOR)
    do_seek_color();
  else if (state == STOP_WAIT)
    do_stop_wait();
  else if (state == IGNORE_COLOR)
    do_ignore_color();
}
