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

#define OLED_ADDR   0x3C
Adafruit_SSD1306 display(-1);

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

double Kp=2, Ki=5, Kd=1;

#define BT_UP A2
#define BT_DW A3
#define BT_RG A1
#define BT_LF A0

int menu = 0;

void setup() {                
  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.display();

  accelgyro.initialize();

  pinMode(BT_UP, INPUT_PULLUP);
  pinMode(BT_DW, INPUT_PULLUP);
  pinMode(BT_RG, INPUT_PULLUP);
  pinMode(BT_LF, INPUT_PULLUP);
}

void loop() {
  
  if (digitalRead(BT_RG) == LOW) {
    menu = (menu + 1) % 3;
    delay(200); // debounce
  }
  if (digitalRead(BT_LF) == LOW) {
    menu = (menu - 1 + 3) % 3;
    delay(200); // debounce
  }
  if (digitalRead(BT_UP) == LOW) {
    if (menu == 0) Kp += 0.1;
    else if (menu == 1) Ki += 0.1;
    else if (menu == 2) Kd += 0.1;
    delay(200); // debounce
  }
  if (digitalRead(BT_DW) == LOW) {
    if (menu == 0) Kp -= 0.1;
    else if (menu == 1) Ki -= 0.1;
    else if (menu == 2) Kd -= 0.1;
    delay(200); // debounce
  }
  
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("" + String(menu == 0 ? "> " : "  ") + "Kp: " + String(Kp));
  display.println("" + String(menu == 1 ? "> " : "  ") + "Ki: " + String(Ki));
  display.println("" + String(menu == 2 ? "> " : "  ") + "Kd: " + String(Kd));
  display.display();

  // accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  // display.println("MPU6050 Data:");
  // display.print("AX: "); display.println(ax);
  // display.print("AY: "); display.println(ay);
  // display.print("AZ: "); display.println(az);
  // display.print("GX: "); display.println(gx);
  // display.print("GY: "); display.println(gy);
  // display.print("GZ: "); display.println(gz);
  // display.display();
}