#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <BluetoothSerial.h>
#include <Kalman.h>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

Adafruit_MPU6050 mpu;
BluetoothSerial BT;
Kalman kalmanX, kalmanY, kalmanZ;

#define PIN_RED    32 // GPIO32
#define PIN_GREEN  35 // GPIO35
#define PIN_BLUE   34 // GPIO34

const int PIN_BUTTON = 33; // GPIO33
bool isProjectOn = false;

void configure();
void sendData();
void setBtOn();
void setBtOff();
void setBtAvailable();

void setup() {
  configure();
}

void loop() {
  if (checkButtonLongPress(PIN_BUTTON, 5000)) {
    isProjectOn = !isProjectOn;


    if (isProjectOn) {
      Serial.println("Linked project!");
    } else {
      Serial.println("Projeto off!");
      esp_sleep_enable_ext0_wakeup((gpio_num_t)PIN_BUTTON, HIGH);  // Ativa o despertar pelo pino do botão
      esp_deep_sleep_start();
    }

    while (digitalRead(PIN_BUTTON) == LOW) {
      delay(10);
    }
  }

  sendData();
  delay(10);
}

void configure() {
  pinMode(PIN_BUTTON, INPUT_PULLUP);

  Wire.begin();
  Serial.begin(115200);
  BT.begin("HAT Tracker");
  Serial.println("Hat Tracker available for pairing");
  setBtOn();

  while(!Serial) 
    delay(10);
  Serial.println("3DOF Hat Tracker!");

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while(1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 found!");

  kalmanX.setAngle(0.0);
  kalmanY.setAngle(0.0);
  kalmanZ.setAngle(0.0);

  pinMode(PIN_RED,   OUTPUT);
  pinMode(PIN_GREEN, OUTPUT);
  pinMode(PIN_BLUE,  OUTPUT);
}

void setBtOn() {
  setColor(102, 255, 51);
}

void setBtOff() {
  setColor(255, 0, 0);
}

void setBtAvailable() {
  setColor(0, 0, 255);
}

void setColor(int R, int G, int B) {
  analogWrite(PIN_RED,   R);
  analogWrite(PIN_GREEN, G);
  analogWrite(PIN_BLUE,  B);
}

void sendData() {
  sensors_event_t accelEvent, gyroEvent, tempEvent;

  mpu.getEvent(&accelEvent, &gyroEvent, &tempEvent);

  float accX = atan2(accelEvent.acceleration.y, sqrt(accelEvent.acceleration.x * accelEvent.acceleration.x + accelEvent.acceleration.z * accelEvent.acceleration.z)) * RAD_TO_DEG;
  float accY = atan2(-accelEvent.acceleration.x, sqrt(accelEvent.acceleration.y * accelEvent.acceleration.y + accelEvent.acceleration.z * accelEvent.acceleration.z)) * RAD_TO_DEG;

  float gyroX = gyroEvent.gyro.x / 131.0;
  float gyroY = gyroEvent.gyro.y / 131.0;
  float gyroZ = gyroEvent.gyro.z / 131.0;

  float roll = kalmanX.getAngle(accX, gyroX, 0.01);
  float pitch = kalmanY.getAngle(accY, gyroY, 0.01);
  float yaw = kalmanZ.getAngle(0.0, gyroZ, 0.01);

  Serial.print(yaw); // Yaw
  Serial.print(" ");
  Serial.print(pitch); // Pitch
  Serial.print(" ");
  Serial.println(roll); // Roll

  BT.print(yaw);
  BT.print(" ");
  BT.print(pitch);
  BT.print(" ");
  BT.println(roll);
}

bool checkButtonLongPress(int pin, unsigned long duration) {
  unsigned long startTime = millis();

  while (digitalRead(pin) == HIGH) {
    delay(10);
    if (millis() - startTime >= duration) {
      return true;
    }
  }

  return false;
}