#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <LiquidCrystal_I2C.h>

MPU6050 mpu;

bool dmpReady = false;  // set true if DMP init was successful
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint8_t fifoBuffer[64]; // FIFO storage buffer
uint32_t total_sleep_movement;
uint32_t time_periods_elapsed;
uint32_t average_sleep_movement;
const int buttonPin = 2;
const int ledPin = 13; 

Quaternion q;           // quaternion container
VectorInt16 aa;         // accel sensor measurements
VectorInt16 aaReal;     // gravity-free accel sensor measurements
VectorFloat gravity;    // gravity vector
LiquidCrystal_I2C lcd(0x27, 16, 2);
void setup() {
    total_sleep_movement = 0;
    time_periods_elapsed = 0;
    pinMode(buttonPin, INPUT_PULLUP);
    pinMode(ledPin, OUTPUT);
    lcd.init();
    // Turn on the backlight
    lcd.backlight();
    // Print a message to the LCD
    lcd.print("Hello, world!");
    Wire.begin();
    Serial.begin(115200);
    mpu.initialize();
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
    devStatus = mpu.dmpInitialize();
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);
    if (devStatus == 0) {
        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);
        mpu.setDMPEnabled(true);
        // attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    }
}

void ShowSleepSituation() {
  lcd.clear();
  average_sleep_movement = total_sleep_movement / time_periods_elapsed;
  if (average_sleep_movement > 1000) {
    digitalWrite(ledPin, HIGH); // Turn on the LED
  } else {
    digitalWrite(ledPin, LOW);  // Turn off the LED
  }
  if (digitalRead(buttonPin) == LOW) {
    lcd.print("Avg move: ");
    lcd.print(average_sleep_movement); 
  } else {
  if(average_sleep_movement < 100) {
    lcd.print("Very Good Sleep");
  } else if(average_sleep_movement < 500) {
    lcd.print("Good Sleep");
  } else if (average_sleep_movement < 1000) {
    lcd.print("Decent Sleep");
  } else{
    lcd.print("Bad Sleep");
  }
  Serial.println(average_sleep_movement);
  } 
}

void loop() {
    if (!dmpReady) return;
    if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetAccel(&aa, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
        Serial.print("areal\t");
        Serial.print(aaReal.x);
        Serial.print("\t");
        Serial.print(aaReal.y);
        Serial.print("\t");
        Serial.println(aaReal.z);
    }
    if(abs(aaReal.x) > abs(aaReal.y)) {
      total_sleep_movement += abs(aaReal.x);
    } else {
      total_sleep_movement += abs(aaReal.y);
    }
    time_periods_elapsed++;
    if(time_periods_elapsed % 5 == 0) {
      ShowSleepSituation();
    }

    delay(500);
}

void dmpDataReady() {
    // MPU interrupt function - this just sets a flag in the main code
}