#include <Bounce.h>
#include "pitches.h"



/***********************/
/***  FASTWRITE *****/
/***  Supposedly faster than digitalwrite.  Important for the POV of the multiplexed LED array  ****/
/***********************/
// the following macro sets a digital pin high or low, pin must be between 0 and 13 inclusive
// usage: fastWrite(2,HIGH); fastWrite(13,LOW);
#define fastWrite(_pin_, _state_) ( _pin_ < 8 ? (_state_ ?  PORTD |= 1 << _pin_ : PORTD &= ~(1 << _pin_ )) : (_state_ ?  PORTB |= 1 << (_pin_ -8) : PORTB &= ~(1 << (_pin_ -8)  )))
// the macro sets or clears the appropriate bit in port D if the pin is less than 8 or port B if between 8 and 13

// this macro does fast digital write on pins shared with  the analog port
#define fastWriteA(_pin_,_state_) (_state_ ?  PORTC |= 1 << (_pin_ ) : PORTC &= ~(1 << (_pin_ ) ))
/***********************/
/***  END FASTWRITE *****/
/***********************/


/***********************/
/***  DEBOUNCE *****/
/***********************/
#define NOTPRESSED 0
#define PRESSED 1
#define DEBOUNCE 2
#define RELEASED 3
// Debounce delay in approx millisecs
#define BOUNCE 200
// Main loop delay in millisecs (used by delay)
#define DELAY 10

struct button {
  byte pin;
  byte debounce;
  byte released;
};
/***********************/
/***  END DEBOUNCE *****/
/***********************/


struct animation {
 int frameCount;
 boolean * frames; 
};


boolean everyOtherFrames[] = {
  0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,
  1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0
};
struct animation everyOtherAnimation = { 2, everyOtherFrames};

boolean divergingFrames[] = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,1,1,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,
0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,1,1,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,
0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};

struct animation divergingAnimation = { 6, divergingFrames};

//struct animation tryMeAnimations[] = { everyOtherAnimation, divergingAnimation };

boolean shrinkAndGrowFrames[] = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
struct animation shrinkAndGrowAnimation = { 7, shrinkAndGrowFrames};
struct animation tryMeAnimations[] = { everyOtherAnimation, divergingAnimation, shrinkAndGrowAnimation };


const int tryMeAnimationRate = 200;
//const int numTryMeAnimationFrames = 4;
int currentTryMeAnimationIndex = 0;
struct animation currentTryMeAnimation;
int numTryMeAnimations = 3;

/*******************************/
/*********** MELODIES *******/
/*******************************/
  /*******  Happy  ********/
  int happyMelody[] = { 
    NOTE_C4,  NOTE_G3,  NOTE_C4,  NOTE_E4, NOTE_G4,  NOTE_C5,  NOTE_G4,  NOTE_GS3, NOTE_C4,  NOTE_DS4, NOTE_GS4, NOTE_E4, 
    NOTE_A5,  NOTE_C5,  NOTE_DS5, NOTE_FS5, NOTE_DS5, NOTE_AS4, NOTE_D4,  NOTE_F4, NOTE_AS5, NOTE_F4,  NOTE_B5,  NOTE_D5, NOTE_F5,  NOTE_AS6, NOTE_F5
  
  };
  // note durations: 4 = quarter note, 8 = eighth note, etc.:
  int happyNoteDurations[] = {
     16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 6
  };
  int numHappyMelodyNotes = 27;
  int happyWholeNoteDuration = 600;

  /*******  Hungry  ********/
  int hungryMelody[] = {
    NOTE_A3, NOTE_E2, NOTE_A2, 0, NOTE_A3, NOTE_E2, NOTE_A2, 0, NOTE_A3, NOTE_E2, NOTE_A2, 0
  };
  int hungryNoteDurations[] = {
     8, 8, 8, 4, 8, 8, 8, 4, 8, 8, 8, 4
  };
  int hungryWholeNoteDuration = 200;
  int numHungryMelodyNotes = 12;
/*******************************/
/*********** END MELODIES *******/
/*******************************/


