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

const uint8_t pinRelay = 2;
const uint8_t pinButton = 3;
const uint8_t pinLed = 4;

const int inclination = 16383;

MPU6050 mpu;

int16_t ax, ay, az;
// int16_t gx, gy, gz;
int ax_offset = 0;
int ay_offset = 0;
int az_offset = 0;

void blinkLed(uint8_t n = 1);
void calibrate(int n = 10, int* ax_ofs=NULL, int* ay_ofs=NULL, int* az_ofs=NULL);

void setup() {
  pinMode(pinRelay, OUTPUT);
  pinMode(pinButton, INPUT_PULLUP);
  pinMode(pinLed, OUTPUT);

  Wire.begin();
  mpu.initialize();
  blinkLed();
  while (!mpu.testConnection()) {
    delay(10);
  }
  blinkLed();
  Serial.begin(9600);
  while (!Serial);
  blinkLed();

  // Pressione o botão para iniciar a calibragem
  while (digitalRead(pinButton) == HIGH) {
    delay(500);
  }
  blinkLed();
  // Calibrando
  calibrate(50, &ax_offset, &ay_offset, &az_offset);
  blinkLed(2);
  // Calibrado!
}

void loop() {
  mpu.getAcceleration(&ax, &ay, &az);

  Serial.print(ax); Serial.print("\t");
  Serial.print(ay); Serial.print("\t");
  Serial.println(az);

  if (abs(ax - ax_offset) > inclination - ax_offset) {
    digitalWrite(pinRelay, HIGH);
  } else {
    digitalWrite(pinRelay, LOW);
  }

  delay(1000);
}

void calibrate(int n = 10, int* ax_ofs=NULL, int* ay_ofs=NULL, int* az_ofs=NULL) {
  int cont = 1;
  int32_t soma_x = 0;
  int32_t soma_y = 0;
  int32_t soma_z = 0;
  while (cont < n) {
    mpu.getAcceleration(&ax, &ay, &az);
    soma_x += ax;
    soma_y += ay;
    soma_z += az;
    cont++;
  }
  *ax_ofs = soma_x / cont;
  *ay_ofs = soma_y / cont;
  *az_ofs = soma_z / cont;
}

void blinkLed(uint8_t n = 1) {
  while (n > 0) {
    digitalWrite(pinLed, HIGH);
    delay(200);
    digitalWrite(pinLed, LOW);
    delay(200);
    n--;
  }
}