#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPR121.h>
#include <Arduino.h>

#include "menu.h"
#include "settings.h"
#include "hardware.h"
#include "config.h"
#include "globals.h"
#include "midi.h"
#include "numenu.h"
#include "led.h"

enum CursorIdx {
  EMain,
  EBreath,
  EControl,
  ERotator,
  EVibrato,
  EExtras,
  // NEVER ADD ANYTHING AFTER THIS, ONLY ABOVE
  NUM_CURSORS
};

// Allocate some space for cursors
static byte cursors[CursorIdx::NUM_CURSORS];
static byte offsets[CursorIdx::NUM_CURSORS];
static byte activeSub[CursorIdx::NUM_CURSORS];

byte cursorNow;
static byte FPD = 0;


// constants
const unsigned long debounceDelay = 30;           // the debounce time; increase if the output flickers
const unsigned long buttonRepeatInterval = 50;
const unsigned long buttonRepeatDelay = 400;
const unsigned long cursorBlinkInterval = 300;    // the cursor blink toggle interval time
const unsigned long patchViewTimeUp = 2000;       // ms until patch view shuts off
const unsigned long menuTimeUp = 60000;           // menu shuts off after one minute of button inactivity


static unsigned long menuTime = 0;
static unsigned long patchViewTime = 0;
unsigned long cursorBlinkTime = 0;          // the last time the cursor was toggled

//Display state
static byte menuState= DISPLAYOFF_IDL;
static byte stateFirstRun = 1;

// The external function of subSquelch has been broken,
// need to come up with a smart way to make it work again.
// The status led was update when the Squelch menu was open.
byte subVibSquelch = 0; //extern

 // 'NuEVI' logo
#define LOGO16_GLCD_WIDTH  128
#define LOGO16_GLCD_HEIGHT 64
static const unsigned char PROGMEM nuevi_logo_bmp[] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xe3, 0x60, 0x00, 0x07, 0x73, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xe3, 0x60, 0x00, 0x0e, 0xe3, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x03, 0x60, 0x00, 0x1d, 0xc3, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xbf, 0xff, 0xff, 0xe3, 0x60, 0x00, 0x3b, 0x83, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xbf, 0xff, 0xff, 0xe3, 0x60, 0x00, 0x77, 0x03, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x60, 0x00, 0xee, 0x03, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x60, 0x01, 0xdc, 0x03, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x60, 0x03, 0xb8, 0x03, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x20, 0x00, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x60, 0x07, 0x70, 0x03, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x60, 0x00, 0x01, 0xbf, 0xff, 0xff, 0xe3, 0x60, 0x0e, 0xe0, 0x03, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x60, 0x00, 0x01, 0xbf, 0xff, 0xff, 0xe3, 0x60, 0x1d, 0xc0, 0x03, 0x60, 0x00,
  0x00, 0x03, 0x00, 0x60, 0x00, 0x01, 0x80, 0x00, 0x00, 0x03, 0x60, 0x3b, 0x80, 0x03, 0x60, 0x00,
  0x00, 0x03, 0x00, 0xe0, 0x00, 0x01, 0xbf, 0xff, 0xff, 0xe3, 0x60, 0x77, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x03, 0x00, 0xc0, 0x00, 0x01, 0xbf, 0xff, 0xff, 0xe3, 0x60, 0xee, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x03, 0x80, 0xc0, 0x00, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x61, 0xdc, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x07, 0x80, 0xc0, 0x00, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x63, 0xb8, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x07, 0xc0, 0xc0, 0x00, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x67, 0x70, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x06, 0xc0, 0xc0, 0x00, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x6e, 0xe0, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x06, 0x60, 0xc1, 0x01, 0x01, 0xb0, 0x00, 0x00, 0x03, 0x7d, 0xc0, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x06, 0x30, 0xc3, 0x03, 0x01, 0xbf, 0xff, 0xff, 0xe3, 0x7b, 0x80, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x0c, 0x30, 0xc3, 0x07, 0x01, 0xbf, 0xff, 0xff, 0xe3, 0x77, 0x00, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x0c, 0x1c, 0xc3, 0x06, 0x01, 0x80, 0x00, 0x00, 0x03, 0x0e, 0x00, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x0c, 0x0c, 0xc2, 0x0e, 0x01, 0xff, 0xff, 0xff, 0xe3, 0xfc, 0x00, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x0c, 0x0e, 0xc6, 0x1e, 0x01, 0xff, 0xff, 0xff, 0xe3, 0xf8, 0x00, 0x00, 0x03, 0x60, 0x00,
  0x00, 0x0c, 0x07, 0xc6, 0x1e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x0c, 0x03, 0xc6, 0x76, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x0c, 0x01, 0xc7, 0xe6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x0c, 0x00, 0xc7, 0xc6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x0c, 0x00, 0x03, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};


extern void readSwitches(void);
extern Adafruit_MPR121 touchSensor;

extern const MenuPage mainMenuPage; // Forward declaration.


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

void initDisplay() {

  // by default, we'll generate the high voltage from the 3.3v line internally! (neat!)
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3D (for the 128x64)

  // Show image buffer on the display hardware.
  // Since the buffer is intialized with an Adafruit splashscreen
  // internally, this will display the splashscreen.

  display.clearDisplay();
  display.drawBitmap(0,0,nuevi_logo_bmp,LOGO16_GLCD_WIDTH,LOGO16_GLCD_HEIGHT,1);
  display.display();

  memset(cursors, 0, sizeof(cursors));
  memset(offsets, 0, sizeof(offsets));
  memset(activeSub, 0, sizeof(activeSub));
}

void showVersion() {
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(85,52);
  display.print("v.");
  display.println(FIRMWARE_VERSION);
  display.display();
}

// Assumes dest points to a buffer of atleast 7 bytes.
static const char* numToString(int16_t value, char* dest, bool plusSign = false) {
  char* ptr = dest;
  uint16_t absVal = abs(value);
  int c = 1;

  if(value < 0) {
    *ptr++ = '-';
  } else if(plusSign && value) {
    *ptr++ = '+';
  }
  while((c*10) < absVal+1) c *= 10;
  while(c > 0) {
    int tmp = absVal / c;
    *ptr++ = tmp + '0';
    absVal %= c;
    c /= 10;
  }
  *ptr = 0;
  return dest;
}

static void plotSubOption(const char* label, const char* unit = nullptr) {
  int text_x, unit_x;
  int label_pixel_width = strlen(label)*12;

  if(unit == nullptr) {
    text_x = 96 - (label_pixel_width/2);
  } else {
      int unit_pixel_width = strlen(unit)*6;
      int halfSum = (label_pixel_width + unit_pixel_width)/2;
      text_x = 96 - halfSum;
      unit_x = 96 + halfSum - unit_pixel_width;
      display.setCursor(unit_x,40);
      display.setTextSize(1);
      display.println(unit);
  }

    display.setTextSize(2);
    display.setCursor(text_x,33);
    display.println(label);
}