/*******************************/
/****** ASYNC MUSIC ************/
/*******************************/
  int staticNoteSpacer = 20; // ms 
  int dynamicNoteSpacer = 0; // a multiplier -- e.g. 1.2
  int *currentMelody;
  int *currentMelodyDurations;
  int currentNumNotes;
  int currentWholeNoteDuration;
  
  boolean currentMelodyPlaying = false;
  boolean currentNoteIndex = -1;
  int currentNoteStartedTime = -1;
  int currentNoteDuration = 0;
/*******************************/
/****** ASYNC MUSIC ************/
/*******************************/


// States that the bot could be in
#define STATE_IDLE 0
#define STATE_HAPPY 1
#define STATE_HUNGRY 2
#define STATE_TRYME 3


int state = STATE_IDLE;  // Which state we're currently in
unsigned long stateStartTime; // How long we've bee in it

const int hungryBlinkDuration = 800; // milliseconds
const int happyBlinkDuration = 250; // milliseconds
const int happyAnimationRate = 50; // milliseconds
const int happyAnimationRepeats = 3;

const int ledArray1BlinkDuration = 2000; // milliseconds
const int ledArray2BlinkDuration = 2600; // Guess what?  Milliseconds.
int currentLedArray1Level = 0; // Superstition
int currentLedArray2Level = 0; // Superstition
int helmetMotorIdleSpeed = 255; // 200 seems to be the floor
int helmetMotorHappySpeed = 255;

int minLedCount;

long maxEnergy = 40000; // milliseconds worth
int happyTime = 5000; // milliseonds of time to display happy mode


long  FRAME_INTERVAL = 180; // Seconds
long framePowerTime = -180001; // Milliseconds



// if (bLilyPad == true) {

/*
  boolean bLilyPad = true;
  int CANDY_BUTTON = A5; 
  //int ledCount = 5;    // the number of LEDs in the bar graph
  //int ledPins[] = {   5,6,A2,A4,A3 };   // an array of pin numbers to which LEDs are attached
  int SPEAKER = 7;      // analog
  int HELMET_MOTOR = 8; // analog
  int LED_ARRAY1 = 3; // analog
  int LED_ARRAY2 = 2;  // analog
  

  //Pin connected to ST_CP of 74HC595
  int latchPin = 8;
  //Pin connected to SH_CP of 74HC595
  int clockPin = 12;
  ////Pin connected to DS of 74HC595
  int dataPin = 11;
 
 
*/
// } else {
/**/
  boolean bLilyPad = false;
  int CANDY_BUTTON = 2;
  int SOUND_BUTTON = 3; 
  int SPEAKER = 5;  // analog
  int ledCount = 64;
  //int ledPins[] = {   13,12,8,7,4,3,1,0 };   // an array of pin numbers to which LEDs are attached
  int HELMET_MOTOR = 10; // analog
  int LED_ARRAY1 =  7; // analog
  int LED_ARRAY2 =  6; // analog
  int FRAME_POWER = 4;
  
  //Pin connected to ST_CP of 74HC595
  int latchPin = 8;
  //Pin connected to SH_CP of 74HC595
  int clockPin = 12;
  ////Pin connected to DS of 74HC595
  int dataPin = 11;
  
//}

// Instantiate a Bounce object with a 5 millisecond debounce time
//Bounce candyBouncer = Bounce( CANDY_BUTTON, 5 );
//Bounce soundBouncer = Bounce( SOUND_BUTTON, 5 );
struct button candyButton = { CANDY_BUTTON, 0, 0 };
struct button soundButton = { SOUND_BUTTON, 0, 0 };

/*
****************
******* SETUP
****************
*/
void setup() {
  
  // protoboard
 //helmetMotorHappySpeed = 10;
 
  pinMode(CANDY_BUTTON, INPUT_PULLUP);
  pinMode(SOUND_BUTTON, INPUT_PULLUP);
  pinMode(HELMET_MOTOR, OUTPUT);
  pinMode(LED_ARRAY1, OUTPUT);
  pinMode(LED_ARRAY2, OUTPUT);
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);
  pinMode(FRAME_POWER, OUTPUT);
  Serial.begin(9600);
  minLedCount = ledCount / 5;
  setState(STATE_IDLE);
  
  //digitalWrite(LED_ARRAY1, HIGH);
  //digitalWrite(LED_ARRAY2, HIGH);
  
}



