// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>

#include <SoftwareSerial.h>
const int RXBLE = 14;
const int TXBLE = 15;

SoftwareSerial bleSerial(RXBLE, TXBLE);

int messageLimit = 3;
int counter = 0;
byte incomming[3];
int counterBLE = 0;
byte incommingBLE[3];
byte endByte = B11111111;

const int VIBRATION = 9;

const int MPU_addr = 0x68; // I2C address of the MPU-6050
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
int16_t xBuf[3], xNorm, yBuf[3], yNorm, zBuf[3], zNorm;
float alpha = 0.4;
int TAP_SENSITIVITY_LEVEL = 8;
int TAP_MIN_THRESHOLD = 800;
int MILLIS_BETWEEN_TAPS = 40;
long previousRead = 0;
struct
{
  byte x: 1;
  int xIntensity;
  byte y: 1;
  int yIntensity;
  byte z: 1;
  int zIntensity;
  long last;
  long current;
} peak;



void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)

  Wire.endTransmission(true);

  xBuf[0] = 10000;
  xBuf[1] = 10000;
  xBuf[2] = 10000;
  yBuf[0] = 10000;
  yBuf[1] = 10000;
  yBuf[2] = 10000;
  zBuf[0] = 10000;
  zBuf[1] = 10000;
  zBuf[2] = 10000;

  Serial.begin(9600);

  pinMode(RXBLE, INPUT);
  pinMode(TXBLE, OUTPUT);
  bleSerial.begin(115200);
}

void loop() {
  if ( bleSerial.available()) {
 byte incommingByte = bleSerial.read();
 Serial.write(incommingByte);
    if (incommingByte == endByte) {
      if(counterBLE != 0){
       //orden the last 3 bytes
       byte temp[3];
      temp[0] = incommingBLE[counterBLE];
      counterBLE = ( counterBLE + 1 ) % messageLimit;
      temp[1] = incommingBLE[counterBLE];
      counterBLE = ( counterBLE + 1 ) % messageLimit;
      temp[2] = incommingBLE[counterBLE];
      vibrate(temp);
      counterBLE = 0;
      }else{
        vibrate(incommingBLE);
      }
    }else{
      incommingBLE[counterBLE] = incommingByte;
      counterBLE = ( counterBLE + 1 ) % messageLimit;
    }
  }
  
  if (millis() - previousRead > 10) {
    previousRead = millis();

    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
    AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

    AcX = abs(AcX);
    xNorm = (AcX * alpha) + (xNorm * (1 - alpha));
    AcX = abs(AcX - xNorm);
    xBuf[0] = xBuf[1];
    xBuf[1] = xBuf[2];
    xBuf[2] = AcX;
    if (xBuf[1] > TAP_MIN_THRESHOLD && xBuf[0]*TAP_SENSITIVITY_LEVEL < xBuf[1] && xBuf[1] > xBuf[2]*TAP_SENSITIVITY_LEVEL) {
      peak.x = 1;
      peak.xIntensity = xBuf[1];
      peak.current = previousRead;
    }
    AcY = abs(AcY);
    yNorm = (AcY * alpha) + (yNorm * (1 - alpha));
    AcY = abs(AcY - yNorm);
    yBuf[0] = yBuf[1];
    yBuf[1] = yBuf[2];
    yBuf[2] = AcY;
    if (yBuf[1] > TAP_MIN_THRESHOLD && yBuf[0]*TAP_SENSITIVITY_LEVEL < yBuf[1] && yBuf[1] > yBuf[2]*TAP_SENSITIVITY_LEVEL) {
      peak.y = 1;
      peak.yIntensity = yBuf[1];
      peak.current = previousRead;
    }
    AcZ = abs(AcZ);
    zNorm = (AcZ * alpha) + (zNorm * (1 - alpha));
    AcZ = abs(AcZ - zNorm);
    zBuf[0] = zBuf[1];
    zBuf[1] = zBuf[2];
    zBuf[2] = AcZ;
    if (zBuf[1] > TAP_MIN_THRESHOLD && zBuf[0]*TAP_SENSITIVITY_LEVEL < zBuf[1] && zBuf[1] > zBuf[2]*TAP_SENSITIVITY_LEVEL) {
      peak.z = 1;
      peak.zIntensity = zBuf[1];
      peak.current = previousRead;
    }

    //handle peaks
    if ((peak.x > 0 || peak.y > 0 || peak.z > 0) &&  peak.current - peak.last > 50) {
      //tap detected, send package over serial
      int intensity = (peak.xIntensity + peak.yIntensity + peak.zIntensity)/3/100;
      byte pattern[4];
      pattern[0] = intensity;//intensity
      pattern[1] = 0x0A;//duration
      if ((peak.current - peak.last) > 1000) {
              pattern[2] = 0x00;//max pause reached
      } else {
             pattern[2] = ((peak.current - peak.last) / 10); //pause
      }
      pattern[3] = 0xFF;//end
    
      bleSerial.write(pattern, 4);
      Serial.write(pattern, 4);
      
      peak.last = peak.current;
      
      vibrate(pattern);
    }
    //reset peak detection
    peak.x = 0;
    peak.xIntensity = 0;
    peak.y = 0;
    peak.yIntensity = 0;
    peak.z = 0;
    peak.zIntensity = 0;

  }
}

void vibrate(byte *pattern){
  Serial.write(pattern, 3);
//  bleSerial.write(pattern, 3);
  
  analogWrite(VIBRATION, pattern[0]);
  delay(pattern[1]*10);
  analogWrite(VIBRATION, 0);
  delay(pattern[2]*10);
}

/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
  while (Serial.available()) {
    byte incommingByte = Serial.read();
    if (incommingByte == endByte) {
      if(counter != 0){
       //orden the last 3 bytes
       byte temp[3];
      temp[0] = incomming[counter];
      counter = ( counter + 1 ) % messageLimit;
      temp[1] = incomming[counter];
      counter = ( counter + 1 ) % messageLimit;
      temp[2] = incomming[counter];
      vibrate(temp);
        counter = 0;
      }else{
        vibrate(incomming);
      }
    }else{
      incomming[counter] = incommingByte;
      counter = ( counter + 1 ) % messageLimit;
    }
  }
}

void resetIncomming(){
  incomming[0] = 0;
  incomming[1] = 0;
  incomming[2] = 0;
}