/* Metronome Program
 * for MMA7260Q accelerometer
 * by Rob Faludi
 * www.faludi.com
 * version 1.0.3
 */

int ledPin = 13;    // declare pin 13 for the LED
//int Xpin = 0;    // slider variable connecetd to analog pin 0
//int Ypin = 1;    // slider variable connecetd to analog pin 1
int Zpin = 2;
//int Xval = 0;     // variable to read the value from the analog pin 0
//int Yval = 0;     // variable to read the value from the analog pin 1
int Zval = 0;
int threshold = 25;
long thisTick = 0;
long lastTick = 0;
boolean positivePhase = true;
long tickAverage = 1300;
int tickArray[10];
int tickArraySize = 10;
long tickCounter = 0;
long sendTime = 0;
int beatCount = 4;
int beatsPerTick = 2;
int beat = 0;
long timeCode = 0;

void setup() 
{
  pinMode(ledPin, OUTPUT);    // initializes digital pins 0 to 7 as outputs
  Serial.begin(9600);          // turn on the serial port
  for(int i = 0; i < tickArraySize; i++) {
    tickArray[i] = 0; 
  }
  blinkLED(ledPin,2,100);
  setupXBee();
}


void loop()
{
  Zval = analogRead(Zpin); // read the accelerometer Z axis
  long now = millis(); // mark the current time
  if (Zval < 500-threshold & positivePhase == true) {
    //mark entry into negative phase of cycle;
    positivePhase = false;
  }
  // if phase has gone positive from negative
  if (Zval > 500 + threshold && positivePhase == false) { 
    thisTick = now - lastTick; // calculate the tick length
    lastTick = now; // set the last tick value to use next time
    positivePhase = true;
    // and if this tick is of some reasonable length (not too short or long)
    if (thisTick > tickAverage * .6 && thisTick < tickAverage * 1.6) { 
      blinkLED(ledPin,1,50);
      for (int i =tickArraySize-1; i > 0 ; i--) {
        tickArray[i] = tickArray[i-1];  // move each array member to the left
      }
      tickArray[0] = thisTick; // insert the latest tick into the array
      tickCounter++;

      tickAverage=0;
      for (int i=0; i < constrain(tickCounter,0,tickArraySize); i++) {
        tickAverage = tickAverage + tickArray[i];
      }
      tickAverage = tickAverage / constrain(tickCounter,0,tickArraySize);
      //Serial.print("avg: ");
      //Serial.println(tickAverage);
    }
  }

  if (millis() - sendTime > tickAverage / beatsPerTick) {
    Serial.print("measure: ");
    Serial.print(timeCode);
    Serial.print(" beat: ");
    Serial.print(beat +1);
    Serial.print(" avg:");
    Serial.println(tickAverage);
    sendTime = millis();
    beat = (beat+1) % beatCount;
    if (beat == beatCount-1) {
      timeCode++;
    }
  }
}



void setupXBee() {
  boolean success = false;
  int ctr = 0;
  while (success == false && ctr < 100) {
    // blink the status LED
    blinkLED(ledPin, 2, 250);
    // an arbitrary byte to wake up the XBee
    //Serial.print("X");
    //delay(1100);
    // put the XBee in command mode
    Serial.print("+++");
    delay(1100);
    // wait for a response from the XBee for 2000 ms, or start
    // over with setup if no valid response comes
    // set the PAN (personal area network) ID number
    // set the MY (16 bit address)
    // set the Destination High to 0x0
    // set the Destination Low (16 bit address)
    // exit command mode (using a Serial.printLN to end the command with a linefeed)
    Serial.println("ATID3333,MY64,DH0,DL65,CN");
    if (checkFor("OK", 1000)) {
      // if an OK was received then continue 
      success = true;
    }
    else {
      success = false;
    }
    ctr++;
  }
}


///////////////////////////////// UTILITY FUNCTIONS //////////////////////////////////////////
// this function checks for a specific response on the serial port
// it accepts a string to look for and a timeout in milliseconds
int checkFor(char* desiredResponse, int timeout) {
  int result = 0;
  int length = 40;
  char incomingResponse[41];
  memset(incomingResponse,0,length); // initialize all incomingResponse string positions to null
  char inByte = NULL;
  long startTime = millis();//makes the start time = to now
  char* ptr_incomingResponse = incomingResponse;
  // while we haven't timed out or gotten back the string that we are looking for
  while (millis() - startTime < timeout && strstr(incomingResponse,desiredResponse) == NULL ) { //strstr compares strings
    if (Serial.available() > 0) { // if there are any bytes waiting to be read
      inByte = Serial.read();  // read one byte
      if (incomingResponse > ptr_incomingResponse-length) { // if we haven't read in 80 characters yet
        *ptr_incomingResponse = inByte;  // put the byte into the current position in the string
        ptr_incomingResponse++;  // advance to the next position in the string
      }
      else {
        //move the last char to be next to last, and so forth until we reach the end of the array.
        for (int i = 0; i < length; i++) {
          incomingResponse[i] = incomingResponse[i+1];  
        }
        incomingResponse[length-1] = inByte;  // put the byte into the current position in the string
      }
    }
  }
  if (strstr(incomingResponse,desiredResponse) != NULL ) { // if the desired string is found
    result = 1;
  }
  else { 
    result = 0; 
  }
  return result;
}



// this function blinks the an LED light as many times as requested
void blinkLED(int targetPin, int numBlinks, int blinkRate) {
  for (int i=0; i<numBlinks; i++) {
    digitalWrite(targetPin, HIGH);   // sets the LED on
    delay(blinkRate);                     // waits for a blinkRate milliseconds
    digitalWrite(targetPin, LOW);    // sets the LED off
    delay(blinkRate);
  }
}