/*
****************
******* LOOP
****************
*/
int candyButtonState = 0;  // Just for debugging
void loop() {
  
  long energyLevel;
  
  if ((millis() - framePowerTime) / 1000 > FRAME_INTERVAL) {
    digitalWrite(FRAME_POWER, HIGH);
    delay(3000);
    digitalWrite(FRAME_POWER, LOW);
    framePowerTime = millis();
  }
  //candyBouncer.update ( );
  //soundBouncer.update ( );
  //int candyButtonState = 1; //candyBouncer.read();
  //int soundButtonState = 1; //soundBouncer.read();
  //Serial.print("Candy state: ");
  //Serial.println(candyButtonState);
  //if (candyButtonState == LOW) {
    analogWrite(HELMET_MOTOR, helmetMotorIdleSpeed);
  if ( buttonHandler( &candyButton ) == PRESSED ) {
    //Serial.println("Candy!");
    //playMelody(numHappyMelodyNotes, happyMelody, happyNoteDurations, happyWholeNoteDuration);
      initMelody(happyMelody, happyNoteDurations, numHappyMelodyNotes, happyWholeNoteDuration);
    startMelody();
    setState(STATE_HAPPY);
    analogWrite(HELMET_MOTOR, helmetMotorHappySpeed);
  } 
  //candyButtonState = HIGH;
  
  if ( buttonHandler( &soundButton ) == PRESSED ) {
  //if (soundButtonState == LOW) {
    //Serial.println("Sound!");
    int randomNoteIndex = millis() % numHappyMelodyNotes;
    tone(SPEAKER, happyMelody[randomNoteIndex],500);
    // noTone(SPEAKER);
    currentTryMeAnimationIndex = millis() % numTryMeAnimations;
    setState(STATE_TRYME);
  }
  
  int ledArray1Level = ((getTimeInState() / ledArray1BlinkDuration) % 2) * 255; // 255 or 0
  int ledArray2Level = ((getTimeInState() / ledArray2BlinkDuration) % 2) * 255; // 255 or 0
  if (currentLedArray1Level != ledArray1Level) {
    digitalWrite(LED_ARRAY1, ledArray1Level);
    currentLedArray1Level = ledArray1Level;
  }
  if (currentLedArray2Level != ledArray2Level) {
    digitalWrite(LED_ARRAY2, ledArray2Level);
    currentLedArray2Level = ledArray2Level;
  }
  //digitalWrite(LED_ARRAY1, HIGH);
  //digitalWrite(LED_ARRAY2, HIGH);
  //Serial.print("Sending to LED ARRAY 1: ");
  //Serial.println(ledArray1Level);
  
  int ledLevel = 0;
  int numAnimationSteps;
  int animationTime;
  int blinkMultiplier;
  long state_time;
  int animationIndex;
  boolean * tmpFrames;
  
  switch (state) {
    
    // Waiting for candy, getting hungry
    case STATE_IDLE:
      // Deplete energy over time
      state_time = getTimeInState();
      energyLevel = (maxEnergy) - getTimeInState();
      // If meter is low, switch to Hungry mode
      if (energyLevel < 0) {
        energyLevel = 0;
        //playMelody(numHungryMelodyNotes, hungryMelody, hungryNoteDurations, hungryWholeNoteDuration);
        //initMelody(hungryMelody, hungryNoteDurations, numHungryMelodyNotes, hungryWholeNoteDuration); 
        initMelody(hungryMelody, hungryNoteDurations, numHungryMelodyNotes, hungryWholeNoteDuration);
        startMelody();

        setState(STATE_HUNGRY);
      }
      ledLevel = map(energyLevel, 0, (maxEnergy), minLedCount, ledCount);   
      analogWrite(HELMET_MOTOR, helmetMotorIdleSpeed);
    break;
    
    // Just got food
    case STATE_HAPPY:
    
      // Animate filling up the graph, then start blinking full
      numAnimationSteps = ledCount - minLedCount;
      animationTime = numAnimationSteps * happyAnimationRate;// * happyAnimationRepeats;
      if (getTimeInState() < animationTime) {
        // Animate filling the graph
        //ledLevel = minLedCount + ((getTimeInState() % happyAnimationRepeats) / happyAnimationRate);
        ledLevel = minLedCount + (getTimeInState() / happyAnimationRate);
        //ledArray1Level = LOW;
        //ledArray2Level = LOW;
      } else if (getTimeInState() < happyTime) {
        // Then blink full
        blinkMultiplier = ((getTimeInState() / happyBlinkDuration) % 2);
        ledLevel = blinkMultiplier * ledCount; // 0 or ledCount
        //ledArray1Level = blinkMultiplier * ledArray1Level;
        //ledArray2Level = blinkMultiplier * ledArray2Level;
      } else {
        setState(STATE_IDLE);
      }
      
    break;
    
    // Feed me, Seymour
    case STATE_HUNGRY:
      // Blink on low level
      blinkMultiplier = (getTimeInState() / hungryBlinkDuration) % 2;
      ledLevel = blinkMultiplier * minLedCount; // 0 or minLedCount
    break;
    
    // Turn off sound, and just display some animations
    case STATE_TRYME:
      currentTryMeAnimation = tryMeAnimations[currentTryMeAnimationIndex];
      tmpFrames = currentTryMeAnimation.frames;
      //animationIndex = ((getTimeInState() / tryMeAnimationRate) % numTryMeAnimationFrames) * ledCount;
      animationIndex = ((getTimeInState() / tryMeAnimationRate) % currentTryMeAnimation.frameCount) * ledCount;
      showTryMeAnimation(tmpFrames, animationIndex, animationIndex + ledCount);
    break;
    
    default:
    break;
  }  
  if (state != STATE_TRYME) {
    setLedBarLevel(ledLevel);
  }
  updateMelody();
  
}



