/*
 * KU Corobot Example: Seek Same Color
 *
 * After turning both Corobots on, wait until they are connected.
 * The left yellow LED on the controller board will light up when connected. 
 *
 * Place one Corobot on a color patch.  Then press the switch on the other
 * Corobot you want it to find a color.  The moving Corobot will follow a
 * black line and stop on a patch with the same color the first robot is on.
 *
 * The three LEDs on the basic I/O board will show the color code of the most
 * recently recognized color on both Corobots.
 */
#include <Wire.h>
#include <Adafruit_TCS34725.h>
#include <Corobot.h>
#include "colors99.h"

#define CRUISE_SPEED    60
#define UPDATE_INTERVAL 250

enum State {
  STOP,        // Stop movement and send color to the remote periodically

  SEEK_COLOR,  // Track line and seek the color sent from the remote

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

  IGNORE_COLOR // Track line while ignoring the color
};

struct Status {
  Color color;
};

Status remoteStatus;
long until;
State state;

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

  Corobot.waitRemote();
  Corobot.setLeftLed(true);

  remoteStatus.color = UNKNOWN;
  until = millis() + UPDATE_INTERVAL;
}

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_stop() {
  // If on a color patch, send color to remote
  // while making sure that the time of UPDATE_INTERVAL has passed
  if (millis() > until) {
    Color c = Corobot.getColor();
    Corobot.setLedValue((int)c);
    Status localStatus;
    localStatus.color = c;
    Corobot.remote.write((char*)&localStatus,sizeof(Status));
    until = millis() + UPDATE_INTERVAL;    
  }

  // Start seeking color when switch is pressed
  if (Corobot.switchPressed()) {
    state = SEEK_COLOR;
    Corobot.setRightLed(true);
  }
}

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);
  
  if (c == remoteStatus.color) {
    state = STOP;
    Corobot.setRightLed(false);
  }
  else {
    state = IGNORE_COLOR;
    until = millis() + 1000; // ignore color for 1 second    
  }  
}

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() {
  // Read the color code sent by the other Corobot, if available
  if (Corobot.remote.available() >= sizeof(Status)) {
    Corobot.remote.readBytes((char*)&remoteStatus,sizeof(Status));
  }  
  
  if (state == STOP)
    do_stop();
  else if (state == SEEK_COLOR)
    do_seek_color();
  else if (state == STOP_WAIT)
    do_stop_wait();  
  else if (state == IGNORE_COLOR)
    do_ignore_color();
}
