#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPR121.h>
#include <Arduino.h>
#include <array>
#include <cstdio>
#include <algorithm>

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

// constants
const unsigned long debounceDelay = 30;           // the debounce time; increase if the output flickers

static unsigned long menuTime = 0;
bool displayOn = true;

std::array<const char*, 128> CC_NAMES = {
    "Bank Select", // 0
    "Mod Wheel", // 1
    "Breath", // 2
    "Undefined", // 3
    "Foot Pedal", // 4
    "Port. Time", // 5
    "Data Entry", // 6
    "Volume", // 7
    "Balance", // 8
    "Undefined", // 9
    "Pan", // 10
    "Expression", // 11
    "Effect 1", // 12
    "Effect 2", // 13
    "Undefined", // 14
    "Undefined", // 15
    "GP 1", // 16
    "GP 2", // 17
    "GP 3", // 18
    "GP 3", // 19
    "Undefined", // 20
    "Undefined", // 21
    "Undefined", // 22
    "Undefined", // 23
    "Undefined", // 24
    "Undefined", // 25
    "Undefined", // 26
    "Undefined", // 27
    "Undefined", // 28
    "Undefined", // 29
    "Undefined", // 30
    "Undefined", // 31
    "LSB 0", // 32
    "LSB 1", // 33
    "LSB 2", // 34
    "LSB 3", // 35
    "LSB 4", // 36
    "LSB 5", // 37
    "LSB 6", // 38
    "LSB 7", // 39
    "LSB 8", // 40
    "LSB 9", // 41
    "LSB 10", // 42
    "LSB 11", // 43
    "LSB 12", // 44
    "LSB 13", // 45
    "LSB 14", // 46
    "LSB 15", // 47
    "LSB 16", // 48
    "LSB 17", // 49
    "LSB 18", // 50
    "LSB 19", // 51
    "LSB 20", // 52
    "LSB 21", // 53
    "LSB 22", // 54
    "LSB 23", // 55
    "LSB 24", // 56
    "LSB 25", // 57
    "LSB 26", // 58
    "LSB 27", // 59
    "LSB 28", // 60
    "LSB 29", // 61
    "LSB 30", // 62
    "LSB 31", // 63
    "Sustain", // 64
    "Portamento", // 65
    "Sostenuto", // 66
    "Soft Pedal", // 67
    "Legato", // 68
    "Hold 2", // 69
    "Variation", // 70
    "Resonance", // 71
    "Release", // 72
    "Attack", // 73
    "Cutoff", // 74
    "Sound 6", // 75
    "Sound 7", // 76
    "Sound 8", // 77
    "Sound 9", // 78
    "Sound 10", // 79
    "Decay", // 80
    "Hi Pass", // 81
    "GP Button 3", // 82
    "GP Button 4", // 83
    "Port. Amount", // 84
    "Undefined", // 85
    "Undefined", // 86
    "Undefined", // 87
    "Undefined", // 88
    "Undefined", // 89
    "Undefined", // 90
    "Reverb", // 91
    "Tremolo", // 92
    "Chorus", // 93
    "Detune", // 94
    "Phaser", // 95
    "Data Inc", // 96
    "Data Dec", // 97
    "NRPN LSB", // 98
    "NRPN MSB", // 99
    "RP LSB", // 100
    "RP MSB", // 101
    "Undefined", // 102
    "Undefined", // 103
    "Undefined", // 104
    "Undefined", // 105
    "Undefined", // 106
    "Undefined", // 107
    "Undefined", // 108
    "Undefined", // 109
    "Undefined", // 110
    "Undefined", // 111
    "Undefined", // 112
    "Undefined", // 113
    "Undefined", // 114
    "Undefined", // 115
    "Undefined", // 116
    "Undefined", // 117
    "Undefined", // 118
    "Undefined", // 119
    "All Sound Off", // 120
    "All CCC Off", // 121
    "Keyboard On", // 122
    "All Notes Off", // 123
    "Omni Mode Off", // 124
    "Omni Mode On", // 125
    "Mono", // 126
    "Poly Mode", // 127
};

// 'NuEVI' or 'NuRAD' 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
};

#define OLED_RESET 4
Adafruit_SSD1306 display(128, 64, &Wire1, OLED_RESET, 1000000, 1000000);
int16_t ctouchVal = 0;

MenuScreen* currentMenu = NULL;

