/* Disco Bot

 This sketch shows you how to use the melody playing 
 feature of the robot, with some really cool 8-bit music. 
 Music will play when the robot is turned on, and it 
 will show you some dance moves.

 Circuit:
 * Arduino Robot
 
 created 1 May 2013
 by X. Yang
 modified 12 May 2013
 by D. Cuartielles
 
 This example is in the public domain
 */

#include <ArduinoRobot.h> // include the robot library
#include <Wire.h>
#include <SPI.h>

/* Dancing steps:
  S: stop
  L: turn left
  R: turn right
  F: go forward
  B: go backwards

  The number after each command determines how long 
  each step lasts. Each number is 1/2 second long.

  The "\0" indicates end of string
*/
char danceScript[] = "S4L1R1S2F1B1S1\0"; 

int currentScript = 0; // what step are we at

int currentSong = 0; // keep track of the current song
static const int SONGS_COUNT = 3; // number of songs

// an array to hold the songs
char musics[][11] = {
  "melody.sqm",
  "menu.sqm",
  "chase.sqm",
};

// variables for non-blocking delay
long waitFrom;
long waitTime = 0;

void setup() {
  // initialize the Robot, SD card, display, and speaker 
  Robot.begin();
  Robot.beginSpeaker();
  Robot.beginSD();
  Robot.beginTFT();
 
  // draw "lg0.bmp" and "lg1.bmp" on the screen
  Robot.displayLogos();
  
  // Print instructions to the screen
  Robot.text("1. Use left and\n right key to switch\n song", 5, 5);
  Robot.text("2. Put robot on the\n ground to dance", 5, 33);

  // wait for a few soconds
  delay(3000);
  
  setInterface(); // display the current song
  play(0);  //play the first song in the array
  
  resetWait();  //Initialize non-blocking delay
}

void loop() {
  // read the butttons on the robot
  int key = Robot.keyboardRead();
  
  // Right/left buttons play next/previous song
  switch(key) {
    case BUTTON_UP:
    case BUTTON_LEFT:
      play(-1);  //play previous song
      break;
    case BUTTON_DOWN:
    case BUTTON_RIGHT:
      play(1);  //play next song
      break;
  }
  
  // dance!
  runScript();
}

// Dancing function
void runScript() { 
 if(!waiting()) { // if the previous instructions have finished
   // get the next 2 commands (direction and duration)
    parseCommand(danceScript[currentScript], danceScript[currentScript+1]);
    currentScript += 2;
    if(danceScript[currentScript] == '\0') // at the end of the array
      currentScript = 0; // start again at the beginning
 }
}

// instead of delay, use this timer
boolean waiting() {
  if(millis()-waitFrom >= waitTime)
    return false;
  else
    return true;
}

// how long to wait
void wait(long t) {
  resetWait();
  waitTime = t;
}

// reset the timer
void resetWait() {
  waitFrom = millis();
}

// read the direction and dirstion of the steps
void parseCommand(char dir, char duration) { 
  //convert the scripts to action
  switch(dir) { 
    case 'L':
      Robot.motorsWrite(-255, 255);
      break;
    case 'R':
      Robot.motorsWrite(255, -255);
      break;
    case 'F':
      Robot.motorsWrite(255, 255);
      break;
    case 'B':
      Robot.motorsWrite(-255, -255);
      break;
    case 'S':
      Robot.motorsStop();
      break;
  }
  //You can change "500" to change the pace of dancing
  wait(500*(duration-'0'));
}

// display the song
void setInterface() {
  Robot.clearScreen();
  Robot.stroke(0, 0, 0);
  Robot.text(musics[0], 0, 0);
}

// display the next song
void select(int seq, boolean onOff) {
  if(onOff){//select
    Robot.stroke(0, 0, 0);
    Robot.text(musics[seq], 0, 0);
  }else{//deselect
    Robot.stroke(255, 255, 255);
    Robot.text(musics[seq], 0, 0);
  }
}

// play the slected song
void play(int seq) {
  select(currentSong, false);
  if(currentSong <= 0 && seq == -1) {  //previous of 1st song?
    currentSong = SONGS_COUNT-1;  //go to last song
  } else if(currentSong >= SONGS_COUNT-1 && seq == 1) {  //next of last?
    currentSong = 0;  //go to 1st song
  } else {
    currentSong += seq;  //next song
  }
  Robot.stopPlayFile();
  Robot.playFile(musics[currentSong]);
  select(currentSong, true);  //display the current song
}