static bool drawSubMenu(const MenuPage *page) {
    int index = cursors[page->cursor];
    // TODO: Null check subMenuFunc
    const MenuEntry* subEntry =  page->entries[index];
    switch(subEntry->type) {
      case MenuType::ESub:
        {
          char buffer[12];
          const char* labelPtr = nullptr;
          const MenuEntrySub* sub = (const MenuEntrySub*)subEntry;
          sub->getSubTextFunc(*sub, buffer, &labelPtr);

          // If EMenuEntryCustom flag is set, we assume that the getSubTextFunc 
          // rendered by it self.
          if( !(sub->flags & EMenuEntryCustom)) {
            plotSubOption(buffer, labelPtr);
          }
        }
        break;

      default:
        break;
    }
    return true;
}

static void clearSub(){
  display.fillRect(63,11,64,52,BLACK);
}

static void clearSubValue() {
  display.fillRect(65, 24, 60, 37, BLACK);
}

static bool updateSubMenuCursor(const MenuPage *page, uint32_t timeNow)
{
  if ((timeNow - cursorBlinkTime) > cursorBlinkInterval) {
    cursorBlinkTime = timeNow;
    if (cursorNow == WHITE) {
      cursorNow = BLACK;
      clearSubValue();
      return true;
    } else {
      cursorNow = WHITE;
      return drawSubMenu(page);
    }
  }
  return false;
}

static void plotMenuEntries(const MenuPage *page, bool clear = false) {
  int row = 0;
  if(clear) {
    display.fillRect( 0, MENU_HEADER_OFFSET, 63, 64-MENU_HEADER_OFFSET, BLACK);
  }
  display.setTextSize(1);
  int offset = offsets[page->cursor];
  for(int item = offset; (item < page->numEntries) && (row < MENU_NUM_ROWS); item++, row++) {
    int rowPixel = (row)*MENU_ROW_HEIGHT + MENU_HEADER_OFFSET;
    const char* lineText = page->entries[item]->title;
    display.setCursor(0,rowPixel);
    display.println(lineText);
  }
  
  if(offset)
    display.drawTriangle(58, MENU_HEADER_OFFSET, 57, MENU_HEADER_OFFSET+3, 59, MENU_HEADER_OFFSET+3, WHITE);

  if((offset+MENU_NUM_ROWS) < page->numEntries)
    display.drawTriangle(58, 63, 57, 60, 59, 60, WHITE);
}

typedef void (*MenuTitleGetFunc)(char*out);

static void drawMenu(const MenuPage *page) {
  //Initialize display and draw menu header + line
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0,0);

  if(page->flags & EMenuCustomTitle) {
    // This is a bit hacky, but we are reusing the title pointer as a function pointer
    MenuTitleGetFunc func = (MenuTitleGetFunc)page->title;
    char buffer[23];
    func(buffer);
    display.println(buffer);
  } else
    display.println(page->title);
  display.drawLine(0, MENU_ROW_HEIGHT, 127, MENU_ROW_HEIGHT, WHITE);

  plotMenuEntries(page);
}

static void mainTitleGetStr(char* out) {
  //Construct the title including voltage reading.
  //Involves intricate splicing of the title string with battery voltage
  char menuTitle[] = "MENU         XXX Y.YV"; //Allocate string buffer of appropriate size with some placeholders
  char* splice1 = menuTitle + 13;
  char* splice2 = menuTitle + 17;

  int vMeterReading = analogRead(vMeterPin);
  memcpy(splice1, (vMeterReading > 3000) ? "USB" : "BAT", 3);
  if (vMeterReading < 2294) {
    memcpy(splice2, "LOW ", 4);
  } else {
    int voltage = map(vMeterReading,0,3030,0,50);
    splice2[0] = (voltage/10)+'0';
    splice2[2] = (voltage%10)+'0';
  }
  strncpy(out, menuTitle, 22);
}

static void drawTrills(){
  if (K5) display.fillRect(0,0,5,5,WHITE); else display.drawRect(0,0,5,5,WHITE);
  if (K6) display.fillRect(10,0,5,5,WHITE); else display.drawRect(10,0,5,5,WHITE);
  if (K7) display.fillRect(20,0,5,5,WHITE); else display.drawRect(20,0,5,5,WHITE);
}

static void drawPatchView(){
  display.clearDisplay();
  if (FPD){
    drawTrills();
  }
  display.setTextSize(6);
  if (FPD < 2){
    int align;
    if (patch < 10) { // 1-9
      align = 48;
    } else if (patch < 100) { // 10-99
      align = 31;
    } else { // 100-128 use default
      align = 10;
    }
    display.setCursor(align,10);
    display.println(patch);
  } else if (FPD == 2){
    display.setCursor(10,10);
    display.println("SET");
  } else {
    display.setCursor(10,10);
    display.println("CLR");
  }
}

static void drawSubBox(const char* label)
{
  display.fillRect(63,11,64,52,BLACK);
  display.drawRect(63,11,64,52,WHITE);
  display.setTextSize(1);
  int len = strlen(label);

  display.setCursor(95-len*3,15);
  display.println(label);
}

void drawMenuCursor(byte itemNo, byte color){
  byte ymid = 15 + 9 * itemNo;
  display.drawTriangle(57, ymid,61, ymid+2,61, ymid-2, color);
}

//***********************************************************

static int readTrills() {
  readSwitches();
  return K5+2*K6+trill3_interval*K7;
}

//***********************************************************

static void setFPS(int trills, uint16_t patchNum) {
  fastPatch[trills-1] = patchNum;
  writeSetting(FP1_ADDR+2*(trills-1), patchNum);
  FPD = 2;
}

//***********************************************************

static void clearFPS(int trills) {
  fastPatch[trills-1] = 0;
  writeSetting(FP1_ADDR+2*(trills-1),0);
  FPD = 3;
}

//***********************************************************
// Main menu
const MenuEntrySub transposeMenu = {
  MenuType::ESub, "TRANSPOSE", "TRANSPOSE", &transpose, 0, 24, MenuEntryFlags::ENone, 
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    numToString(transpose - 12, out, true);
  },
  [](const MenuEntrySub &sub) { writeSetting(TRANSP_ADDR,*sub.valuePtr); }
  , nullptr
};

const MenuEntrySub octaveMenu = {
  MenuType::ESub, "OCTAVE", "OCTAVE", &octave, 0, 6, MenuEntryFlags::ENone, 
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    numToString(octave-3, out, true);
  },
  [](SubMenuRef __unused) { writeSetting(OCTAVE_ADDR,octave); }
  , nullptr
};

