#include <LiquidCrystal.h>

/*
 * Some opcode definitions for the iRobot Create.
 * Copy and paste into the top of your program file.
 */

#define IR_START                        (128)
#define IR_SAFE                         (131)
#define IR_FULL                         (132)
#define IR_SPOT                         (134)
#define IR_COVER                        (135)
#define IR_DEMO                         (136)
#define IR_DRIVE                        (137)
#define IR_DIRECT_DRIVE                 (145)
#define SET_SONG                        (140)
#define PLAY_SONG                       (141)
#define SENSOR                          (142)


#define PACKET_WALL                     (8)

#define TURN_LEFT                       (0x0001)
#define TURN_RIGHT                      (0xFFFF)
#define DRIVE_STRAIGHT                  (0x7FFF)
#define MEDIUM_SPEED                    (200)
#define FAST_SPEED                      (500)

#define quaver                          (16)


long lastMillis;

// init LCD display
LiquidCrystal lcd(6,7,8,2,3,4,5);

void setup() {

  // Initialise connection to robot
  lcd.begin(16,2);
  lcd.print("Starting...");
  delay(3000);
  lcd.clear();
  Serial.begin(57600);

  // set mode, start sets passive mode by default
  Serial.write(IR_START);

  Serial.write(IR_FULL);
  delay(100);
  Serial.write(SET_SONG);
  Serial.write((byte)0); // Song number
  Serial.write(3); // Song lenghth (in notes)

  Serial.write(62);
  Serial.write(3*quaver);
  Serial.write(69);
  Serial.write(3*quaver);
  Serial.write(74);
  Serial.write(2*quaver);

  Serial.write(SET_SONG);
  Serial.write(1); // Song number
  Serial.write(3); // Song lenghth (in notes)

  Serial.write(69);
  Serial.write(3*quaver);
  Serial.write(74);
  Serial.write(3*quaver);
  Serial.write(77);
  Serial.write(2*quaver);

  Serial.write(SET_SONG);
  Serial.write(2); // Song number
  Serial.write(3); // Song lenghth (in notes)

  Serial.write(65);
  Serial.write(3*quaver);
  Serial.write(72);
  Serial.write(3*quaver);
  Serial.write(76);
  Serial.write(2*quaver);

  Serial.write(SET_SONG);
  Serial.write(3); // Song number
  Serial.write(3); // Song lenghth (in notes)

  Serial.write(67);
  Serial.write(3*quaver);
  Serial.write(72);
  Serial.write(3*quaver);
  Serial.write(74);
  Serial.write(2*quaver);
  //( insert your code here )
  delay(100);

}

void loop() {
  static int x = 0;
  int songPlaying;
  Serial.write(PLAY_SONG);
  Serial.write(x); // Song number
  do
  {
    delay(50);
    Serial.write(SENSOR);
    Serial.write(37);
    delay(20);
    songPlaying = Serial.read();
    lcd.clear();
    lcd.print(songPlaying);
  } while(songPlaying);
  x++;
  x %= 4;
}