#include <Wire.h>
#include <MPU6050.h>

#define LED_PIN  D0
#define BUZZ_PIN D2
#define THRESHOLD 5000

MPU6050 mpu;

void setup() {
  Serial.begin(115200);
  while (!Serial);

  pinMode(LED_PIN,  OUTPUT);
  pinMode(BUZZ_PIN, OUTPUT);

  // Startup blink/beep
  digitalWrite(LED_PIN,  HIGH);
  digitalWrite(BUZZ_PIN, HIGH);
  delay(500);
  digitalWrite(LED_PIN,  LOW);
  digitalWrite(BUZZ_PIN, LOW);

  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 not found! Check wiring.");
    // Rapid blink = error
    while (1) {
      digitalWrite(LED_PIN, HIGH);
      delay(100);
      digitalWrite(LED_PIN, LOW);
      delay(100);
    }
  }

  Serial.println("MPU6050 Found!");
}

void loop() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  Serial.print("Accel  X: "); Serial.print(ax);
  Serial.print("  Y: ");      Serial.print(ay);
  Serial.print("  Z: ");      Serial.println(az);

  Serial.print("Gyro   X: "); Serial.print(gx);
  Serial.print("  Y: ");      Serial.print(gy);
  Serial.print("  Z: ");      Serial.println(gz);
  Serial.println("---");

  if (abs(gx) > THRESHOLD ||
      abs(gy) > THRESHOLD ||
      abs(gz) > THRESHOLD) {
    Serial.println(">> MOVING!");
    digitalWrite(LED_PIN,  HIGH);
    digitalWrite(BUZZ_PIN, HIGH);
    delay(300);
  } else {
    digitalWrite(LED_PIN,  LOW);
    digitalWrite(BUZZ_PIN, LOW);
  }

  delay(200);
}