#include <SoftwareSerial.h>
#include <Wire.h>
#include "Adafruit_MPR121.h"
#ifndef _BV
#define _BV(bit) (1 << (bit))
#endif

// You can have up to 4 on one i2c bus but one is enough for testing!
Adafruit_MPR121 cap = Adafruit_MPR121();

// Keeps track of the last pins touched
// so we know when buttons are 'released'
uint16_t lasttouched = 0;
uint16_t currtouched = 0;
int baseNote = 48;            // C4, used as the base note

SoftwareSerial midiSerial(2, 3); // digital pins that we'll use for soft serial RX & TX

void setup() {
  Serial.begin(9600);
  midiSerial.begin(31250);

  while (!Serial) { // needed to keep leonardo/micro from starting too fast!
    delay(10);
  }

  Serial.println("Adafruit MPR121 Capacitive Touch sensor test");

  // Default address is 0x5A, if tied to 3.3V its 0x5B
  // If tied to SDA its 0x5C and if SCL then 0x5D
  if (!cap.begin(0x5A)) {
    Serial.println("MPR121 not found, check wiring?");
    while (1);
  }
  Serial.println("MPR121 found!");
}

void loop() {
  // Get the currently touched pads
  currtouched = cap.touched();
  //Serial.println(currtouched);
  for (uint8_t i = 0; i < 12; i++) {
    // it if *is* touched and *wasnt* touched before, alert!
    int thisCommand = 0;
    int thisNote = i + baseNote;                // calculate note

    if ((currtouched & _BV(i)) && !(lasttouched & _BV(i)) ) {
      //
      Serial.print(i); Serial.println(" touched");
      thisCommand = 0x90;
      midiCommand(thisCommand, thisNote, 127);  // play or stop the note
      // Serial.println(thisNote, HEX);            // print note

    }

    // if it *was* touched and now *isnt*, alert!
    if (!(currtouched & _BV(i)) && (lasttouched & _BV(i)) ) {
      Serial.print(i); Serial.println(" released");
      thisCommand = 0x80;
      midiCommand(thisCommand, thisNote, 127);  // play or stop the note
      // Serial.println(thisNote, HEX);            // print note
    }
  }

  // reset our state
  lasttouched = currtouched;
  delay(100);
}

void midiCommand(byte cmd, byte data1, byte  data2) {
  Serial.print("sending midi note: ");
  Serial.print(cmd);
  Serial.print(" ");
  Serial.print(data1); //note
  Serial.print(" ");
  Serial.println(data2); //speed

  midiSerial.write(cmd);     // command byte (should be > 127)
  midiSerial.write(data1);   // data byte 1 (should be < 128)
  midiSerial.write(data2);   // data byte 2 (should be < 128)
  delay(50);
}