#include <U8glib.h> // include U8glib library
#include <Wire.h> // include Wire library for i2c communication

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE | U8G_I2C_OPT_DEV_0); // OLED display object

// define pins for rotary encoder
const int CLK = 2;
const int DT = 3;
const int SW = 4;

// define initial values for menu items
int motorAPower = 50;
int motorBPower = 60;
int launchAngle = 25;

// define menu state variables
int selectedMenu = 0; // selected menu item index
int oldValue = 0; // previous value of selected menu item
int newValue = 0; // new value of selected menu item

// function to update menu item value
void updateValue(int delta) {
  switch (selectedMenu) {
    case 0: // Motor A Power
      motorAPower += delta;
      motorAPower = constrain(motorAPower, 0, 100);
      newValue = motorAPower;
      break;
    case 1: // Motor B Power
      motorBPower += delta;
      motorBPower = constrain(motorBPower, 0, 100);
      newValue = motorBPower;
      break;
    case 2: // Launch Angle
      launchAngle += delta;
      launchAngle = constrain(launchAngle, 0, 90);
      newValue = launchAngle;
      break;
  }
}

void setup() {
  // initialize rotary encoder pins
  pinMode(CLK, INPUT);
  pinMode(DT, INPUT);
  pinMode(SW, INPUT_PULLUP);

  // initialize display
  u8g.firstPage();
  do {
    u8g.setFont(u8g_font_profont12);
    u8g.drawStr(10, 10, "Motor A Power:");
    u8g.drawStr(10, 20, "Motor B Power:");
    u8g.drawStr(10, 30, "Launch Angle:");
    u8g.drawStr(0, 10, ">");
    u8g.setPrintPos(100, 10);
    u8g.print(motorAPower);
    u8g.setPrintPos(100, 20);
    u8g.print(motorBPower);
    u8g.setPrintPos(100, 30);
    u8g.print(launchAngle);
  } while (u8g.nextPage());
}

void loop() {
  // read rotary encoder pins
  static int lastEncoded = 0;
  static int encoded = 0;
  static int value = 0;
  static int direction = 0;
  encoded = (digitalRead(DT) << 1) | digitalRead(CLK);
  int sum  = (lastEncoded << 2) | encoded;
  lastEncoded = encoded;
  if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
    direction = 1; // clockwise
  }
  if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
    direction = -1; // counter-clockwise
  }
  value += direction;
  value = constrain(value, -2, 2);

  // detect rotary encoder switch press
  static bool buttonPressed = false;
  if (digitalRead(SW) == LOW) {
    if (!buttonPressed) {
      selectedMenu = (selectedMenu + 1) % 3; // cycle through menu items
      oldValue = newValue; // save previous value
      switch (selectedMenu) {
        case 0: // Motor A Power
          newValue = motorAPower;
          break;
        case 1: // Motor B Power
          newValue = motorBPower;
          break;
        case 2: // Launch Angle
          newValue = launchAngle;
          break;
      }
    }
    buttonPressed = true;
  } else {
    buttonPressed = false;
  }

  // update menu item value if changed
  if (value != 0) {
    updateValue(value);
  }

  // redraw display if menu item value or selected menu item changed
  if (newValue != oldValue || value != 0) {
    u8g.firstPage();
    do {
      u8g.setFont(u8g_font_profont12);
      u8g.drawStr(10, 10, "Motor A Power:");
      u8g.drawStr(10, 20, "Motor B Power:");
      u8g.drawStr(10, 30, "Launch Angle:");
      if (selectedMenu == 0) {
        u8g.drawStr(0, 10, ">");
      }
      if (selectedMenu == 1) {
        u8g.drawStr(0, 20, ">");
      }
      if (selectedMenu == 2) {
        u8g.drawStr(0, 30, ">");
      }
      u8g.setPrintPos(100, 10);
      u8g.print(motorAPower);
      u8g.setPrintPos(100, 20);
      u8g.print(motorBPower);
      u8g.setPrintPos(100, 30);
      u8g.print(launchAngle);
    } while (u8g.nextPage());
    oldValue = newValue; // save new value as old value
  }
}