#include <AccelStepper.h>
#include <U8g2lib.h>
#include <Bounce2.h>

byte button_pins[] = {3, 4, 5}; // button pins, 3,5 = up/down, 4 = select
#define NUMBUTTONS sizeof(button_pins)
Bounce * buttons = new Bounce[NUMBUTTONS];

const int m1StepPin = 6;
const int m2StepPin = 7;
const int m3StepPin = 8;
const int m4StepPin = 9;

const int m1DirPin = 10;
const int m2DirPin = 11;
const int m3DirPin = 12;
const int m4DirPin = 13;

#define motorInterfaceType 1

AccelStepper stepM1(motorInterfaceType, m1StepPin, m1DirPin);
AccelStepper stepM2(motorInterfaceType, m2StepPin, m2DirPin);
AccelStepper stepM3(motorInterfaceType, m3StepPin, m3DirPin);
AccelStepper stepM4(motorInterfaceType, m4StepPin, m4DirPin);

const int stepsPerRevolution = 200;

U8X8_SSD1306_128X64_NONAME_HW_I2C display(U8X8_PIN_NONE);

int mainMenuLength = 5;
char *menu[5] = {"Main Menu", "X Axis", "Y Axis", "Z Axis", "Extruder"};

int stepperMenuLength = 6;
char *moveStepperMenu[6] = {"Stepper Motor", "-10 mm", "-1 mm", "1 mm", "10 mm", "back"};

int m1StepPos = 0;
int m2StepPos = 0;
int m3StepPos = 0;
int m4StepPos = 0;

int cursor=1;
int selectedMotor = -1;
int currentMenuLen = 0;
int currentMenu = 0;

void setup() {

  stepM1.setMaxSpeed(1000);
  stepM1.setAcceleration(50);
  stepM1.setSpeed(200);

  stepM2.setMaxSpeed(1000);
  stepM2.setAcceleration(50);
  stepM2.setSpeed(200);

  stepM3.setMaxSpeed(1000);
  stepM3.setAcceleration(50);
  stepM3.setSpeed(200);

  stepM4.setMaxSpeed(1000);
  stepM4.setAcceleration(50);
  stepM4.setSpeed(200);

  for (int i=0; i<NUMBUTTONS; i++) {
    buttons[i].attach( button_pins[i], INPUT_PULLUP); // setup the bounce instance for the current button
    buttons[i].interval(25); // interval in ms
  }

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

void showMainMenu() {
  cursor=1;
  display.clearDisplay();
  for (int i = 0; i<mainMenuLength; i++) {
    display.drawString(2,i,menu[i]);
  }
  currentMenu = 0;
  currentMenuLen = mainMenuLength;
  display.setCursor(0,1);
  display.print('>');
}

void showStepperMenu() {
  cursor=1;
  display.clearDisplay();
  for (int i = 0; i<stepperMenuLength; i++) {
    display.drawString(2,i,moveStepperMenu[i]);
  }
  currentMenu = 1;
  currentMenuLen = stepperMenuLength;
  display.setCursor(0,1);
  display.print('>');
}

void moveStepperMotor(int movementAmount){
  switch (selectedMotor){
    case 1 : 
      m1StepPos += movementAmount;
      stepM1.moveTo(m1StepPos);
      break;
    case 2 : 
      m2StepPos += movementAmount;
      stepM2.moveTo(m2StepPos);
      break;
    case 3 : 
      m3StepPos += movementAmount;
      stepM3.moveTo(m3StepPos);
      break;
    case 4 : 
      m4StepPos += movementAmount;
      stepM4.moveTo(m4StepPos);
      break;
  }
}

void executeChoice(int choice){
    switch(choice) {
      case 1 :
                moveStepperMotor(-10);
                break;
      case 2 :
                moveStepperMotor(-1);
                break;
      case 3 :
                moveStepperMotor(1);
                break;
      case 4 :
                moveStepperMotor(10);
                break;
      case 5 :
                showMainMenu();
                break;
      default :
                Serial.print("This menu item should not work");
                break;
  }
}

void loop() {
  for (int i = 0; i<NUMBUTTONS; i++) {
    buttons[i].update(); // Update the Bounce instance
    if ( buttons[i].fell() ) { // If it fell
      if (i==1) { // select
         display.clearLine(7);
         display.setCursor(0,7);
         if (currentMenu == 0)
         {
          selectedMotor=cursor;
          moveStepperMenu[0] = menu[cursor];
          showStepperMenu();
         }
         else{
          executeChoice(cursor);
         }
      }
      else {
        // erase previous cursor:
        display.setCursor(0,cursor);
        display.print(' ');
        if (i==0) { // up
          cursor++;
          if (cursor>(currentMenuLen-1)) cursor=1;
        }
        else { // down
          cursor--;
          if (cursor<1) cursor=(currentMenuLen-1);
        }
        // show cursor at new line:
        display.setCursor(0,cursor);
        display.print('>');
      }
    }
  }

  stepM1.run(); 
  stepM2.run(); 
  stepM3.run(); 
  stepM4.run(); 
}