/*
 * KU Corobot Example: Remote Color
 * 
 * Read current floor color using the color sensor every 1/10 second, 
 * then transmit the color code via bluetooth.  The other Corobot displays the
 * color code on the LEDs on the basic I/O board.
 *
 */
#include <Wire.h>
#include <Adafruit_TCS34725.h>
#include <Corobot.h>
#include "colors99.h"

#define UPDATE_INTERVAL 250  // status update interval in ms

struct Status {
  Color color;
};

long nextUpdate;
Status remoteStatus;

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

  Corobot.waitRemote();
  Corobot.setLeftLed(true);
  nextUpdate = millis() + UPDATE_INTERVAL;
}

void loop() {
  // Update my color to the remote
  if (millis() >= nextUpdate) {
    Status local;
    local.color = Corobot.getColor();
    Corobot.remote.write((char*)&local,sizeof(Status));
    nextUpdate = millis() + UPDATE_INTERVAL;
  }

  // Receive remote color, if available
  if (Corobot.remote.available() >= sizeof(Status)) {
    Corobot.remote.readBytes((char*)&remoteStatus,sizeof(Status));
    Corobot.setLedValue(remoteStatus.color);
  }
}
