/*
┌────── Move
│       ───┬─Start
│          └─Stop
├───Settings
│   ───────┬─Stepper X
│          │ ────────┬─Direction
│          │         ├─Speed
│          │         └─Steps
│          └─Stepper X
│            ────────┬─Direction
│                    ├─Speed
│                    └─Steps
└─ Disable steppers
   ────────┐
           ├─Disable stepper X
           └─Disable stepper Y
*/

#include <U8g2lib.h>

#define ENCODER_CLK 2
#define ENCODER_DT  3
#define ENCODER_SW  12

int cursor = 0;
int cursor_prev = 0;

U8X8_SSD1306_128X64_NONAME_HW_I2C display(U8X8_PIN_NONE);

#define MENU_SIZE 3

struct MenuItem {
  char* label;
  void (*callback)();
  MenuItem* childItems;
};

void funcMove() {}
void funcStart() {}
void funcStop() {}
void funcStepperXDirection() {}
void funcStepperXSpeed() {}
void funcStepperXSteps() {}
void funcStepperYDirection() {}
void funcStepperYSpeed() {}
void funcStepperYSteps() {}
void funcDisableSteppers() {}
void funcDisableStepperX() {}
void funcDisableStepperY() {}
void funcBack() {}

MenuItem menuBack = {"Back", funcBack, NULL};

MenuItem menuMove[] = {
  {"Start", funcStart, NULL},
  {"Stop", funcStop, NULL},
  menuBack
};

MenuItem menuStepperX[] = {
  {"Direction", funcStepperXDirection, NULL},
  {"Speed", funcStepperXSpeed, NULL},
  {"Steps", funcStepperXSteps, NULL},
  menuBack
};

MenuItem menuStepperY[] = {
  {"Direction", funcStepperYDirection, NULL},
  {"Speed", funcStepperYSpeed, NULL},
  {"Steps", funcStepperYSteps, NULL},
  menuBack
};

MenuItem menuSettings[] = {
  {"Stepper X", NULL, menuStepperX},
  {"Stepper Y", NULL, menuStepperY},
  menuBack
};


MenuItem menuDisStep[] = {
  {"DisStep. X", NULL, menuStepperX},
  {"DisStep. Y", NULL, menuStepperY},
  menuBack
};

MenuItem menu[] = {
  {"Move", NULL, menuMove},
  {"Settings", NULL, menuSettings},
  {"!able steppers", NULL, menuDisStep},
};

void setup() {
  Serial.begin(9600);

  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK),
                  readEncoder, FALLING);

  display.begin();
  display.setPowerSave(0);
  display.setFont(u8x8_font_pxplusibmcgathin_f);

  showMenu();
}

void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT);
  if (dtValue) cursor++;
  else cursor--;

  if (cursor > (MENU_SIZE - 1)) cursor = 0;
  if (cursor < 0) cursor = (MENU_SIZE - 1);

  Serial.println(cursor);
}

void loop() {
  if (digitalRead(ENCODER_SW) == LOW) {
    delay(200);
    executeChoice(cursor);
    MenuItem selectedMenuItem = menu[cursor];
    if (selectedMenuItem.childItems != NULL) {
      // Display submenu
      displaySubMenu(selectedMenuItem.childItems);
    } else {
      // Execute leaf menu item
      display.clearLine(7);
      display.setCursor(0, 7);
      display.print(">>");
      display.print(selectedMenuItem.label);
    }
  }

  if (cursor_prev != cursor) {
    display.setCursor(0, cursor_prev);
    display.print(' ');
    display.setCursor(0, cursor);
    display.print('>');
    cursor_prev = cursor;
  }
}

void showMenu() {
  cursor = 0;
  display.clearDisplay();
  for (int i = 0; i < MENU_SIZE; i++) {
    display.drawString(2, i, menu[i].label);
  }
  display.setCursor(0, 0);
  display.print('>');
}

void displaySubMenu(MenuItem* submenu) {
  Serial.println("displaySubMenu");
  Serial.println(submenu[0].label);
  Serial.println(submenu[1].label);
  Serial.println(submenu[2].label);
  cursor = 0;
  display.clearDisplay();
  int submenuSize = sizeof(submenu[0]) / sizeof(submenu);
  for (int i = 0; i < submenuSize; i++) {
     Serial.println(i);
    display.drawString(2, i, submenu[i].label);
    Serial.println(submenu[i].label);
  }
  display.setCursor(0, 0);
  display.print('>');
}

void executeChoice(int choice) {
  if (0 <= choice && choice < MENU_SIZE)
    Serial.println("Execute choice " + String(choice) + " - " + menu[choice].label);
  else
    Serial.println("Undefined execute choice ");
}