// Track pixels for faster redrawing
struct AdjustDrawing {
  int row;
  int thrX;
  int maxX;
  int valX;
};

struct AdjustValue {
  const char* title;
  const int16_t instrument_state_t::* value;
  int16_t calibration_t::* thrVal;
  int16_t calibration_t::* maxVal;
  const int16_t limitLow;
  const int16_t limitHigh;

  // If not null, thr and max are relative to zeroPoint
  const int16_t instrument_state_t::* zeroPoint;
};

void autoCalSelected() {
}

static void drawIndicator(int x, int row, int color) {
  display.fillTriangle(x - 2, row + 1, x + 2, row + 1, x, row + 3, color);
  display.fillTriangle(x - 2, row + 10, x + 2, row + 10, x, row + 7, color);
}

static void drawAdjustIndicators(state_t& state, const AdjustValue& value, AdjustDrawing& drawing) {
  const int thrX = mapConstrain(state.calibration->*value.thrVal, value.limitLow, value.limitHigh, 1, 127);
  const int maxX = mapConstrain(state.calibration->*value.maxVal, value.limitLow, value.limitHigh, 1, 127);
  if (drawing.maxX != maxX) {
    drawIndicator(drawing.thrX, drawing.row, BLACK);
    drawIndicator(thrX, drawing.row, WHITE);
    drawing.maxX = maxX;
  }

  if (drawing.thrX != thrX) {
    drawIndicator(drawing.thrX, drawing.row, BLACK);
    drawIndicator(thrX, drawing.row, WHITE);
    drawing.thrX = thrX;
  }
}

static void drawAdjustTitle(const AdjustValue& value, AdjustDrawing& drawing, bool highlight) {
  display.setTextSize(1);
  if (highlight) {
    display.setTextColor(BLACK, WHITE);
  } else {
    display.setTextColor(WHITE, BLACK);
  }
  display.setCursor(0, drawing.row);
  display.println(value.title);
}

static void drawAdjustValues(state_t& state, const AdjustValue& value, AdjustDrawing& drawing) {
  char buffer[13];
  snprintf(buffer, 13, "%d>%d<%d", value.thrVal, value.value, value.maxVal);
  display.setTextSize(1);
  display.setCursor(128 - 6 * strlen(buffer), drawing.row);
  display.println(buffer);

  const int valX = mapConstrain(state.instrument->*value.value, value.limitLow, value.limitHigh, 1, 127);
  if (drawing.valX != valX) {
    display.drawFastVLine(drawing.valX, drawing.row + 4, 4, BLACK);
    display.drawFastVLine(valX, drawing.row + 4, 4, WHITE);
    drawing.valX = valX;
  }
}

void drawAdjustFrame(int line) {
  display.drawLine(25, line, 120, line, WHITE); // Top line
  display.drawLine(25, line + 12, 120, line + 12, WHITE); // Bottom line

  display.drawLine(25, line + 1, 25, line + 2, WHITE);
  display.drawLine(120, line + 1, 120, line + 2, WHITE);

  display.drawLine(120, line + 10, 120, line + 11, WHITE);
  display.drawLine(25, line + 10, 25, line + 11, WHITE);
}

void drawAdjustRow(state_t& state, const AdjustValue& value, AdjustDrawing& drawing, bool highlight) {
  display.fillRect(0, drawing.row, 128, 21, BLACK);
  drawAdjustFrame(drawing.row);
  drawAdjustTitle(value, drawing, highlight);
  drawAdjustValues(state, value, drawing);
  drawAdjustIndicators(state, value, drawing);
}

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

  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);
}

template<size_t N>
static void plotMenuEntries(std::array<MenuScreen const*, N> entries, size_t cursorPos, int xOffset = 0) {
  display.fillRect(0 + xOffset, MENU_HEADER_OFFSET, 63 + xOffset, 64 - MENU_HEADER_OFFSET, BLACK);
  display.setTextSize(1);

  int scrollPos = 0;
  if (entries.size() >= MENU_NUM_ROWS) {
    if ((cursorPos - scrollPos) > (MENU_NUM_ROWS - 2)) {
      scrollPos = cursorPos - (MENU_NUM_ROWS - 2);
    } else if ((cursorPos - scrollPos) < 1) {
      scrollPos = cursorPos - 1;
    }

    scrollPos = constrain(scrollPos, 0, entries.size() - MENU_NUM_ROWS);
  }

  size_t row = 0;
  size_t end = constrain(scrollPos + MENU_NUM_ROWS, 0, entries.size());
  for (size_t i = scrollPos; i < end; i++, row++) {
    int rowPixel = (row)*MENU_ROW_HEIGHT + MENU_HEADER_OFFSET;
    display.setCursor(xOffset, rowPixel);
    if (cursorPos == i) {
      display.setTextColor(BLACK, WHITE);
    } else {
      display.setTextColor(WHITE);
    }

    display.println(entries[i]->title());
  }
}