// These are the values to send to the shift register to turn on each pin.  So for the third pin, send a value of 4 (00000100 in binary)
// Anodes
int matrixrow[8] = {1,2,4,8,16,32,64,128};

// Cathodes
// Remember that these turn on LEDs by grounding the pin, so "0" is "on".  So for the third pin, send a value of 251 (11111011 in binary)
int matrixcolumn[8] = { 254,253,251,247,239,223,191,127}; 

// To turn on one LED
void pixeldisplay(int row, int column, int holdtime)
// turns on and off a pixel at row, column - with delay 'holdtime'
{
  fastWrite(latchPin, LOW);
  shiftOutRaw(matrixcolumn[column]); // sets the digit to address
  shiftOutRaw(matrixrow[row]); // clears the digit
  fastWrite(latchPin, HIGH);
  
  //delay(holdtime);
}

void showTryMeAnimation (boolean * frames, int animationStartIndex, int animationStopIndex) {
  int pix = 0;
  //Serial.print("Animating from ");
  //Serial.print(animationStartIndex);
  //Serial.print(" to ");
  //Serial.println(animationStopIndex);
  for (int i=animationStartIndex;i<animationStopIndex;i++) {
    //Serial.println("Alive1");
    
    //Serial.println(frames[i]);
    //Serial.println("Alive2");
    //if (currentTryMeAnimationframes[i] == 1) {
      if (frames[i]) {
       // Serial.println("Alive3");
       pix = i % ledCount;
      int col = (pix % 8);
      int row = (pix / 8);
      //Serial.print("Turning on ");
      //Serial.print(row);
      //Serial.print(" , ");
      //Serial.println(col);
      //Serial.println("For real ");
      pixeldisplay(row,col,0);
      
    }
    
    //Serial.println("Alive4");
  }
  //if (ledLevel == 0) {
    fastWrite(latchPin, LOW);
    shiftOutRaw(1); // sets the digit to address
    shiftOutRaw(0); // clears the digit
    fastWrite(latchPin, HIGH);
}
// Turn on all LEDS between 0 and ledLevel
// Relies on POV to make it look lit
void setLedBarLevel(int ledLevel) {
  //Serial.print("Setting bar to ");
  //Serial.println(ledLevel);
  /*
  int fullBlocks = (ledLevel / 8);
  int partialBlock = ledLevel % 8;
  
  
  digitalWrite(latchPin, LOW);
  shiftOut(dataPin, clockPin, MSBFIRST, 0); // we want all the cathodes on, which is 11111111 in binary, 255 decimal
  shiftOut(dataPin, clockPin, MSBFIRST, 255 >> (8 - fullBlocks)); // clears the digit
  digitalWrite(latchPin, HIGH);
  
  for (int i = fullBlocks * 8; i <= ledLevel ; i++) {
    int col = (i % 8) + 1;
    int row = (i / 8) + 1;
    pixeldisplay(row,col,0);
  }  
  */
  //Serial.print("Setting led level to ");
  //Serial.println(ledLevel);
  for (int i=0;i<ledLevel;i++) {
    int col = (i % 8);
    int row = (i / 8);
    //Serial.println("For real ");
    pixeldisplay(row,col,0);
  }
  //if (ledLevel == 0) {
    fastWrite(latchPin, LOW);
    shiftOutRaw(1); // sets the digit to address
    shiftOutRaw(0); // clears the digit
    fastWrite(latchPin, HIGH);
  //}
  /*
  
    //ground latchPin and hold low for as long as you are transmitting
    digitalWrite(latchPin, 0);
    //count up on GREEN LEDs
    shiftOut(dataPin, clockPin, j); 
    //count down on RED LEDs
    shiftOut(dataPin, clockPin, 255-j);
    //return the latch pin high to signal chip that it 
    //no longer needs to listen for information
    digitalWrite(latchPin, 1);
    */
}
/*
void setLedBarLevel(int ledLevel) {
  // loop over the LED array:
  for (int thisLed = 0; thisLed < ledCount; thisLed++) {
    // if the array element's index is less than ledLevel,
    // turn the pin for this element on:
    if (thisLed < ledLevel) {
      //Serial.print("Setting pin ");
     // Serial.print(ledPins[thisLed]);
      //Serial.println(" on");
      digitalWrite(ledPins[thisLed], HIGH);
    } 
    // turn off all pins higher than the ledLevel:
    else {
      //Serial.print("Setting pin ");
      //Serial.print(ledPins[thisLed]);
      //Serial.println(" off");
      digitalWrite(ledPins[thisLed], LOW); 
    }
  }
}
*/
void setState(int newState) {
  state = newState;
  //Serial.print("Switching state to ");
  //Serial.println(newState);
  stateStartTime = millis();
}

