/*
 * KU Corobot Example: Remote Control
 * 
 * Command a Corobot remotely from a PC's serial port.
 * The paired corobot must run the Serial Bridge sketch. 
 * 
 * Command Keys
 * ------------
 * 'w' - move forward for 0.1 second
 * 's' - move backward for 0.1 second
 * 'a' - spin counter-clockwise for 0.1 second
 * 'd' - spin clockwise for 0.1 second
 * 'q' - turn left for 0.1 second
 * 'e' - turn right for 0.1 second
 * 'z' - reverse left for 0.1 second
 * 'c' - reverse right for 0.1 second
 * '1' - report line trackers' readings
 * '2' - report color sensor's readings
 * '3' - report accelerometer's readings
 * '4' - recalibrate accelerometer
 * '5' - report switch pressing status
 * 'r' - turn on red LED
 * 'y' - turn on yellow LED
 * 'g' - turn on green LED
 * 'R' - turn off red LED
 * 'Y' - turn off yellow LED
 * 'G' - turn off green LED
 * 
 */
#include <Wire.h>
#include <Adafruit_TCS34725.h>
#include <Corobot.h>

#include "colors99.h"

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);
  Corobot.remote.println("Corobot ready.");
}

void move() {
  delay(100);
  Corobot.setLeftMotor(FORWARD,0);
  Corobot.setRightMotor(FORWARD,0);
}

void loop() {
  if (Corobot.remote.available()) {
    char in = Corobot.remote.read();

    /////////////////////
    // Movement control
    /////////////////////
    if (in == 'w') {
      Corobot.setLeftMotor(FORWARD,50);
      Corobot.setRightMotor(FORWARD,50);
      Corobot.remote.println("forward");
      move();
    }
    if (in == 's') {
      Corobot.setLeftMotor(BACKWARD,50);
      Corobot.setRightMotor(BACKWARD,50);
      Corobot.remote.println("reverse");
      move();
    }
    if (in == 'a') {
      Corobot.setLeftMotor(BACKWARD,50);
      Corobot.setRightMotor(FORWARD,50);
      Corobot.remote.println("spin left");
      move();
    }
    if (in == 'd') {
      Corobot.setLeftMotor(FORWARD,50);
      Corobot.setRightMotor(BACKWARD,50);
      Corobot.remote.println("spin right");
      move();
    }
    if (in == 'q') {
      Corobot.setLeftMotor(FORWARD,0);
      Corobot.setRightMotor(FORWARD,50);
      Corobot.remote.println("turn left");
      move();
    }
    if (in == 'e') {
      Corobot.setLeftMotor(FORWARD,50);
      Corobot.setRightMotor(FORWARD,0);
      Corobot.remote.println("turn right");
      move();
    }
    if (in == 'z') {
      Corobot.setLeftMotor(FORWARD,0);
      Corobot.setRightMotor(BACKWARD,50);
      Corobot.remote.println("reverse left");
      move();
    }
    if (in == 'c') {
      Corobot.setLeftMotor(BACKWARD,50);
      Corobot.setRightMotor(FORWARD,0);
      Corobot.remote.println("reverse right");
      move();
    }

    ///////////////////////////
    // Sensor reading control
    ///////////////////////////
    if (in == '1') { // line trackers
      Corobot.remote.print("Eyes:");
      Corobot.remote.print(" Left=");
      Corobot.remote.print(Corobot.getLeftEye());
      Corobot.remote.print(" Center=");
      Corobot.remote.print(Corobot.getCenterEye());
      Corobot.remote.print(" Right=");
      Corobot.remote.print(Corobot.getRightEye());
      Corobot.remote.println();
    }
    
    if (in == '2') { // color sensor
      uint16_t rr,rg,rb,rc;
      Color color;
      
      Corobot.getRawColor(&rr,&rg,&rb,&rc);
      color = Corobot.getColor(rr,rg,rb,rc);

      Corobot.remote.print("Color:");
      Corobot.remote.print(" Raw=(");
      Corobot.remote.print(rr); Corobot.remote.print(',');
      Corobot.remote.print(rg); Corobot.remote.print(',');
      Corobot.remote.print(rb); Corobot.remote.print(',');
      Corobot.remote.print(rc); Corobot.remote.print(')');
      Corobot.remote.print(" Color=");
      Corobot.remote.println(Corobot.getColorName(color));
    }
    
    if (in == '3') { // accelerometer
      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);
      Corobot.remote.print("Accel:");
      Corobot.remote.print(" Raw=(");
      Corobot.remote.print(ax); Corobot.remote.print(',');
      Corobot.remote.print(ay); Corobot.remote.print(',');
      Corobot.remote.print(az); Corobot.remote.print(')');
      Corobot.remote.print(" Pitch="); Corobot.remote.print(pitch);
      Corobot.remote.print(" Roll="); Corobot.remote.print(roll);
      Corobot.remote.println();
    }

    if (in == '4') { // recalibrate accelerometer
      Corobot.calibrateAccel();
      Corobot.remote.println("Accelerometer calibrated");
    }

    if (in == '5') { // switch status
      Corobot.remote.print("Switch:");
      Corobot.remote.println(Corobot.switchPressed() ? "PRESSED" : "RELEASED");
    }

    // LED control
    if (in == 'r') Corobot.setLed(RED,HIGH);
    if (in == 'y') Corobot.setLed(YELLOW,HIGH);
    if (in == 'g') Corobot.setLed(GREEN,HIGH);
    if (in == 'R') Corobot.setLed(RED,LOW);
    if (in == 'Y') Corobot.setLed(YELLOW,LOW);
    if (in == 'G') Corobot.setLed(GREEN,LOW);
  }
}