static void midiSaveFunc(const MenuEntrySub & __unused sub) { writeSetting(MIDI_ADDR, MIDIchannel); }
static void midiCustomDrawFunc(SubMenuRef __unused, char* __unused, const char** __unused) {
  char buff[7];
  numToString(MIDIchannel, buff);
  plotSubOption(buff);
  if (slowMidi) {
    display.setTextSize(1);
    display.setCursor(116,51);
    display.print("S");
  }
}

//***********************************************************

const MenuEntrySub legacyPBMenu = {
  MenuType::ESub, "LEGACY PB", "LEGACY PB", &legacy, 0, 1, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char ** __unused unit) {
    strncpy(out, legacy?"ON":"OFF", 4);
  }, [](const MenuEntrySub & __unused sub) {
    setBit(dipSwBits, DIPSW_LEGACY, legacy);
    writeSetting(DIPSW_BITS_ADDR,dipSwBits);
  }
  , nullptr
};

const MenuEntrySub legacyBRMenu = {
  MenuType::ESub, "LEGACY BR", "LEGACY BR", &legacyBrAct, 0, 1, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char ** __unused unit) {
    strncpy(out, legacyBrAct?"ON":"OFF", 4);
  }, [](const MenuEntrySub & __unused sub) {
    setBit(dipSwBits, DIPSW_LEGACYBRACT, legacyBrAct);
    writeSetting(DIPSW_BITS_ADDR, dipSwBits);
  }
  , nullptr
};

const MenuEntrySub gateOpenMenu = {
  MenuType::ESub, "GATE HOLD", "GATE HOLD", &gateOpenEnable, 0, 1, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char ** __unused unit) {
    strncpy(out, gateOpenEnable?"ON":"OFF", 4);
  }, [](const MenuEntrySub & __unused sub) {
    setBit(dipSwBits, DIPSW_GATEOPEN, gateOpenEnable);
    writeSetting(DIPSW_BITS_ADDR, dipSwBits);
  }
  , nullptr
};

const MenuEntrySub specialKeyMenu = {
  MenuType::ESub, "SPEC KEY", "SPEC KEY", &specialKeyEnable, 0, 1, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char ** __unused unit) {
    strncpy(out, specialKeyEnable?"ON":"OFF", 4);
  }, [](const MenuEntrySub & __unused sub) {
    setBit(dipSwBits, DIPSW_SPKEYENABLE, specialKeyEnable);
    writeSetting(DIPSW_BITS_ADDR, dipSwBits);
  }
  , nullptr
};

const MenuEntrySub trill3Menu = {
  MenuType::ESub, "3RD TRILL", "3RD TRILL", &trill3_interval, 3, 4, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    numToString(trill3_interval, out, true);
  },
  [](SubMenuRef __unused) { writeSetting(TRILL3_INTERVAL_ADDR, trill3_interval); }
  , nullptr
};

const MenuEntrySub bcasModeMenu = {
  MenuType::ESub, "BCAS MODE", "BCAS MODE", &bcasMode, 0, 1, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    strncpy(out, bcasMode?"ON":"OFF", 4);
  },
  [](SubMenuRef __unused) {
    setBit(dipSwBits, DIPSW_BCASMODE, bcasMode);
    writeSetting(DIPSW_BITS_ADDR, dipSwBits);
  }
  , nullptr
};

const MenuEntrySub dacModeMenu = {
  MenuType::ESub, "DAC OUT", "DAC OUT", &dacMode, 0, 1, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    const char* dacModeLabels[] = { "BRTH", "PTCH"};
    strncpy(out, dacModeLabels[dacMode], 5);
  },
  [](SubMenuRef __unused) { writeSetting(DAC_MODE_ADDR, dacMode); }
  , nullptr
};

const MenuEntrySub fastBootMenu = {
  MenuType::ESub, "FAST BOOT", "FAST BOOT", &fastBoot, 0, 1, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    strncpy(out, fastBoot?"ON":"OFF", 4);
  },
  [](SubMenuRef __unused) {
    setBit(dipSwBits, DIPSW_FASTBOOT, fastBoot);
    writeSetting(DIPSW_BITS_ADDR, dipSwBits);
  }
  , nullptr
};


static uint16_t wireless_power=0;
static uint16_t wireless_channel=4;

const MenuEntrySub wlPowerMenu = {
  MenuType::ESub, "WL POWER", "WL POWER", &wireless_power, 0, 3, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    numToString(-6*wireless_power, out, true);
  },
  [](SubMenuRef __unused) { sendWLPower(wireless_power); }
  , nullptr
};

const MenuEntrySub wlChannelMenu = {
  MenuType::ESub, "WL CHAN", "WL CHAN", &wireless_channel, 4, 80, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    numToString(wireless_channel, out, false);
  },
  [](SubMenuRef __unused) { sendWLChannel(wireless_channel); }
  , nullptr
};


const MenuEntry* extrasMenuEntries[] = {
  (MenuEntry*)&legacyPBMenu,
  (MenuEntry*)&legacyBRMenu,
  (MenuEntry*)&gateOpenMenu,
  (MenuEntry*)&specialKeyMenu,
  (MenuEntry*)&trill3Menu,
  (MenuEntry*)&bcasModeMenu,
  (MenuEntry*)&dacModeMenu,
  (MenuEntry*)&fastBootMenu,
  (MenuEntry*)&wlPowerMenu,
  (MenuEntry*)&wlChannelMenu,
};

const MenuPage extrasMenuPage = {
  "EXTRAS",
  0,
  CursorIdx::EExtras,
  MAIN_MENU,
  ARR_LEN(extrasMenuEntries), extrasMenuEntries
}; 

static bool midiEnterHandlerFunc() {
  readSwitches();
  if (pinkyKey){
    slowMidi = !slowMidi;
    dipSwBits = dipSwBits ^ (1<<3);
    writeSetting(DIPSW_BITS_ADDR,dipSwBits);
    return false;
  } else {
    writeSetting(MIDI_ADDR, MIDIchannel);
    return true;
  }
}

const MenuEntrySub midiMenu = {
  MenuType::ESub, "MIDI CH", "MIDI CHNL", &MIDIchannel, 1, 16, EMenuEntryCustom | EMenuEntryEnterHandler, 
  midiCustomDrawFunc, midiSaveFunc, midiEnterHandlerFunc
};

const MenuEntryStateCh adjustMenu     = { MenuType::EStateChange, "ADJUST", ADJUST_MENU };
const MenuEntryStateCh breathMenu     = { MenuType::EStateChange, "SETUP BR", SETUP_BR_MENU };
const MenuEntryStateCh controlMenu    = { MenuType::EStateChange, "SETUP CTL", SETUP_CT_MENU };
const MenuEntryStateCh extrasMenu     = { MenuType::EStateChange, "EXTRAS", EXTRAS_MENU };
const MenuEntryStateCh aboutMenu      = { MenuType::EStateChange, "ABOUT", ABOUT_MENU };

