#include <LiquidCrystal_I2C.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

#define I2C_ADDR 0x27
#define LCD_COLUMNS 20
#define LCD_LINES 4
#define BTN_PIN 6
#define ACC_RANGE MPU6050_RANGE_2_G
#define GYRO_RANGE MPU6050_RANGE_250_DEG
#define BANDWIDTH MPU6050_BAND_21_HZ
#define DEBUG false

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);
Adafruit_MPU6050 mpu;

int mode = 0;
int gyroX = 0;
int gyroY = 0;
int gyroZ = 0;

void setup() {
  //Setup LCD
  lcd.init();
  lcd.backlight();
  //Init button
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(BTN_PIN, INPUT_PULLUP);
  //Init gyro
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  if(DEBUG)
    Serial.println("Adafruit MPU6050 test!");
  // Try to initialize!
  if (!mpu.begin()) {
    if(DEBUG)
      Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  mpu.setAccelerometerRange(ACC_RANGE);
  mpu.setGyroRange(GYRO_RANGE);
  mpu.setFilterBandwidth(BANDWIDTH);

  if(DEBUG){
    Serial.println("MPU6050 Found!");
    String range = "";
    switch (mpu.getAccelerometerRange()) {
      case MPU6050_RANGE_2_G:
        range = "+-2G";
        break;
      case MPU6050_RANGE_4_G:
        range = "+-4G";
        break;
      case MPU6050_RANGE_8_G:
        range = "+-8G";
        break;
      case MPU6050_RANGE_16_G:
        range = "+-16G";
        break;
    }
    Serial.print("Accelerometer range set to: ");
    Serial.println(range);
    String gyroRange = "";
    switch (mpu.getGyroRange()) {
      case MPU6050_RANGE_250_DEG:
        gyroRange = "+- 250 deg/s";
        break;
      case MPU6050_RANGE_500_DEG:
        gyroRange = "+- 500 deg/s";
        break;
      case MPU6050_RANGE_1000_DEG:
        gyroRange = "+- 1000 deg/s";
        break;
      case MPU6050_RANGE_2000_DEG:
        gyroRange = "+- 2000 deg/s";
        break;
    }
    Serial.print("Gyro range set to: ");
    Serial.println(gyroRange);
    String band = "";
    switch (mpu.getFilterBandwidth()) {
      case MPU6050_BAND_260_HZ:
        band = "260 Hz";
        break;
      case MPU6050_BAND_184_HZ:
        band = "184 Hz";
        break;
      case MPU6050_BAND_94_HZ:
        band = "94 Hz";
        break;
      case MPU6050_BAND_44_HZ:
        band = "44 Hz";
        break;
      case MPU6050_BAND_21_HZ:
        band = "21 Hz";
        break;
      case MPU6050_BAND_10_HZ:
        band = "10 Hz";
        break;
      case MPU6050_BAND_5_HZ:
        band = "5 Hz";
        break;
    }
    Serial.print("Filter bandwidth set to: ");
    Serial.println(band);
    Serial.println("");
  }
  delay(100);
}

void loop() {   
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  //print values
  if(DEBUG){
    debug(a, g, temp);
  }
  //Switch mode it button pressed
  if (digitalRead(BTN_PIN) == LOW) {
    while(digitalRead(BTN_PIN) == LOW){
      continue;
    }
    blinkLed(500);      
    switchMode();
  }
  //Display values
  display(a, g, temp);
}

void switchMode() {
  if(mode < 2){
    mode++;
  }else if(mode == 2) {
    mode = 0;
  }
  lcd.clear();
}

void display(sensors_event_t a, sensors_event_t g, sensors_event_t temp) {
  switch(mode) {
    case 0:
    default:
      displayAcc(a);
      break;
    case 1:
      displayGyr(g);
      break;
    case 2:
      displayTemp(temp);
      break;
  }
}

void displayAcc(sensors_event_t a) {
  lcd.setCursor(3, 0);
  lcd.print("ACCELEROMETER");

  lcd.setCursor(0, 1);
  lcd.print("X:");
  lcd.setCursor(3, 1);
  lcd.print(a.acceleration.x);
  lcd.setCursor(15, 1);
  lcd.print("m/s^2");

  lcd.setCursor(0, 2);
  lcd.print("Y:");
  lcd.setCursor(3, 2);
  lcd.print(a.acceleration.y);
  lcd.setCursor(15, 2);
  lcd.print("m/s^2");

  lcd.setCursor(0, 3);
  lcd.print("Z:");
  lcd.setCursor(3, 3);
  lcd.print(a.acceleration.z);
  lcd.setCursor(15, 3);
  lcd.print("m/s^2");
}

void displayGyr(sensors_event_t g) {
  lcd.setCursor(5, 0);
  lcd.print("GYROSCOPE");

  lcd.setCursor(0, 1);
  lcd.print("X:");
  lcd.setCursor(3, 1);
  lcd.print(g.gyro.x);
  lcd.setCursor(15, 1);
  lcd.print("rad/s");
  blinkOnchange(g.gyro.x, gyroX);
  gyroX = g.gyro.x;

  lcd.setCursor(0, 2);
  lcd.print("Y:");
  lcd.setCursor(3, 2);
  lcd.print(g.gyro.y);
  lcd.setCursor(15, 2);
  lcd.print("rad/s");
  blinkOnchange(g.gyro.y, gyroY);
  gyroY = g.gyro.y;

  lcd.setCursor(0, 3);
  lcd.print("Z:");
  lcd.setCursor(3, 3);
  lcd.print(g.gyro.z);
  lcd.setCursor(15, 3);
  lcd.print("rad/s");
  blinkOnchange(g.gyro.z, gyroZ);
  gyroZ = g.gyro.z;
}

void displayTemp(sensors_event_t temp) {
  lcd.setCursor(4, 0);
  lcd.print("TEMPERATURE");

  lcd.setCursor(0, 1);
  lcd.print("C");
  lcd.print(char(223));
  lcd.print(":");
  lcd.setCursor(4, 1);
  lcd.print(temp.temperature);

  lcd.setCursor(0, 2);
  lcd.print("F");
  lcd.print(char(223));
  lcd.print(":");
  lcd.setCursor(4, 2);
  lcd.print((temp.temperature * 9/5) + 32);
}

void blinkOnchange(int current, int previous) {
  if(current != previous){
    blinkLed(100); 
  }
}

void blinkLed(int delayMs) {
  digitalWrite(LED_BUILTIN, HIGH);
  delay(delayMs);
  digitalWrite(LED_BUILTIN, LOW);  
}

void debug(sensors_event_t a, sensors_event_t g, sensors_event_t temp) {
  /* Print out the values */
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");

  Serial.println("");
  delay(1000);
}