class AboutMenu : public MenuScreen {
  const char* title() {
    return "ABOUT";
  }

  bool update(state_t& state, InputState& input, bool redraw) {
    if (redraw) {
      display.fillRect(63, 11, 64, 52, BLACK);
      display.setTextColor(WHITE);
      display.setTextSize(0);
      display.setCursor(64, 12);
      display.print("xEVI");
      display.setCursor(64, 21);
      display.print("firmware");
      display.setCursor(74, 30);
      display.print("v");
      display.print(FIRMWARE_VERSION);
      display.setCursor(64, 39);
      display.print("eeprom");
      display.setCursor(74, 48);
      display.print("v");
      display.print(EEPROM_VERSION);
      return true;
    }
  }
};

template<size_t N>
class MainMenu : public MenuScreen {
public:
  MainMenu(std::array<MenuScreen const*, N> entries) : _entries(entries) {}
  bool update(state_t& state, InputState& input, bool redraw) {
    bool dirty = redraw;

    if (input.changed && input.knobMenu) {
      _cursorPos = (_cursorPos + input.knobMenu + _entries.size()) % _entries.size();
      input.changed = false;
      _entries[_cursorPos]->update(state, input, true);
      dirty = true;
      draw(redraw);
    } else {
      dirty |= _entries[_cursorPos]->update(state, input, redraw);
      if (redraw) {
        draw(redraw);
        dirty = true;
      }
    }

    return dirty;
  };

  const char* title() {
    return "MENU";
  }

private:
  void draw(bool redraw) {
    if (redraw) {
      Serial.println("main menu redraw");
      display.clearDisplay();
      display.setTextSize(1);
      display.setCursor(0, 0);
      display.drawLine(0, MENU_ROW_HEIGHT, 127, MENU_ROW_HEIGHT, WHITE);
      display.println("MENU");
    }

    plotMenuEntries(_entries, _cursorPos);

  }

  size_t _cursorPos = 0;
  std::array<MenuScreen const*, N> _entries;
};

template<size_t N>
class SubMenu : public MenuScreen {
public:
  SubMenu(const char* title, std::array<MenuScreen const*, N> entries) : _title(title), _entries(entries) {}
  bool update(state_t& state, InputState& input, bool redraw) {
    bool redrawValue = false;
    bool dirty = false;
    return false;
    if (input.changed && input.btnMenu) {
      _focused = !_focused;
      if (_focused) {
        lastMenu = currentMenu;
        currentMenu = this;
        redraw = true;
      } else {
        currentMenu = lastMenu;
        redraw = true;
      }
    }

    if (_focused && input.changed && input.knobMenu != 0) {
      _cursorPos = (_cursorPos + input.knobMenu) % _entries.size();
      draw(false);
      input.changed = false;
      redrawValue = true;
      dirty = true;
    }

    if (redraw) {
      draw(redraw);
      dirty = true;
    }

    if (_focused) {
      dirty |= _entries[_cursorPos]->update(state, input, redrawValue);
    }

    return dirty;
  };

  const char* title() {
    return _title;
  }

private:
  const char* _title;
  size_t _cursorPos;
  std::array<MenuScreen const*, N> _entries;
  bool _focused = false;
  MenuScreen* lastMenu;

  void draw(bool redraw) {
    if (redraw) {
      if (_focused) {
        display.clearDisplay();
      } else {
        display.fillRect(63, 11, 64, 52, BLACK);
      }
      display.setTextSize(1);
      display.drawLine(_focused ? 0 : 63, MENU_ROW_HEIGHT, 127, MENU_ROW_HEIGHT, WHITE);
      display.setCursor(_focused ? 0 : 63, 0);
      display.println(_title);
    }

    plotMenuEntries(_entries, _cursorPos, _focused ? 0 : 63);
  }
};

template<
  size_t L,
  typename T,
  typename U,
  typename U_p