const MenuEntry* mainMenuEntries[] = {
  (MenuEntry*)&transposeMenu,
  (MenuEntry*)&octaveMenu,
  (MenuEntry*)&midiMenu,
  (MenuEntry*)&adjustMenu,
  (MenuEntry*)&breathMenu,
  (MenuEntry*)&controlMenu,
  (MenuEntry*)&extrasMenu,
  (MenuEntry*)&aboutMenu,
};

const MenuPage mainMenuPage = {
  (const char*)mainTitleGetStr,
  EMenuPageRoot | EMenuCustomTitle,
  CursorIdx::EMain,
  DISPLAYOFF_IDL,
  ARR_LEN(mainMenuEntries), mainMenuEntries
};

//***********************************************************
// Rotator menu

static void rotatorSave(const MenuEntrySub& __unused sub) {
  int16_t stored;
  for(int i = 0; i < 4; ++i) {
    stored = readSetting(ROTN1_ADDR+2*i);
    if(stored != rotations[i])
      writeSetting(ROTN1_ADDR+2*i,(rotations[i]));
  }
}

static void rotatorOptionGet(SubMenuRef sub, char *out, const char** __unused unit) {
  numToString((*sub.valuePtr) - 24, out, true);
}

static void parallelOptionGet(SubMenuRef __unused, char *out, const char** __unused unit) {
  numToString(parallel-24, out, true);
}

static void parallelSave(SubMenuRef __unused) {
  writeSetting(PARAL_ADDR, parallel);
}

const MenuEntrySub rotatorParaMenu  = {
  MenuType::ESub, "PARALLEL", "SEMITONES", &parallel, 0, 48, MenuEntryFlags::ENone,
  parallelOptionGet, parallelSave, nullptr
};

const MenuEntrySub rotator1Menu  = {
  MenuType::ESub, "ROTATE 1", "SEMITONES", &rotations[0], 0, 48, MenuEntryFlags::ENone,
  rotatorOptionGet, rotatorSave, nullptr
};

const MenuEntrySub rotator2Menu  = {
  MenuType::ESub, "ROTATE 2", "SEMITONES", &rotations[1], 0, 48, MenuEntryFlags::ENone,
  rotatorOptionGet, rotatorSave, nullptr
};

const MenuEntrySub rotator3Menu  = {
  MenuType::ESub, "ROTATE 3", "SEMITONES", &rotations[2], 0, 48, MenuEntryFlags::ENone,
  rotatorOptionGet, rotatorSave, nullptr
};

const MenuEntrySub rotator4Menu  = {
  MenuType::ESub, "ROTATE 4", "SEMITONES", &rotations[3], 0, 48, MenuEntryFlags::ENone,
  rotatorOptionGet, rotatorSave, nullptr
};

static void rotatorPrioOptionGet(SubMenuRef __unused, char* out, const char** __unused) {
    if (priority) strncpy(out, "ROT", 4);
    else strncpy(out, "MEL", 4);
}

static void rotatorPrioSave(SubMenuRef __unused) {
  writeSetting(PRIO_ADDR,priority);
}

const MenuEntrySub rotatorPrioMenu = {
  MenuType::ESub, "PRIORITY", "MONO PRIO", &priority, 0,1, MenuEntryFlags::EMenuEntryWrap, 
  rotatorPrioOptionGet, rotatorPrioSave, nullptr,
};

const MenuEntry* rotatorMenuEntries[] = {
  (MenuEntry*)&rotatorParaMenu,
  (MenuEntry*)&rotator1Menu,
  (MenuEntry*)&rotator2Menu,
  (MenuEntry*)&rotator3Menu,
  (MenuEntry*)&rotator4Menu,
  (MenuEntry*)&rotatorPrioMenu
};

const MenuPage rotatorMenuPage = {
  "ROTATOR SETUP",
  EMenuPageRoot,
  CursorIdx::ERotator,
  DISPLAYOFF_IDL,
  ARR_LEN(rotatorMenuEntries), rotatorMenuEntries
};

//***********************************************************
// Breath menu
const MenuEntrySub breathCCMenu = {
  MenuType::ESub, "BRTH CC1", "BRTH CC1", &breathCC, 0, 10, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    const char* breathCCMenuLabels[] = { "OFF", "MW", "BR", "VL", "EX", "MW+",
                                            "BR+", "VL+", "EX+", "CF", "UNO" };
    strncpy(out, breathCCMenuLabels[breathCC], 4);
  },
  [](const MenuEntrySub & __unused sub){
    if (readSetting(BREATH_CC_ADDR) != breathCC) {
      writeSetting(BREATH_CC_ADDR,breathCC);
      midiReset();
    }
  }
  , nullptr
};

const MenuEntrySub breathCC2Menu = {
  MenuType::ESub, "BRTH CC2",  "BRTH CC2", &breathCC2, 0, 127, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    if(breathCC2) numToString(breathCC2, out);
    else strncpy(out, "OFF", 4);
  },
  [](const MenuEntrySub & __unused sub){
    if (readSetting(BREATH_CC2_ADDR) != breathCC2) {
      writeSetting(BREATH_CC2_ADDR,breathCC2);
      midiReset();
    }
  }
  , nullptr
};

const MenuEntrySub breathCC2RiseMenu = {
  MenuType::ESub, "CC2 RISE", "CC2 RISE", &breathCC2Rise, 1, 10, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char *out, const char** label) {
    numToString(breathCC2Rise, out);
    *label = "X";
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(BREATH_CC2_RISE_ADDR,breathCC2Rise); }
  , nullptr
};


const MenuEntrySub breathATMenu = {
  MenuType::ESub, "BREATH AT", "BREATH AT", &breathAT, 0, 1, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char ** __unused unit) {
    strncpy(out, breathAT?"ON":"OFF", 4);
  }, [](const MenuEntrySub & __unused sub) {
    if (readSetting(BREATH_AT_ADDR) != breathAT) {
      writeSetting(BREATH_AT_ADDR, breathAT);
      midiReset();
    }
  }
  , nullptr
};

const MenuEntrySub velocityMenu = {
  MenuType::ESub, "VELOCITY",  "VELOCITY", &velocity, 0, 127, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    if(velocity) numToString(velocity, out);
    else strncpy(out, "DYN", 4);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VELOCITY_ADDR,velocity); }
  , nullptr
};

