/*
 * RoombaMoodLight 
 * ---------------
 * Add a nice RGB color-shifting mood light to RoombaBumpTurn
 * Makes for a mobile mood light that adds changing splashes of color
 * on the ceiling and walls.
 *
 * For Roomba:
 * Arduino pin 0 (RX) is connected to Roomba TXD
 * Arduino pin 1 (TX) is connected to Roomba RXD
 * Arduino pin 2      is conencted to Roomba DD
 * 
 * For RGB LEDs,
 * Arduino pin  9 is connected to Red LED
 * Arduino pin 10 is connected to Green LED
 * Arduino pin 11 is connected to Blue LED
 * 
 * Created 1 August 2006
 * copyleft 2006 Tod E. Kurt <tod@todbot.com>
 * http://hackingroomba.com/
 */

int ddPin = 2;
int ledPin = 13;
char sensorbytes[10];

#define bumpright (sensorbytes[0] & 0x01)
#define bumpleft  (sensorbytes[0] & 0x02)

int redPin   = 9;   // red LED
int greenPin = 10;  // green LED
int bluePin  = 11;  // blue LED

int ledi = 0;       // led loop counter    
int redVal   = 255; // brightness values to send to LEDs
int greenVal = 255; // start with all at full bright(==white)
int blueVal  = 255;

void setup() {
  pinMode(redPin,   OUTPUT);
  pinMode(greenPin, OUTPUT);   
  pinMode(bluePin,  OUTPUT);

  pinMode(ddPin,   OUTPUT);   // sets the pins as output
  pinMode(ledPin,  OUTPUT);   // sets the pins as output
  Serial.begin(57600);
  
  digitalWrite( ledPin, HIGH); // say we're alive
   
  // wake up the robot
  digitalWrite( ddPin, LOW);
  delay(100);
  digitalWrite( ddPin, HIGH);
  delay(2000);
  // set up SCI to receive commands  
  Serial.print(128, BYTE);  // START
  delay(50);
  Serial.print(130, BYTE);  // CONTROL
  delay(50);
  digitalWrite( ledPin, LOW);  // say we've finished setup
}

void loop() {
  updateLEDs();
  digitalWrite( ledPin, HIGH); // say we're starting loop
  updateSensors();
  digitalWrite( ledPin, LOW);  // say we're after updateSensors
  if( bumpleft ) {
    spinRight();
    delay(1000);
  }
  else if( bumpright ) {
    spinLeft();
    delay(1000);
  }
  goForward();
}

void updateLEDs() {
  ledi++;
  if( ledi < 255 ) {
    redVal   -= 1;   // red down
    greenVal -= 1;   // green down
  } else if( ledi < 255*2 ) {
    redVal   += 1;   // red up
    blueVal  -= 1;   // blue down
  } else if( ledi < 255*3 ) {
    greenVal += 1;   // green up
    blueVal  += 1;   // blue up
  } else {
    ledi = 0;  // reset
  }
  analogWrite(redPin,   redVal);  // write brightness vals to LEDs
  analogWrite(greenPin, greenVal); 
  analogWrite(bluePin,  blueVal);
}

void goForward() {
  char c[] = { 137, 0x00, 0xc8, 0x80, 0x00 };    // 0x00c8 == 200
  Serial.print( c );
}
void goBackward() {
  char c[] = { 137, 0xff, 0x38, 0x80, 0x00 };    // 0xff38 == -200
  Serial.print( c );
}
void spinLeft() {
  char c[] = { 137, 0x00, 0xc8, 0x00, 0x01 };
  Serial.print( c );
}
void spinRight() {
  char c[] = { 137, 0x00, 0xc8, 0xff, 0xff };
  Serial.print( c );
}
void updateSensors() {
  Serial.print( 142, BYTE);
  Serial.print( 1,   BYTE);  // sensor packet 1, 10 bytes
  delay(100); // wait for sensors 
  char i = 0;
  while( Serial.available() ) {
    int c = serialRead();
    if( c==-1 ) {
      for( int i=0; i<5; i ++ ) {
        digitalWrite( ledPin, HIGH); delay(50);
        digitalWrite( ledPin, LOW);  delay(50);
      }
    }
    sensorbytes[i++] = c;
  }    
}