long getTimeInState() {
  return millis() - stateStartTime;
}


/*********************************/
/************ MELODY **********/
/*********************************/
  // TODO:  Figure our how to do this via some kind of multitasking
  /*
  void playMelody(int numNotes, int melody[], int noteDurations[], int wholeNoteDuration) {
    for (int thisNote = 0; thisNote < numNotes; thisNote++) {
  
      // to calculate the note duration, take one second 
      // divided by the note type.
      //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
      int noteDuration = wholeNoteDuration/noteDurations[thisNote];
      tone(SPEAKER, melody[thisNote],noteDuration);
  
      // to distinguish the notes, set a minimum time between them.
      // the note's duration + 30% seems to work well:
      int pauseBetweenNotes = noteDuration; // * 1.30;
      delay(pauseBetweenNotes);
      
      // stop the tone playing:
      noTone(SPEAKER);
    }
  
  }
  */
  
  
  // TODO : Pause between notes
  // TODO : Do I need a noTone call? 
  void updateMelody() {
    if (currentMelodyPlaying) {
      if (millis() - currentNoteStartedTime >= currentNoteDuration) {
        currentNoteIndex++;
        if (currentNoteIndex < currentNumNotes) {
          currentNoteDuration = currentMelodyDurations[currentNoteIndex];
          int noteDuration = currentWholeNoteDuration/currentNoteDuration;
          //Serial.print("Playing note ");
          //Serial.print(currentNoteIndex);
          //Serial.print(" of ");
          //Serial.println(currentNumNotes);
          tone(SPEAKER, currentMelody[currentNoteIndex], currentNoteDuration);
          currentNoteStartedTime = millis();
          // Allow for some downtime between notes
          currentNoteDuration = currentNoteDuration + staticNoteSpacer + (currentNoteDuration * dynamicNoteSpacer);
        } else {
          currentMelodyPlaying = false;
          noTone(SPEAKER);
        }
      }
    } 
  }
  
  void startMelody() {
    //Serial.println("Starting Tune");
     currentMelodyPlaying = true;
     currentNoteIndex = -1;
  }
  
  
  void initMelody(int notes[], int durations[], int numNotes, int wholeNoteDuration) {
    //Serial.println("Init Tune");
     currentMelody =  notes;
     currentMelodyDurations = durations;
     currentNumNotes = numNotes;
     currentWholeNoteDuration = wholeNoteDuration;
  }