static void curveCustomDraw(SubMenuRef __unused, char* __unused out, const char** __unused unit) {
  const char* curveMenuLabels[] = {"-4", "-3", "-2", "-1", "LIN", "+1", "+2",
                                        "+3", "+4", "S1", "S2", "Z1", "Z2" };
  int y0 = 0, x0 = 0;
  int scale = ((1<<14)-1)/60;
  for(int x = x0; x < 60; x+=1) {
    int y = multiMap(x*scale, curveIn, curves[curve], 17);
    y = (y*37) / ((1<<14)-1);
    display.drawLine(x0 + 65, 60 - y0, x + 65, 60 - y, WHITE);
    x0 = x; y0 = y;
  }
  display.setCursor(125 - 3*6, 60-8 );
  display.setTextSize(0);
  display.print(curveMenuLabels[curve]);
}

static void curveWriteSettings(const MenuEntrySub & __unused sub){ writeSetting(BREATHCURVE_ADDR,curve); }

const MenuEntrySub curveMenu = {
  MenuType::ESub, "CURVE", "CURVE", &curve, 0, 12, MenuEntryFlags::EMenuEntryWrap | MenuEntryFlags::EMenuEntryCustom,
  curveCustomDraw, curveWriteSettings, nullptr
};

const MenuEntrySub velSmpDlMenu = {
  MenuType::ESub, "VEL DELAY", "VEL DELAY", &velSmpDl, 0, 30, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char *out, const char** label) {
    if (velSmpDl) {
      numToString(velSmpDl, out);
      *label = "ms";
    } else strncpy(out, "OFF", 4);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VEL_SMP_DL_ADDR,velSmpDl); }
  , nullptr
};

const MenuEntrySub velBiasMenu = {
  MenuType::ESub, "VEL BOOST", "VEL BOOST", &velBias, 0, 9, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    if (velBias) numToString(velBias, out);
    else strncpy(out, "OFF", 4);
  },
  [](SubMenuRef __unused){ writeSetting(VEL_BIAS_ADDR,velBias); }
  , nullptr
};

const MenuEntry* breathMenuEntries[] = {
  (MenuEntry*)&breathCCMenu,
  (MenuEntry*)&breathCC2Menu,
  (MenuEntry*)&breathCC2RiseMenu,
  (MenuEntry*)&breathATMenu,
  (MenuEntry*)&velocityMenu,
  (MenuEntry*)&curveMenu,
  (MenuEntry*)&velSmpDlMenu,
  (MenuEntry*)&velBiasMenu
};

const MenuPage breathMenuPage = {
  "SETUP BREATH",
  0,
  CursorIdx::EBreath,
  MAIN_MENU,
  ARR_LEN(breathMenuEntries), breathMenuEntries
};

//***********************************************************
// Control menu
const MenuEntrySub portMenu = {
  MenuType::ESub, "PORT/GLD", "PORT/GLD", &portamento, 0, 2, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused,char* out, const char ** __unused unit) {
    const char* labs[] = { "OFF", "ON", "SW" };
    strncpy(out, labs[portamento], 4);
  },
  [](SubMenuRef __unused sub) { writeSetting(PORTAM_ADDR,portamento); }
  , nullptr
};

const MenuEntrySub pitchBendMenu = {
  MenuType::ESub, "PITCHBEND", "PITCHBEND", &PBdepth, 0, 12, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    if(PBdepth) {
      memcpy(out, "1/", 2);
      numToString(PBdepth, &out[2]);
    }
    else strncpy(out, "OFF", 4);
  },
  [](SubMenuRef __unused){ writeSetting(PB_ADDR,PBdepth); }
  , nullptr
};

const MenuEntrySub extraMenu = {
  MenuType::ESub, "EXTRA CTR", "EXTRA CTR", &extraCT, 0,4, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused,char* out, const char** __unused unit) {
    const char* extraMenuLabels[] = { "OFF", "MW", "FP", "CF", "SP" };
    strncpy(out, extraMenuLabels[extraCT], 12);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(EXTRA_ADDR,extraCT); }
  , nullptr
};

const MenuEntrySub extraCC2Menu = {
  MenuType::ESub, "EXCTR CC2",  "EXCTR CC2", &extraCT2, 0, 127, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    if(extraCT2) numToString(extraCT2, out);
    else strncpy(out, "OFF", 4);
  },
[](const MenuEntrySub & __unused sub) { writeSetting(EXTRA2_ADDR,extraCT2); }
  , nullptr
};

const MenuEntryStateCh vibratoSubMenu = { MenuType::EStateChange, "VIBRATO", VIBRATO_MENU };

const MenuEntrySub deglitchMenu = {
  MenuType::ESub, "DEGLITCH", "DEGLITCH", &deglitch, 0, 70, MenuEntryFlags::ENone,
  [](SubMenuRef __unused,char* textBuffer, const char** label) {
    if(deglitch) {
      numToString(deglitch, textBuffer);
      *label = "ms";
    } else
      strncpy(textBuffer, "OFF", 4);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(DEGLITCH_ADDR,deglitch); }
  , nullptr
};

const MenuEntrySub pinkyMenu = {
  MenuType::ESub, "PINKY KEY", "PINKY KEY", &pinkySetting, 0, 28, MenuEntryFlags::ENone,
  [](SubMenuRef __unused,char* textBuffer, const char** __unused unit) {
    if (pinkySetting == PBD)
      strncpy(textBuffer, "PBD", 4);
    else if (pinkySetting == EC2)
      strncpy(textBuffer, "EC2", 4);
    else if (pinkySetting == ECSW)
      strncpy(textBuffer, "ECS", 4);
    else if (pinkySetting == LVL)
      strncpy(textBuffer, "LVL", 4);
    else if (pinkySetting == LVLP)
      strncpy(textBuffer, "LVP", 4);
    else
      numToString(pinkySetting-12, textBuffer, true);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(PINKY_KEY_ADDR,pinkySetting); }
  , nullptr
};

const MenuEntrySub lvlCtrlCCMenu = {
  MenuType::ESub, "LEVEL CC",  "LEVEL CC", &levelCC, 0, 127, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    if(levelCC) numToString(levelCC, out);
    else strncpy(out, "AT", 4);
  },
[](const MenuEntrySub & __unused sub) { writeSetting(LEVEL_CC_ADDR,levelCC); }
  , nullptr
};

const MenuEntry* controlMenuEntries[] = {
  (MenuEntry*)&portMenu,
  (MenuEntry*)&pitchBendMenu,
  (MenuEntry*)&extraMenu,
  (MenuEntry*)&extraCC2Menu,
  (MenuEntry*)&vibratoSubMenu,
  (MenuEntry*)&deglitchMenu,
  (MenuEntry*)&pinkyMenu,
  (MenuEntry*)&lvlCtrlCCMenu
};

const MenuPage controlMenuPage = {
  "SETUP CTRLS",
  0,
  CursorIdx::EControl,
  MAIN_MENU,
  ARR_LEN(controlMenuEntries), controlMenuEntries
};


//***********************************************************
// Vibrato menu

