/*
 * KU Corobot Example: Sensor Reader
 * 
 * Every time the switch on the basic I/O board is pressed, print out on
 * Serial Monitor raw values from the color sensor, as well as the color name
 * it recognizes.
 *
 * Sending the character 'c' from the Serial Monitor will calibrate the
 * accelerometer.
 */
#include <Wire.h>
#include <Adafruit_TCS34725.h>
#include <Corobot.h>
#include "colors99.h"

void setup() {
  Serial.begin(9600);
  while (!Serial) delay(100);
  Serial.println("Corobot ready.");
  Serial.print("*** Please power on the Corobot");
  Serial.println(", or the color reading will be incorrect.");
  Serial.println("Press switch on the basic I/O board to read sensors.");
  Serial.println("Send 'c' from Serial Monitor to calibrate the accelerometer.");
  Corobot.init();
  Corobot.calibrateColor(
    RAW_BLACK,
    RAW_WHITE,
    RAW_RED,
    RAW_GREEN,
    RAW_BLUE,
    RAW_YELLOW,
    RAW_MAGENTA);
  Corobot.calibrateAccel();
}

void showColor() {
  uint16_t rr,rg,rb,rc;
  Corobot.getRawColor(&rr,&rg,&rb,&rc);
  Color color = Corobot.getColor(rr,rg,rb,rc);

  Serial.print("Color:");
  Serial.print(" Raw=(");
  Serial.print(rr); Serial.print(',');
  Serial.print(rg); Serial.print(',');
  Serial.print(rb); Serial.print(',');
  Serial.print(rc); Serial.print(')');
  Serial.print(" Recognized=");
  Serial.println(Corobot.getColorName(color));
}

void showAccel() {
  uint16_t ax,ay,az;
  int8_t pitch,roll;

  Corobot.getRawAccel(&ax,&ay,&az);
  pitch = Corobot.getPitch(ax,ay,az);
  roll = Corobot.getRoll(ax,ay,az);
  Serial.print("Accel:");
  Serial.print(" Raw=(");
  Serial.print(ax); Serial.print(',');
  Serial.print(ay); Serial.print(',');
  Serial.print(az); Serial.print(')');
  Serial.print(" Pitch="); Serial.print(pitch);
  Serial.print(" Roll="); Serial.print(roll);
  Serial.println();
}

void showEyes() {
  Serial.print("Eyes: L=");
  Serial.print(Corobot.getLeftEye());
  Serial.print(",C=");
  Serial.print(Corobot.getCenterEye());
  Serial.print(",R=");
  Serial.print(Corobot.getRightEye());
  Serial.println();
}

void loop() {
  if (Serial.available()) {
    if (Serial.read() == 'c') {
      Corobot.calibrateAccel();
      Serial.println("Accelerometer calibrated.");
    }
  }
  if (Corobot.switchPressed()) {
    Serial.println("--------------------------------------------------------");
    showColor();
    showAccel();
    showEyes();
    Corobot.waitSwitchRelease();
  }
}
