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

#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 64
#define OLED_RESET     4 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

MPU6050 accelgyro;

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

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

#define BT_PU A2
#define BT_PD A3
#define BT_VU A0
#define BT_VD A1
#define BT_LF A6
#define BT_RG A7

int menu = 0;
int act = 0;

void setup() {                
  Wire.begin();
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); 
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.display();

  accelgyro.initialize();

  pinMode(BT_PU, INPUT_PULLUP);
  pinMode(BT_PD, INPUT_PULLUP);
  pinMode(BT_VU, INPUT_PULLUP);
  pinMode(BT_VD, INPUT_PULLUP);
  pinMode(BT_LF, INPUT_PULLUP);
  pinMode(BT_RG, INPUT_PULLUP);
}

void loop() {
  

  if (digitalRead(BT_PD) == LOW) {
    menu = (menu + 1) % 4;
    delay(200); // debounce
  }
  if (digitalRead(BT_PU) == LOW) {
    menu = (menu - 1 + 4) % 4;
    delay(200); // debounce
  }
  if (digitalRead(BT_VU) == LOW) {
    if (menu == 0) Kp += 0.1;
    else if (menu == 1) Ki += 0.1;
    else if (menu == 2) Kd += 0.1;
    else if (menu == 3) Power += 0.1;
    delay(200); // debounce
  }
  if (digitalRead(BT_VD) == LOW) {
    if (menu == 0) Kp -= 0.1;
    else if (menu == 1) Ki -= 0.1;
    else if (menu == 2) Kd -= 0.1;
    else if (menu == 3) Power -= 0.1;
    delay(200); // debounce
  }

  if (digitalRead(BT_RG) == LOW){

  }
  if (digitalRead(BT_LF) == LOW){

  }
  
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("PID Stability Control");
  display.println("=====================");
  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.println("" + String(menu == 3 ? "> " : "  ") + "Power: " + String(Power));
  display.setCursor(0, 55);
  display.println("         START       ");
  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();
}