static void vibGetStr(SubMenuRef __unused, char* textBuffer, const char** __unused unit) {
  if(vibrato)
    numToString(vibrato, textBuffer);
  else
    strncpy(textBuffer, "OFF", 4);
}

static void vibStore(const MenuEntrySub & __unused sub) {
  writeSetting(VIBRATO_ADDR,vibrato);
}

const MenuEntrySub vibDepthMenu = {
  MenuType::ESub, "DEPTH", "LEVEL", &vibrato, 0, 9, MenuEntryFlags::ENone,
  vibGetStr, 
  vibStore,
  nullptr
};

const MenuEntrySub vibRetnMenu = {
  MenuType::ESub, "RETURN", "LEVEL", &vibRetn, 0, 4, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* textBuffer, const char** __unused unit) {
    numToString(vibRetn, textBuffer);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VIB_RETN_ADDR,vibRetn); }
  , nullptr
};

const MenuEntrySub vibSenseMenu = {
  MenuType::ESub, "SENSE LVR", "LEVEL", &vibSens, 1, 12, MenuEntryFlags::ENone,
  [](SubMenuRef __unused,char* textBuffer, const char** __unused unit) {
    numToString(vibSens, textBuffer);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VIB_SENS_ADDR,vibSens); }
  , nullptr
};

const MenuEntrySub vibSquelchMenu = {
  MenuType::ESub, "SQUELCH L", "LEVEL", &vibSquelch, 1, 30, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* textBuffer, const char** __unused unit) {
    numToString(vibSquelch, textBuffer);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VIB_SQUELCH_ADDR,vibSquelch); }
  , nullptr
};

const MenuEntrySub vibSenseBiteMenu = {
  MenuType::ESub, "SENSE BTE", "LEVEL", &vibSensBite, 1, 17, MenuEntryFlags::ENone,
  [](SubMenuRef __unused,char* textBuffer, const char** __unused unit) {
    numToString(vibSensBite, textBuffer);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VIB_SENS_BITE_ADDR,vibSensBite); }
  , nullptr
};

const MenuEntrySub vibSquelchBiteMenu = {
  MenuType::ESub, "SQUELCH B", "LEVEL", &vibSquelchBite, 1, 30, MenuEntryFlags::ENone,
  [](SubMenuRef __unused, char* textBuffer, const char** __unused unit) {
    numToString(vibSquelchBite, textBuffer);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VIB_SQUELCH_BITE_ADDR,vibSquelchBite); }
  , nullptr
};

const MenuEntrySub vibControlMenu = {
  MenuType::ESub, "CONTROL", "CONTROL", &vibControl , 0, 2, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    if (2 == vibControl)
      strncpy(out, "BTH", 4);
    else if (1 == vibControl)
      strncpy(out, "BIT", 4);
    else
      strncpy(out, "LVR", 4);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VIB_CONTROL_ADDR,vibControl); }
  , nullptr
};

const MenuEntrySub vibDirMenu = {
  MenuType::ESub, "DIRECTION", "DIRECTION", &vibDirection , 0, 1, MenuEntryFlags::EMenuEntryWrap,
  [](SubMenuRef __unused, char* out, const char** __unused unit) {
    if (DNWD == vibDirection)
      strncpy(out, "NRM", 4);
    else
      strncpy(out, "REV", 4);
  },
  [](const MenuEntrySub & __unused sub) { writeSetting(VIB_DIRECTION_ADDR,vibDirection); }
  , nullptr
};

const MenuEntry* vibratorMenuEntries[] = {
    (MenuEntry*)&vibControlMenu,
    (MenuEntry*)&vibDepthMenu,
    (MenuEntry*)&vibRetnMenu,
    (MenuEntry*)&vibDirMenu,
    (MenuEntry*)&vibSenseMenu,
    (MenuEntry*)&vibSquelchMenu,
    (MenuEntry*)&vibSenseBiteMenu,
    (MenuEntry*)&vibSquelchBiteMenu,
};

const MenuPage vibratoMenuPage = {
  "VIBRATO", 0, CursorIdx::EVibrato, SETUP_CT_MENU, ARR_LEN(vibratorMenuEntries), vibratorMenuEntries
};


//***********************************************************

static bool patchPageUpdate(KeyState& __unused input, uint32_t __unused timeNow);
static bool idlePageUpdate(KeyState& __unused input, uint32_t __unused timeNow);

const MenuPageCustom adjustMenuPage = { nullptr, EMenuPageCustom, adjustPageUpdate };
const MenuPageCustom patchMenuPage =  { nullptr, EMenuPageCustom, patchPageUpdate };
const MenuPageCustom idleMenuPage =   { nullptr, EMenuPageCustom, idlePageUpdate };

const MenuPageCustom aboutMenuPage =  { nullptr, EMenuPageCustom,
  [](KeyState& __unused input, uint32_t __unused timeNow) -> bool {
    static uint32_t timer = 0;
    if(stateFirstRun) {
      display.clearDisplay();
      timer = timeNow + 3000;
      stateFirstRun = 0;
      display.setCursor(49,0);
      display.setTextColor(WHITE);
      display.setTextSize(0);
      display.println("NuEVI");

      display.setCursor(16,12);
      display.print("firmware v.");
      display.println(FIRMWARE_VERSION);

      return true;
    } else {
      if(timeNow >= timer) {
        menuState = MAIN_MENU;
        stateFirstRun = 1;
      }
      return false;
    }
  }
};


//***********************************************************

static bool selectMenuOption(const MenuPage *page) {
  int cursorPosition = cursors[page->cursor];
  const MenuEntry* menuEntry = page->entries[cursorPosition];
  cursorBlinkTime = millis();

  switch(menuEntry->type) {
    case MenuType::ESub:
      activeSub[page->cursor] = cursorPosition+1;
      drawMenuCursor(cursorPosition-offsets[page->cursor], WHITE);
      drawSubBox( ((const MenuEntrySub*)menuEntry)->subTitle);
      drawSubMenu(page);
      return true;

    case MenuType::EStateChange:
      menuState= ((const MenuEntryStateCh*)menuEntry)->state;
      stateFirstRun = 1;
      break;
  }

  return false;
}

//***********************************************************

