#include "Wire.h"
#include <SoftwareSerial.h>
#include "EBYTE22.h"
#include "MPU6050.h"

#define PIN_TX 2
#define PIN_RX 3
#define PIN_M0 4
#define PIN_M1 5
#define PIN_AX 6

MPU6050 mpu;
SoftwareSerial E22Serial(PIN_TX, PIN_RX, false);
EBYTE22 E22(&E22Serial, PIN_M0, PIN_M1, PIN_AX);

int16_t ax, ay, az;
int16_t gx, gy, gz;
long ACC, GYR;
long maxACC, maxGYR, trACC, trGYR;
unsigned long timing;
unsigned long N = 1000;
long maxCount = 100; //max = 548
long count = 0;
byte sig = 0x01010101;//менять здесь для разных звуков

void setup() {

  Wire.begin();
  E22Serial.begin(9600);
  Serial.begin(9600);
  delay(200);
  if (E22.init()) {
    Serial.println("init OK");
  } else {
    Serial.println("init Error");
  }
  mpu.initialize();
  mpu.setFullScaleAccelRange(0x00);
  delay(2000);

  // "калибровка" максимальных значений
  for (int i = 0; i < 30; i++) {
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    ACC = abs(ax) + abs(ay) + abs(az);
    GYR = abs(gx) + abs(gy) + abs(gz);
    if (ACC > maxACC) maxACC = ACC;
    if (GYR > maxGYR) maxGYR = GYR;
    delay(5);
  }
  maxACC = maxACC + trACC;
  maxGYR = maxGYR + trGYR;
}

void loop() {

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  // сумма модулей
  ACC = abs(ax) + abs(ay) + abs(az);
  GYR = abs(gx) + abs(gy) + abs(gz);

  if (ACC > maxACC || GYR > maxGYR) {
    count++;
    if (millis() - timing >= N && count >= maxCount) {
      Serial.print(count);
      timing = millis();
      count = 0;
      Serial.println(" KEK!");
      if (E22.getBusy()) {                       
        E22.completeTask(500);                 
      }
      E22.sendByte(sig);
      E22.flush();
      delay(500);
    }
  }

}