>
class ValueMenu : public MenuScreen {
public:
  ValueMenu(
    const char* title,
    U_p state_t::* obj,
    T U::* value,
    const T min, const T max, const bool wrap = false,
    const std::array<const char*, L> labels = {}
  ) :
    _title(title),
    _obj(obj),
    _value(value),
    _min(min),
    _max(max),
    _wrap(wrap),
    _labels(labels) {
  }

  bool update(state_t& state, InputState& input, bool redraw) {
    if (input.knobVal1) {
      int32_t newVal = (state.*_obj)->*_value + input.knobVal1;
      if (newVal > _max) {
        if (_wrap) {
          newVal = _min;
        } else {
          newVal = _max;
        }
      } else if (newVal < _min) {
        if (_wrap) {
          newVal = _max;
        } else {
          newVal = _min;
        }
      }

      (state.*_obj)->*_value = newVal;

      draw(state, redraw);
      return true;
    } else if (redraw) {
      draw(state, redraw);
      return true;
    }

    return false;
  }
  const char* title() {
    return _title;
  }
private:
  const char* _title;
  U_p state_t::* _obj;
  T U::* _value;
  const T _min;
  const T _max;
  const bool _wrap;
  const std::array<const char*, L> _labels;

  void draw(state_t &state, bool redraw) {
    if (redraw) {
      display.fillRect(63, 11, 64, 52, BLACK);
      display.drawRect(63, 11, 64, 52, WHITE);
      display.setTextSize(1);
      int len = strlen(this->_title);

      display.setCursor(95 - len * 3, 15);
      display.println(this->_title);
    } else {
      display.fillRect(64, 33, 62, 29, BLACK);
    }

    char buffer[12];
    snprintf(buffer, 12, "%+d", (state.*_obj)->*_value);
    size_t label_idx = (state.*_obj)->*_value - _min;
    if (_labels.size() > 0 && label_idx >= 0 && label_idx < _labels.size()) {
      plotSubOption(_labels[label_idx], buffer);
    } else {
      plotSubOption(buffer);
    }
  }
};

template<
  size_t L,
  typename T
>
class PresetValueMenu : public ValueMenu<L, T, preset_t, preset_t*> {
  public:
  PresetValueMenu(
    const char* title,
    T preset_t::* value,
    const T min, const T max, const bool wrap = false,
    const std::array<const char*, L> labels = {}
  ) : ValueMenu<L, T, preset_t, preset_t*>(title, &state_t::currentPreset, value, min, max, wrap, labels) {}
};

template<
  size_t L,
  typename T
>
class StateValueMenu : public ValueMenu<L, T, instrument_state_t, instrument_state_t*> {
  public:
  StateValueMenu(
    const char* title,
    T instrument_state_t::* value,
    const T min, const T max, const bool wrap = false,
    const std::array<const char*, L> labels = {}
  ) : ValueMenu<L, T, instrument_state_t, instrument_state_t*>(title, &state_t::instrument, value, min, max, wrap, labels) {}
};

template<
  size_t N,
  typename T
>
class ChoiceMenu : public MenuScreen {
public:
  ChoiceMenu(
    const char* title, T preset_t::* value,
    std::array<T, N> choices,
    const std::array<const char*, N> labels = {}
  ) : _title(title), _value(value), _choices(choices), _labels(labels) {
  }
  bool update(state_t& state, InputState& input, bool redraw) {
    auto it = std::find(_choices.begin(), _choices.end(), state.currentPreset->*_value);
    if (input.knobVal1) {
      it += input.knobVal1;
      if (it >= _choices.end()) {
        state.currentPreset->*_value = _choices.front();
        it = _choices.begin();
      } else if (it < _choices.begin()) {
        state.currentPreset->*_value = _choices.back();
        it = _choices.end();
      } else {
        state.currentPreset->*_value = _choices[it];
      }

      draw(redraw, it - _choices.begin());
      return true;
    } else if (redraw) {
      draw(redraw, it - _choices.begin());
      return true;
    }

    return false;
  }
  const char* title() {
    return _title;
  }
private:
  const char* _title;
  T preset_t::*_value;
  const std::array<T, N> _choices;
  const std::array<const char*, N> _labels;