static bool updateSubMenu(const MenuPage *page, KeyState &input, uint32_t timeNow) {

  bool redraw = false;
  bool redrawSubValue = false;
  if (input.changed) {
    int current_sub = activeSub[page->cursor] -1;

    if( current_sub < 0)
      return false;

    auto sub = (const MenuEntrySub*)page->entries[current_sub];
    uint16_t currentVal = *sub->valuePtr;

    switch (input.current){
      case BTN_DOWN:
        if(currentVal > sub->min) {
          currentVal -= 1;
        } else if(sub->flags & MenuEntryFlags::EMenuEntryWrap) {
          currentVal = sub->max;
        }
        break;

      case BTN_UP:
        if(currentVal < sub->max) {
          currentVal += 1;
        } else if(sub->flags & MenuEntryFlags::EMenuEntryWrap) {
          currentVal = sub->min;
        }
        break;

      case BTN_ENTER:
        if(sub->flags & EMenuEntryEnterHandler) {
          bool result = sub->onEnterFunc();
          if(result) {
            activeSub[page->cursor] = 0;
          }
        } else {
          activeSub[page->cursor] = 0;
          sub->applyFunc(*sub);
        }
        break;

      case BTN_MENU:
        activeSub[page->cursor] = 0;
        sub->applyFunc(*sub);
        break;
    }
    *sub->valuePtr = currentVal;
    redrawSubValue = true;
  } else {
    redraw = updateSubMenuCursor( page, timeNow );
  }

  if(redrawSubValue) {
    clearSubValue();
    redraw |= drawSubMenu(page);
    cursorNow = BLACK;
    cursorBlinkTime = timeNow;
  }

  return redraw;
}

static bool updateMenuPage(const MenuPage *page, KeyState &input, uint32_t timeNow) {
  byte cursorPos = cursors[page->cursor];
  byte newPos = cursorPos;
  bool redraw = false;

  if (input.changed) {
    int lastEntry = page->numEntries-1;
    switch (input.current) {
      case BTN_DOWN:
        if (cursorPos < lastEntry)
          newPos = cursorPos+1;
        break;

      case BTN_ENTER:
        redraw |= selectMenuOption(page);
        break;

      case BTN_UP:
        if (cursorPos > 0)
          newPos = cursorPos-1;
        break;

      case BTN_MENU:
        menuState= page->parentPage;
        stateFirstRun = 1;
        break;
    }

    if(newPos != cursorPos) {

      int offset = offsets[page->cursor];
      drawMenuCursor(cursorPos-offset, BLACK); // Clear old cursor

      if(page->numEntries >= MENU_NUM_ROWS) {
        // Handle scrolling..
        if((newPos - offset) > (MENU_NUM_ROWS-2) ) {
          offset = newPos - (MENU_NUM_ROWS-2);
        } else if( (newPos - offset) < 1) {
          offset = newPos - 1;
        }

        offset = constrain(offset, 0, page->numEntries - MENU_NUM_ROWS);

        if( offset != offsets[page->cursor]) {
          offsets[page->cursor] = offset;
          plotMenuEntries(page, true);
        }
      }

      drawMenuCursor(newPos-offset, WHITE);
      cursorNow = BLACK;
      clearSub();
      redraw = true;
    }
    cursors[page->cursor] = newPos;
  } else if ((timeNow - cursorBlinkTime) > cursorBlinkInterval) {
    // Only need to update cursor blink if no buttons were pressed
    if (cursorNow == WHITE) cursorNow = BLACK; else cursorNow = WHITE;
    drawMenuCursor(cursorPos-offsets[page->cursor], cursorNow);
    redraw = true;
    cursorBlinkTime = timeNow;
  }

  return redraw;
}

static bool updatePage(const MenuPage *page, KeyState &input, uint32_t timeNow) {

  display.setTextColor(WHITE);

  if(page->flags & EMenuPageCustom) {
    const MenuPageCustom* custom = (const MenuPageCustom*)page;
    return custom->menuUpdateFunc(input, timeNow);
  }

  bool redraw = stateFirstRun;

  if (stateFirstRun) {
    drawMenu(page);
    stateFirstRun = 0;
  }
  if (activeSub[page->cursor]) {
    redraw = updateSubMenu(page, input, timeNow);
  } else {
    redraw = updateMenuPage(page, input, timeNow);

    if((page->flags & EMenuPageRoot) && input.changed) {
        int trills = readTrills();
        switch (input.current) {
          case BTN_MENU+BTN_ENTER:
            if (trills) {
              menuState = PATCH_VIEW;
              stateFirstRun = 1;
              setFPS(trills, patch);
            }
            break;

          case BTN_MENU+BTN_UP:
            if (trills) {
              menuState = PATCH_VIEW;
              stateFirstRun = 1;
              clearFPS(trills);
            }
            break;
          default: break;
        }
    }
  }

  return redraw;
}


//***********************************************************


static bool updateSensorPixelsFlag = false;
void drawSensorPixels() {
  updateSensorPixelsFlag = true;
}

//***********************************************************


bool adjustPageUpdate(KeyState &input, uint32_t timeNow) {
  // This is a hack to update touch_Thr is it was changed..
  int old_thr = ctouchThrVal;
  int result = updateAdjustMenu(timeNow, input, stateFirstRun, updateSensorPixelsFlag);
  bool redraw = false;

  updateSensorPixelsFlag = false;
  stateFirstRun = 0;

  if(result < 0) {
    // Go back to main menu
    menuState= MAIN_MENU;
    stateFirstRun = true;
  } else {
    redraw = result;
  }

  if( old_thr != ctouchThrVal) {
    touch_Thr = map(ctouchThrVal,ctouchHiLimit,ctouchLoLimit,ttouchLoLimit,ttouchHiLimit);
  }
  return redraw;
}


static bool patchPageUpdate(KeyState& input, uint32_t timeNow) {
  bool redraw = stateFirstRun;

  if (stateFirstRun) {
    display.ssd1306_command(SSD1306_DISPLAYON);
    drawPatchView();
    patchViewTime = timeNow;
    stateFirstRun = 0;
  }
  if ((timeNow - patchViewTime) > patchViewTimeUp) {
    menuState= DISPLAYOFF_IDL;
    stateFirstRun = 1;
    doPatchUpdate = 1;
    FPD = 0;
    writeSetting(PATCH_ADDR,patch);
  }
  if (input.changed) {
    patchViewTime = timeNow;
    int trills = readTrills();
    switch (input.current){
      case BTN_DOWN:
      // fallthrough
      case BTN_UP:
        if (trills && (fastPatch[trills-1] > 0)){
          patch = fastPatch[trills-1];
          activePatch = 0;
          doPatchUpdate = 1;
          FPD = 1;
          writeSetting(PATCH_ADDR,patch);
        } else if (!trills){
          patch = (((patch-1u) + ((input.current == BTN_UP)?1u:-1u))&127u) + 1u;
          activePatch = 0;
          doPatchUpdate = 1;
          FPD = 0;
        }
        drawPatchView();
        redraw = true;
        break;

      case BTN_ENTER:
        if (trills && (fastPatch[trills-1] > 0)){
          patch = fastPatch[trills-1];
          activePatch = 0;
          doPatchUpdate = 1;
          FPD = 1;
          drawPatchView();
          redraw = true;
        }
        break;


      case BTN_MENU:
        if (FPD < 2){
          menuState= DISPLAYOFF_IDL;
          stateFirstRun = 1;
          doPatchUpdate = 1;
        }
        writeSetting(PATCH_ADDR,patch);
        FPD = 0;
        break;

      case BTN_MENU+BTN_ENTER:
          display.clearDisplay();
          display.setTextSize(2);
          display.setCursor(35,15);
          display.println("DON'T");
          display.setCursor(35,30);
          display.println("PANIC");
          display.display(); // call display explicitly _before_ we call midiPanic
          midiPanic();
          break;

        case BTN_MENU+BTN_ENTER+BTN_UP+BTN_DOWN:
        //all keys depressed, reboot to programming mode
        _reboot_Teensyduino_();
    }
  }

  return redraw;
}


