#include <Arduino.h>
#include <menu.h>
#include <menuIO/adafruitGfxOut.h>
#include <menuIO/chainStream.h>

#include "Adafruit_FreeTouch.h"

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

using namespace Menu;

#define PIN_LED 15
#define N_TOUCH 6
#define MAX_DEPTH 3

#define THRESHOLD 500
Adafruit_SSD1306 display(128, 64, &Wire, -1);

unsigned int speed = 400;

// HANDLE TOUCH INPUT -- CODE LARGELY WRITTEN BY QUENTIN
// -----------------------------------------------------
// Q touch peripherals
uint8_t touch_pins[N_TOUCH] = {2, 3, 4, 5, 6, 7};
Adafruit_FreeTouch* touch_devices[N_TOUCH];

// touch state
int touch_values[N_TOUCH] = {0, 0, 0, 0, 0, 0};
bool pin_touched_now[N_TOUCH] = {false, false, false, false, false, false};
bool pin_touched_past[N_TOUCH] = {false, false, false, false, false, false};

void update_touch() {
  Ptc *ptc = ((Ptc *)PTC);

  for (int i = 0; i < N_TOUCH; i++) {
    // start
    touch_devices[i]->begin();

    // read
    touch_values[i] = touch_devices[i]->measure();

    // reset
    ptc->CTRLA.bit.ENABLE = 0;
    ptc->CTRLA.bit.SWRESET = 1;

    // update the state
    pin_touched_past[i] = pin_touched_now[i];
    pin_touched_now[i] = touch_values[i] > THRESHOLD;
  }
}

void print_touch() {
  char print_buffer[30];
  for (int i=0; i < N_TOUCH; i++) {
    sprintf(print_buffer, "%4d ", touch_values[i]);
  }
}

// HANDLE INPUTS TO DRIVE MENU
// ---------------------------
void handle_commands(navRoot &nav) {
  if(pin_touched_now[2] && !pin_touched_past[2]) nav.doNav(upCmd);
  if(pin_touched_now[5] && !pin_touched_past[5]) nav.doNav(downCmd);
  if(pin_touched_now[1] && !pin_touched_past[1]) nav.doNav(enterCmd);
  if(pin_touched_now[0] && !pin_touched_past[0]) digitalWrite(PIN_LED, !digitalRead(PIN_LED)); // toggle just for tests.
}

// HANDLE MESSAGES
// ---------------

void showMessageHTMAA() {
  const char msg[] = "howtomakealmostanything";
  showMessage(msg);
}

void showMessageHelp() {
  const char msg[] = "helpme";
  showMessage(msg);
}

void showMessageLost() {
  const char msg[] = "iamlost";
  showMessage(msg);
}

void showMessage(const char msg[]) {
for (int i = 0; msg[i]; ++i) {
    Serial.print(msg[i]);
    delay(speed);
  } 
}

// MENU DEFINITION
// ---------------

// mono color table
const colorDef<uint16_t> colors[] MEMMODE = {
  {{WHITE,BLACK},{WHITE,BLACK,WHITE}},
  {{BLACK,WHITE},{BLACK,WHITE,BLACK}},
  {{BLACK,WHITE},{WHITE,BLACK,WHITE}},
  {{WHITE,BLACK},{WHITE,BLACK,BLACK}},
  {{WHITE,BLACK},{BLACK,WHITE,WHITE}},
  {{WHITE,BLACK},{WHITE,BLACK,BLACK}},
};

MENU(settingsMenu, "Settings", doNothing, noEvent, wrapStyle
  ,FIELD(speed ,"Speed" ,"ms",0,1000 ,10,1, doNothing,noEvent,noStyle)
  ,EXIT("<Back>")
);

template<char C> void pick(){ Serial.print(C); }
// Two menus needed because the menus have a limit of 16 entries.
MENU(lettersAM, "Letters A-M", doNothing, noEvent, wrapStyle
  ,OP("a", pick<'a'>, enterEvent)
  ,OP("b", pick<'b'>, enterEvent)
  ,OP("c", pick<'c'>, enterEvent)
  ,OP("d", pick<'d'>, enterEvent)
  ,OP("e", pick<'e'>, enterEvent)
  ,OP("f", pick<'f'>, enterEvent)
  ,OP("g", pick<'g'>, enterEvent)
  ,OP("h", pick<'h'>, enterEvent)
  ,OP("i", pick<'i'>, enterEvent)
  ,OP("j", pick<'j'>, enterEvent)
  ,OP("k", pick<'k'>, enterEvent)
  ,OP("l", pick<'l'>, enterEvent)
  ,OP("m", pick<'m'>, enterEvent)
  ,EXIT("<Back>")
);

MENU(lettersNZ, "Letters N-Z", doNothing, noEvent, wrapStyle
  ,OP("n", pick<'n'>, enterEvent)
  ,OP("o", pick<'o'>, enterEvent)
  ,OP("p", pick<'p'>, enterEvent)
  ,OP("q", pick<'q'>, enterEvent)
  ,OP("r", pick<'r'>, enterEvent)
  ,OP("s", pick<'s'>, enterEvent)
  ,OP("t", pick<'t'>, enterEvent)
  ,OP("u", pick<'u'>, enterEvent)
  ,OP("v", pick<'v'>, enterEvent)
  ,OP("w", pick<'w'>, enterEvent)
  ,OP("x", pick<'x'>, enterEvent)
  ,OP("y", pick<'y'>, enterEvent)
  ,OP("z", pick<'z'>, enterEvent)
  ,EXIT("<Back>")
);

MENU(messagesMenu, "Messages", doNothing, noEvent, wrapStyle
  ,OP("HTMAA", showMessageHTMAA, enterEvent)
  ,OP("Help Me", showMessageHelp, enterEvent)
  ,OP("I am lost", showMessageLost, enterEvent)
  ,EXIT("<Back>")
);

MENU(mainMenu, "Upside Down", Menu::doNothing, Menu::noEvent, Menu::wrapStyle
  ,SUBMENU(settingsMenu)
  ,SUBMENU(messagesMenu)
  ,SUBMENU(lettersAM)
  ,SUBMENU(lettersNZ)
  // ,EXIT("<Back")  // This is the main menu, I don't want the user to be able to exit it.
);

MENU_INPUTS(in);

// Output DEFINITION (using ADAGFX lib to drive screen)
MENU_OUTPUTS(out, MAX_DEPTH,
  ADAGFX_OUT(display, colors, 12, 14, {0,0,18,5}),
  NONE
);

NAVROOT(nav, mainMenu, MAX_DEPTH, in, out);

// END OF MENU DEFINITION
// ----------------------

void setup() {
  pinMode(PIN_LED, OUTPUT);
  Serial.begin(115200);

  Wire.begin();
  delay(500);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay(); 
  display.display();

  nav.refresh();

  // initialize capacitive touch peripherals
  for (int i = 0; i < N_TOUCH; i++) {
    touch_devices[i] = new Adafruit_FreeTouch(touch_pins[i], OVERSAMPLE_1, RESISTOR_100K, FREQ_MODE_NONE);
  }
}

void loop() {
    // update the touch sensors
  update_touch();
  handle_commands(nav);

  nav.poll();
  display.display();

  // slow down the loop to not print too fast
  // delay(50);
}