  void draw(bool redraw, size_t label_idx) {
    if (redraw) {
      display.fillRect(63, 11, 64, 52, BLACK);
      display.drawRect(63, 11, 64, 52, WHITE);
      display.setTextSize(1);
      int len = strlen(this->_title);

      display.setCursor(95 - len * 3, 15);
      display.println(this->_title);
    }

    char buffer[12];
    snprintf(buffer, 12, "%+d", _value);
    if (_labels.size() > 0 && label_idx >= 0 && label_idx <= _labels.size()) {
      plotSubOption(_labels[label_idx], buffer);
    } else {
      plotSubOption(buffer);
    }
  }
};

class StatusMenu : public MenuScreen {
public:
  StatusMenu(MenuScreen& mainMenu) : _mainMenu(mainMenu) {
    Serial.println("init statusmenu");
  }
  bool update(state_t& state, InputState& input, bool redraw) {
    // TODO: handle cc/patch/preset changes
    Serial.println("status update");
    if (input.changed && input.btnMenu) {
      Serial.println("menu");
      currentMenu = &_mainMenu;
      input.changed = false;
      return currentMenu->update(state, input, true);
    }

    return false;
  };
private:
  MenuScreen& _mainMenu;
};

class ExitMenu : public MenuScreen {
public:
  ExitMenu() {}
  bool update(state_t& state, InputState& input, bool redraw) {
    if (redraw) {
      display.fillRect(63, 11, 64, 52, BLACK);
    }

    if (input.changed && input.btnMenu) {
      displayOff();
    }

    return false;
  }
  const char* title() {
    return "EXIT";
  }
};

template<size_t N>
class AdjustMenuScreen : public MenuScreen {
public:
  AdjustMenuScreen(const char* title, std::array<AdjustValue, N> entries) : _title(title), _entries(entries) {}
  bool update(state_t& state, InputState& input, bool redraw) {
    if (input.changed && input.btnMenu) {
      _focused = !_focused;
    }

    if (!_focused) {
      display.fillRect(63, 11, 64, 52, BLACK);
      return true;
    }

    bool redrawIndicators = false;
    if (input.changed) {
      if (input.knobMenu) {
        _selectedEntry = _selectedEntry + input.knobMenu;
        redraw = true;
      }

      AdjustValue value = _entries[_selectedEntry];
      if (input.knobVal1) {
        state.calibration->*value.thrVal += input.knobVal1;
        state.calibration->*value.thrVal = constrain(state.calibration->*value.thrVal, value.limitLow, value.limitHigh);
        redrawIndicators = true;
      }

      if (input.knobVal2) {
        state.calibration->*value.maxVal += input.knobVal2;
        state.calibration->*value.maxVal = constrain(state.calibration->*value.maxVal, value.limitLow, value.limitHigh);
        redrawIndicators = true;
      }
    } else {
      draw(state, redrawIndicators, redraw);
    }

    return true;
  }
  const char* title() {
    return _title;
  }
private:
  const char* _title;
  size_t _selectedEntry = 0;
  std::array<AdjustValue, N> _entries;
  std::array<AdjustDrawing, N> _rowDrawings;
  bool _focused = false;

  void draw(state_t& state, bool redrawIndicators, bool redraw) {
    size_t scrollPos = 0;
    if (_entries.size() >= ADJUST_NUM_ROWS) {
      if ((_selectedEntry - scrollPos) > (ADJUST_NUM_ROWS - 2)) {
        scrollPos = _selectedEntry - (ADJUST_NUM_ROWS - 2);
      } else if ((_selectedEntry - scrollPos) < 1) {
        scrollPos = _selectedEntry - 1;
      }

      scrollPos = constrain(scrollPos, 0, _entries.size() - ADJUST_NUM_ROWS);
    }

    int end = constrain(scrollPos + ADJUST_NUM_ROWS, 0, N);
    for (size_t i = scrollPos; i < end; i++) {
      if (redraw) {
        drawAdjustRow(state, _entries[i], _rowDrawings[i], i == _selectedEntry);
      } else if (redrawIndicators && i == _selectedEntry) {
        drawAdjustIndicators(state, _entries[i], _rowDrawings[i]);
      } else {
        drawAdjustValues(state, _entries[i], _rowDrawings[i]);
      }
    }
  }
};

