/*
 * KU Corobot Example: Tilt Control
 * 
 * After turning both Corobots on, wait until they are connected.
 * The left yellow LED on the controller board will light up when connected. 
 *
 * Press the switch on the Corobot you want to move.  Its movement can be
 * controlled by tilting the other Corobot.
 *   - Tilt down: forward
 *   - Tilt up: stop
 *   - Tilt down-left: curve left
 *   - Tilt down-right: curve right
 *   - Tilt left: turn left
 *   - Tilt right: turn right
 */
#include <Wire.h>
#include <Adafruit_TCS34725.h>
#include <Corobot.h>

#define UPDATE_INTERVAL 100  // status update interval in ms

struct Status {
  int8_t pitch;
  int8_t roll;
};

Status localStatus,remoteStatus;
bool started;
long nextUpdate;

void setup() {
  Corobot.init();
  Corobot.calibrateAccel(); // calibrate using the current position
  started = false;
  remoteStatus.pitch = 0;
  remoteStatus.roll = 0;

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

  nextUpdate = millis() + UPDATE_INTERVAL;
}

void move() {
  bool forward = (remoteStatus.pitch > 10);
  bool left = (remoteStatus.roll < -10);
  bool right = (remoteStatus.roll > 10);
  
  if (forward & !left & !right)  // go straight
    Corobot.setMotors(FORWARD,80,FORWARD,80);

  if (forward & left & !right)   // curve left
    Corobot.setMotors(FORWARD,40,FORWARD,80);

  if (forward & !left & right)   // curve right
    Corobot.setMotors(FORWARD,80,FORWARD,40);

  if (!forward & !left & !right) // stop
    Corobot.setMotors(FORWARD,0,FORWARD,0);

  if (!forward & left & !right)  // turn left
    Corobot.setMotors(FORWARD,0,FORWARD,80);

  if (!forward & !left & right)  // turn right
    Corobot.setMotors(FORWARD,80,FORWARD,0);
}

void loop() {
  if (millis() > nextUpdate) {
    // Update my tilting status to my friend
    localStatus.pitch = Corobot.getPitch();
    localStatus.roll = Corobot.getRoll();
    Corobot.remote.write((char*)&localStatus,sizeof(Status));
    nextUpdate = millis() + UPDATE_INTERVAL;
  }

  // If available, get status update from my friend
  if (Corobot.remote.available() >= sizeof(Status)) {
    Corobot.remote.readBytes((char*)&remoteStatus,sizeof(Status));
  }

  // Start movement when switch is pressed
  if (Corobot.switchPressed()) {
    started = true;
    Corobot.setRightLed(true);
  }

  if (started) move();
}