static bool idlePageUpdate(KeyState& __unused input, uint32_t __unused timeNow) {
  bool redraw = false;
  if (stateFirstRun) {
    display.ssd1306_command(SSD1306_DISPLAYOFF);
    display.clearDisplay();
    redraw = true;
    stateFirstRun = 0;
  }
  if (input.changed) {
    int trills = readTrills();
    switch (input.current){
      case BTN_UP:
        // fallthrough
      case BTN_DOWN:
        if (!trills) {
          patch = (((patch-1u) + ((input.current == BTN_UP)?1u:-1u))&127u) + 1u;
        } 
        // fallthrough
      case BTN_ENTER:
        if (trills && (fastPatch[trills-1] > 0)){
          patch = fastPatch[trills-1];
          activePatch = 0;
          doPatchUpdate = 1;
          FPD = 1;
        }
        menuState= PATCH_VIEW;
        stateFirstRun = 1;
        break;

      case BTN_MENU:
        if (pinkyKey && (exSensor >= ((extracThrVal+extracMaxVal)/2))) { // switch breath activated legacy settings on/off
          legacyBrAct = !legacyBrAct;
          dipSwBits = dipSwBits ^ (1<<2);
          writeSetting(DIPSW_BITS_ADDR,dipSwBits);
          statusLedBlink();
        } else if ((exSensor >= ((extracThrVal+extracMaxVal)/2))) { // switch pb pad activated legacy settings control on/off
          legacy = !legacy;
          dipSwBits = dipSwBits ^ (1<<1);
          writeSetting(DIPSW_BITS_ADDR,dipSwBits);
          statusLedBlink();
        } else if (pinkyKey && !specialKey){ //hold pinky key for rotator menu, and if too high touch sensing blocks regular menu, touching special key helps
          display.ssd1306_command(SSD1306_DISPLAYON);
          menuState= ROTATOR_MENU;
          stateFirstRun = 1;
        } else {
          display.ssd1306_command(SSD1306_DISPLAYON);
          menuState = MAIN_MENU;
          stateFirstRun = 1;
        }
        break;

      case BTN_UP | BTN_DOWN | BTN_ENTER | BTN_MENU:
        //all keys depressed, reboot to programming mode
        _reboot_Teensyduino_();
    }
  }
  return redraw;
}


//***********************************************************

static KeyState readInput(uint32_t timeNow) {

  static uint32_t lastDebounceTime = 0;         // the last time the output pin was toggled
  static uint32_t buttonRepeatTime = 0;
  static uint32_t buttonPressedTime = 0;
  static uint8_t lastDeumButtons = 0;
  static uint8_t deumButtonState = 0;

  KeyState keys = { deumButtonState, 0 };

  // read the state of the switches (note that they are active low, so we invert the values)
  uint8_t deumButtons = 0x0f ^(digitalRead(dPin) | (digitalRead(ePin) << 1) | (digitalRead(uPin) << 2) | (digitalRead(mPin)<<3));

  // check to see if you just pressed the button
  // (i.e. the input went from LOW to HIGH), and you've waited long enough
  // since the last press to ignore any noise:

  // If the switch changed, due to noise or pressing:
  if (deumButtons != lastDeumButtons) {
    // reset the debouncing timer
    lastDebounceTime = timeNow;
  }


  if ((timeNow - lastDebounceTime) > debounceDelay) {
    // whatever the reading is at, it's been there for longer than the debounce
    // delay, so take it as the actual current state:

    // if the button state has changed:
    if (deumButtons != deumButtonState) {
      keys.current = deumButtons;
      keys.changed = deumButtonState ^ deumButtons;

      deumButtonState = deumButtons;
      menuTime = timeNow;
      buttonPressedTime = timeNow;
    }

    if (((deumButtons == BTN_DOWN) || (deumButtons == BTN_UP)) && (timeNow - buttonPressedTime > buttonRepeatDelay) && (timeNow - buttonRepeatTime > buttonRepeatInterval)){
      buttonRepeatTime = timeNow;
      keys.changed = deumButtons; // Key repeat
    }
  }

  // save the reading. Next time through the loop, it'll be the lastButtonState:
  lastDeumButtons = deumButtons;

  return keys;
}

void menu() {
  unsigned long timeNow = millis();

  bool redraw = stateFirstRun;

  KeyState input = readInput(timeNow);

  // shut off menu system if not used for a while (changes not stored by exiting a setting manually will not be stored in EEPROM)
  if (menuState&& ((timeNow - menuTime) > menuTimeUp)) {
    menuState= DISPLAYOFF_IDL;
    stateFirstRun = 1;
    subVibSquelch = 0;
    memset(activeSub, 0, sizeof(activeSub));
  }

  if (menuState== DISPLAYOFF_IDL) {
    redraw |= updatePage((const MenuPage*)&idleMenuPage, input, timeNow);
  } else if (menuState== PATCH_VIEW) {
    redraw |= updatePage((const MenuPage*)&patchMenuPage, input, timeNow);
  } else if (menuState== MAIN_MENU) {
    redraw |= updatePage(&mainMenuPage, input, timeNow);
  } else if (menuState== ROTATOR_MENU) {
    redraw |= updatePage(&rotatorMenuPage, input, timeNow);
  } else if (menuState== ADJUST_MENU) {
    redraw |= updatePage((const MenuPage*)&adjustMenuPage, input, timeNow);
  } else if (menuState== SETUP_BR_MENU) {
    redraw |= updatePage(&breathMenuPage, input, timeNow);
  } else if (menuState== SETUP_CT_MENU) {
    redraw |= updatePage(&controlMenuPage, input, timeNow);
  } else if (menuState== VIBRATO_MENU) {
    redraw |= updatePage(&vibratoMenuPage, input, timeNow);
  } else if (menuState == ABOUT_MENU) {
    redraw |= updatePage((const MenuPage*)&aboutMenuPage, input, timeNow);
  } else if (menuState == EXTRAS_MENU) {
    redraw |= updatePage((const MenuPage*)&extrasMenuPage, input, timeNow);
  }

  if(redraw) {
    display.display();
  }
}