std::array<AdjustValue, 8> adjustValues = { {
  {"BREATH", &instrument_state_t::breathSignal, &calibration_t::breathThrValOffset, &calibration_t::breathMaxValOffset,
    BREATH_LO_LIMIT, BREATH_HI_LIMIT, &instrument_state_t::breathZero},
  {"BR ALT", &instrument_state_t::breathAltSignal, &calibration_t::breathAltThrValOffset, &calibration_t::breathAltMaxValOffset,
    BREATH_LO_LIMIT, BREATH_HI_LIMIT, &instrument_state_t::breathAltZero},
  {"BITE",&instrument_state_t::biteSignal, &calibration_t::biteThrVal, &calibration_t::biteMaxVal, BITE_LO_LIMIT, BITE_HI_LIMIT, NULL},
  {"PB DOWN",&instrument_state_t::pbDnSignal, &calibration_t::pbDnThrVal, &calibration_t::pbDnMaxVal, PITCHB_LO_LIMIT, PITCHB_HI_LIMIT, NULL},
  {"PB UP", &instrument_state_t::pbUpSignal, &calibration_t::pbUpThrVal, &calibration_t::pbUpMaxVal, PITCHB_LO_LIMIT, PITCHB_HI_LIMIT, NULL},
  {"EXTRA", &instrument_state_t::extraSignal, &calibration_t::extraThrVal, &calibration_t::extraMaxVal, EXTRA_LO_LIMIT, EXTRA_HI_LIMIT, NULL},
  {"LEVER", &instrument_state_t::leverSignal, &calibration_t::leverThrVal, &calibration_t::leverMaxVal, LEVER_LO_LIMIT, LEVER_HI_LIMIT, NULL},
  {"TOUCH", &instrument_state_t::avgCTouchSignal, &calibration_t::ctouchThrVal, &calibration_t::ctouchThrVal, CTOUCH_LO_LIMIT, CTOUCH_HI_LIMIT, NULL},
}};

const AdjustMenuScreen<8> adjustMenu("ADJUST", adjustValues);


//***********************************************************
const ChoiceMenu<4, BreathMode> breathModeMenu("BREATH MODE", &preset_t::breathMode, { { BREATH_STD, BREATH_LSB, BREATH_AT, BREATH_LSB_AT} }, { { "STANDARD", "LSB", "AT", "LSB_AT" } });
const PresetValueMenu<128, uint8_t> breathCCMenu("BREATH CC", &preset_t::breathCC, 0, 127, true, CC_NAMES);
const PresetValueMenu<1, uint8_t> velocityMenu("VELOCITY", &preset_t::fixedVelocity, 0, 127, true, { { "DYN" } });
const PresetValueMenu<0, uint8_t> curveMenu("CURVE", &preset_t::breathCurve, 0, 12); // TODO: curve display
const PresetValueMenu<1, uint8_t> velSmpDlMenu("VEL DELAY", &preset_t::velSmpDl, 0, 30, true, { "OFF" }); // TODO: unit ms
const PresetValueMenu<1, uint8_t> velBiasMenu("VEL BOOST", &preset_t::velBias, 0, 30, true, { "OFF" });
const PresetValueMenu<0, uint8_t> breathIntervalMenu("BR INTERV", &preset_t::breathInterval, 0, 30, true);
const PresetValueMenu<0, int8_t> trill3Menu("TRILL3", &preset_t::trill3_interval, 3, 4, true, {});
const PresetValueMenu<0, int8_t> cvTuneMenu("CV Tune", &preset_t::cvTune, -100, 100, false, {});
const PresetValueMenu<0, uint8_t> cvVibMenu("CV Vib LFO", &preset_t::cvVibRate, 0, 8, false, {});
const PresetValueMenu<0, int8_t> cvScaleMenu("CV SCALING", &preset_t::cvScale, -100, 100, false, {});
const std::array<const char*, 25> transposeLabels = {
    "C>", "C#>", "D>", "D#>", "E>", "F>", "F#>", "G>", "G#>", "A>", "Bb>", "B>",
    ">C<", "<C#", "<D", "<D#", "<E", "<F", "<F#", "<G", "<G#", "<A", "<Bb", "<B", "<C"
};