/*********************************/
/************ END MELODY **********/
/*********************************/


/****************************/
/************  FAST SHIFT  */
/***  Supposedly faster than shiftOut.  Important for the POV of the multiplexed LED array  ****/
/****************************/
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))

#define sbipin(_pin_) sbi(_pin_<8 ? PORTD:PORTB, _pin_ - (_pin_<8 ? 0:8))
#define cbipin(_pin_) cbi(_pin_<8 ? PORTD:PORTB, _pin_ - (_pin_<8 ? 0:8))

#define DATA_PIN   11
#define CLOCK_PIN  12
#define BIT_ORDER  MSBFIRST

  void shiftOutRaw(byte val)
  {
    byte  mask = BIT_ORDER == LSBFIRST ? 1 : 128;
    if (val & mask) sbipin(DATA_PIN);
    else            cbipin(DATA_PIN);
    sbipin(CLOCK_PIN); cbipin(CLOCK_PIN);
    mask = BIT_ORDER == LSBFIRST ? mask << 1 : mask >> 1;
    if (val & mask) sbipin(DATA_PIN);
    else            cbipin(DATA_PIN);
    sbipin(CLOCK_PIN); cbipin(CLOCK_PIN);
    mask = BIT_ORDER == LSBFIRST ? mask << 1 : mask >> 1;
    if (val & mask) sbipin(DATA_PIN);
    else            cbipin(DATA_PIN);
    sbipin(CLOCK_PIN); cbipin(CLOCK_PIN);
    mask = BIT_ORDER == LSBFIRST ? mask << 1 : mask >> 1;
    if (val & mask) sbipin(DATA_PIN);
    else            cbipin(DATA_PIN);
    sbipin(CLOCK_PIN); cbipin(CLOCK_PIN);
    mask = BIT_ORDER == LSBFIRST ? mask << 1 : mask >> 1;
    if (val & mask) sbipin(DATA_PIN);
    else            cbipin(DATA_PIN);
    sbipin(CLOCK_PIN); cbipin(CLOCK_PIN);
    mask = BIT_ORDER == LSBFIRST ? mask << 1 : mask >> 1;
    if (val & mask) sbipin(DATA_PIN);
    else            cbipin(DATA_PIN);
    sbipin(CLOCK_PIN); cbipin(CLOCK_PIN);
    mask = BIT_ORDER == LSBFIRST ? mask << 1 : mask >> 1;
    if (val & mask) sbipin(DATA_PIN);
    else            cbipin(DATA_PIN);
    sbipin(CLOCK_PIN); cbipin(CLOCK_PIN);
    mask = BIT_ORDER == LSBFIRST ? mask << 1 : mask >> 1;
    if (val & mask) sbipin(DATA_PIN);
    else            cbipin(DATA_PIN);
    sbipin(CLOCK_PIN); cbipin(CLOCK_PIN);
  }
/****************************/
/********  END FAST SHIFT  *****/
/****************************/



/********************************
*****   DEBOUNCER  ********
*****   This one does not use delays, which is important for the POV in the multiplexted LEDs
**********************************/
  // Two example buttons
  //struct button modebutton = { modepin, 0, 0 };
  //struct button selectbutton = { selectpin, 0, 0 };
   
  // Button Handler
  int buttonHandler(struct button * b) {
    if ( b->debounce ) {        // Debounce delay
      b->debounce--;
      return( DEBOUNCE );
    } else {
      if ( digitalRead(b->pin) == HIGH ) {
        if ( ! b->released ) {  // Button has been released
          b->debounce = BOUNCE / DELAY;
          b->released = 1;
          return( PRESSED );
        }
      } else {                 // Button released
        b->released = 0;
        return( RELEASED );
      }
    }
    return( NOTPRESSED );
  }
   
  // example usage
  //if ( buttonHandler( &modebutton ) == PRESSED ) {
  //  mode = (mode + 1) & 3;
/********************************
*****   END DEBOUNCER  ********
**********************************/