const StateValueMenu<25, int8_t> transposeMenu("TRANSPOSE", &instrument_state_t::transpose, -12, 12, true, transposeLabels);
const StateValueMenu<0, int8_t> octaveMenu("OCTAVE", &instrument_state_t::octave, -3, 3, true, {});
const PresetValueMenu<0, byte> midiMenu("MIDI CH", &preset_t::MIDIchannel, 1, 16, false, {});
const PresetValueMenu<1, uint8_t> vibDepthMenu("DEPTH", &preset_t::vibratoDepth, 0, 9, true, { "OFF" });
const PresetValueMenu<1, uint8_t> vibRetnMenu("RETURN", &preset_t::vibRetn, 0, 4, true, { "OFF" });
const PresetValueMenu<0, uint8_t> vibSenseMenu("SENSE LVR", &preset_t::vibSens, 0, 12);
const PresetValueMenu<0, uint8_t> vibSquelchMenu("SQUELCH LVR", &preset_t::vibSquelch, 0, 12);
const ChoiceMenu<2, VibratoMode> vibDirMenu("DIRECTION", &preset_t::vibratoMode, { {VSTART_DOWN, VSTART_UP} }, { "START DOWN", "START UP" });
const ChoiceMenu<4, ExtraControl> biteCtlMenu("BITE CTL", &preset_t::biteControl, { {OFF, VIBRATO, GLIDE, CC} }, { {
  "OFF",
  "VIBRATO",
  "GLIDE",
  "CC"
} });
const PresetValueMenu<128, uint8_t> biteCCMenu("BITE CC", &preset_t::biteCC, 0, 127, true, CC_NAMES);
const ChoiceMenu<4, ExtraControl> leverCtlMenu("LEVER CTL", &preset_t::leverControl, { {OFF, VIBRATO, GLIDE, CC} }, {
  "OFF",
  "VIBRATO",
  "GLIDE",
  "CC"
  });
const PresetValueMenu<128, uint8_t> leverCCMenu("LEVER CC", &preset_t::leverCC, 0, 127, true, CC_NAMES);
const ChoiceMenu<4, PortamentoMode> portMenu("GLIDE MOD", &preset_t::portamentoMode, { {POFF, PON, PSWITCH_ONLY, PGLIDE_ONLY} }, {
  "OFF",
  "ON",
  "SWITCH_ONLY",
  "GLIDE_ONLY",
  });
const PresetValueMenu<0, uint8_t> portLimitMenu("GLIDE LMT", &preset_t::portamentoLimit, 1, 127, true);
const PresetValueMenu<0, uint8_t> pitchBendMenu("PITCHBEND", &preset_t::PBdepth, 0, 12, true);
const ChoiceMenu<4, ExtraControl> extraCtlMenu("EXCT CC A", &preset_t::extraControl, { {OFF, VIBRATO, GLIDE, CC} }, {
  "OFF",
  "ON",
  "SWITCH_ONLY",
  "GLIDE_ONLY",
  });
const PresetValueMenu<128, uint8_t> extraCCMenu("EXCT CC", &preset_t::extraCC, 0, 4, true, CC_NAMES);
const PresetValueMenu<1, uint8_t> deglitchMenu("DEGLITCH", &preset_t::deglitch, 0, 70, true, { "OFF" });
const PresetValueMenu<29, uint8_t> pinkyMenu("PINKY KEY", &preset_t::pinkySetting, 0, 29, true, {
  "-12", "-11", "-10", "-9", "-8", "-7", "-6", "-5", "-4", "-3", "-2", "-1",
  "PB/2",
  "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "12",
  "PBD", "GLD", "MOD", "QTN"
  });
const ChoiceMenu<4, FingeringMode> fingeringMenu("FINGERING", &preset_t::fingering, { EVI, EVR, TPT, HRN }, {
    "EVI",
    "EVR",
    "TPT",
    "HRN",
  });
const ChoiceMenu<6, RollerMode> rollerMenu("ROLLRMODE", &preset_t::rollerMode, {
  HIGHEST,
  HIGHEST_EXTEND,
  HIGHEST_PAIR,
  HIGHEST_PAIR_EXTEND,
  PARTIAL,
  PARTIAL_EXTEND
  }, {
    "HIGHEST",
    "HIGHEST_EXTEND",
    "HIGHEST_PAIR",
    "HIGHEST_PAIR_EXTEND",
    "PARTIAL",
    "PARTIAL_EXTEND",
  });

std::array<MenuScreen const*, 7> breathMenuEntries = {
  &breathModeMenu,
  &breathCCMenu,
  &velocityMenu,
  &curveMenu,
  &velSmpDlMenu,
  &velBiasMenu,
  &breathIntervalMenu,
};
const SubMenu<7> breathMenu("BR SETUP", breathMenuEntries);

const std::array<MenuScreen const*, 13> controlMenuEntries = {
  &fingeringMenu,
  &rollerMenu,
  &biteCtlMenu,
  &biteCCMenu,
  &leverCtlMenu,
  &leverCCMenu,
  &extraCtlMenu,
  &extraCCMenu,
  &portMenu,
  &portLimitMenu,
  &deglitchMenu,
  &pinkyMenu,
  &pitchBendMenu
};
const SubMenu<13> controlMenu("CTL SETUP", controlMenuEntries);

const std::array<MenuScreen const*, 5> vibratoMenuEntries = {
    &vibDepthMenu,
    &vibRetnMenu,
    &vibDirMenu,
    &vibSenseMenu,
    &vibSquelchMenu,
};
const SubMenu<5> vibratoMenu("VIBRATO", vibratoMenuEntries);


const AboutMenu aboutMenu = AboutMenu();

std::array <MenuScreen const*, 4> extrasMenuEntries = {
  &trill3Menu,
  &cvTuneMenu,
  &cvScaleMenu,
  &cvVibMenu,
};
const SubMenu<4> extrasMenu("EXTRAS", extrasMenuEntries);

const ExitMenu exitMenu = ExitMenu();

// Top-level screens
const std::array<MenuScreen const*, 10> mainMenuEntries = {
  &transposeMenu,
  &octaveMenu,
  &midiMenu,
  &breathMenu,
  &controlMenu,
  &vibratoMenu,
  &adjustMenu,
  &extrasMenu,
  &aboutMenu,
  &exitMenu,
};
MainMenu<10> mainMenu(mainMenuEntries);
StatusMenu statusMenu = StatusMenu(mainMenu);

/*
static void curveCustomDraw() {
  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[currentPreset->breathCurve], 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[currentPreset->breathCurve]);
}
*/


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

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

static InputState readInput(uint32_t timeNow) {

  static uint32_t lastDebounceTime = 0;         // the last time the output pin was toggled
  static uint8_t lastDeumButtons = 0;
  static uint8_t deumButtonState = 0;
  static int lastKnobs[] = { 0, 0, 0, 0 };

  InputState input;

  uint8_t deumButtons = buttonState();

  // Debounce buttons
  if (deumButtons != lastDeumButtons) {
    lastDebounceTime = timeNow;
  }

  if ((timeNow - lastDebounceTime) > debounceDelay) {
    if (deumButtons != deumButtonState) {
      input.btnMenu = deumButtons & BTN_MENU;
      input.btnVal1 = deumButtons & BTN_VAL1;
      input.btnVal2 = deumButtons & BTN_VAL2;
      input.btnPreset = deumButtons & BTN_PRESET;
      input.changed = true;

      deumButtonState = deumButtons;
      menuTime = timeNow;
    }
  }

  for (int i = 0; i < 4; i++) {
    int val = readKnob(i);
    if (val != lastKnobs[i]) {
      input.changed = true;
      switch (i) {
      case 0:
        input.knobMenu = val;
        break;
      case 1:
        input.knobVal1 = val;
        break;
      case 2:
        input.knobVal2 = val;
        break;
      case 3:
        input.knobPreset = val;
        break;
      }
      menuTime = timeNow;
    }
  }

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

  return input;
}

void displayOff() {
  display.clearDisplay();
  display.display();
  display.ssd1306_command(SSD1306_DISPLAYOFF);
  currentMenu = &statusMenu;
  displayOn = false;
}

void displayError(const char* error) {
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println(error);
  display.display();
  Serial.print("ERROR: ");
  Serial.println(error);
}

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

void initDisplay() {
  // by default, we'll generate the high voltage from the 3.3v line internally! (neat!)
  display.begin(SSD1306_SWITCHCAPVCC, 0x3D);  // 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();
  currentMenu = &statusMenu;
}

void handleMenu(state_t& state, bool draw) {
  unsigned long timeNow = millis();

  InputState input = readInput(timeNow);

  // shut off menu system if not used for a while
  if (displayOn && ((timeNow - menuTime) > MENU_AUTO_OFF_TIME)) {
    displayOff();
  }

  if (input.changed) {
    display.ssd1306_command(SSD1306_DISPLAYON);
    displayOn = true;
  }

  if (currentMenu && (draw || input.changed)) {
    if (currentMenu->update(state, input, false)) {
      display.display();
    }
  }
}