#include <Adafruit_ILI9341.h>
#include <TaskScheduler.h>
#include <ArduinoJson.h>

// Keypad
#define PIN_UP 2
#define PIN_RIGHT 3
#define PIN_DOWN 18
#define PIN_LEFT 19
#define PIN_SEL 20

// TFT LCD Display
#define PIN_TFT_CS 53
#define PIN_TFT_DC 47
#define PIN_TFT_RST 45

Adafruit_ILI9341 tft = Adafruit_ILI9341(PIN_TFT_CS, PIN_TFT_DC, PIN_TFT_RST);

Scheduler runner;
// Scheduler & Task setup.
void screenUpdateCallback();
Task screenUpdateTask(50, TASK_FOREVER, &screenUpdateCallback);
void inputDeviceReadCallback();
Task inputDeviceReadTask(100, TASK_FOREVER, &inputDeviceReadCallback);
void uiProcessCallback();
Task uiProcessTask(50, TASK_FOREVER, &uiProcessCallback);
void commChannelProcessCallback();
Task commChannelProcessTask(50, TASK_FOREVER, &commChannelProcessCallback);

String rfidReader1CardId = "_RFID1_";
String rfidReader2CardId = "_RFID2_";
float loadCellReadingLb = 5.99;
float loadCellFillTarget = 50.0;

/////////////////
// Actual Code //
/////////////////

// UI
#define UI_INPUT_NULL     0
#define UI_INPUT_UP       1
#define UI_INPUT_DOWN     2
#define UI_INPUT_LEFT     3
#define UI_INPUT_RIGHT    4
#define UI_INPUT_SEL      5
#define UI_INPUT_SIZE     6
volatile int uiInput = UI_INPUT_NULL;

#define UI_LINE_HEIGHT 20
#define UI_LINE_CHAR_SIZE 20

#define UI_MENU_AUTO_REFRESH_CYCLE 500

// Pages
#define PAGE_MAIN_MENU            0
#define PAGE_DEVICE_INFO          1
#define PAGE_CREATE_PALLET        2
#define PAGE_CREATE_BUCKET        3
#define PAGE_FILL_BUCKET          4
#define PAGE_CREATE_ROAST         5
#define PAGE_INPUT_ROAST          6
#define PAGE_START_ROAST          7
#define PAGE_END_ROAST            8
#define PAGE_OUTPUT_ROAST         9
#define PAGE_CREATE_LOT           10
#define PAGE_FILL_LOT             11
#define PAGE_SIZE                 12

int cursorOn = 0;
int pageOn = PAGE_MAIN_MENU;
bool screenToRefresh = false;
bool screenToPartialRefresh = false;
long screenToPartialRefreshTimeout = 0;
bool screenToClear = false;

#define CALLBACK_NOOP nullptr
#define CALLBACK_GOTO_PAGE(page) [](){ gotoPage(page); }

#define ARRAY_LEN(arr) (sizeof(arr) / sizeof(*arr))

// Communications
// Connected to Arduino Cloud
bool connected = false;
// Connected to MQTT
bool online = false;
String deviceStatus = "OFFLINE";

// Typically declared by Arduino Cloud
String channelUp = "";
// Typically declared by Arduino Cloud
String channelDown = "";
String channelReq = "";
String channelResp = "";
bool channelDownReady = false;
bool channelReqSent = false;
bool channelRespReady = false;

#define DEVICE_MODE_IOT 0
#define DEVICE_MODE_LIZARD 1
String deviceModes[] = {"IoT", "Lizard"};
int deviceMode = DEVICE_MODE_IOT;


////////////////////
// Helper Classes //
////////////////////

class MenuItem {
    using CallbackFn = void (*)();
  public:
    String name;
    // Optional field name for JSON serialization.
    String fieldName;
    // Stores enumerable values for this item.
    String *values = nullptr;
    // Size of @values array.
    size_t valuesSize = 0;
    size_t selectedIdx = 0;
    CallbackFn onSelectCallback = nullptr;

    MenuItem() {}

    // Update entire values array.
    void setValues(String* values_, size_t size) {
      if (values) {
        delete[] values;
      }
      values = new String[size];
      for (int i = 0; i < size; i++) {
        values[i] = values_[i];
      }
      valuesSize = size;
    }

    // Update a single value at position. Index must be within the bounds
    // of existing values array.
    void updateValue(const String& value, size_t index) {
      values[index] = value;
    }

    void select(size_t index) {
      selectedIdx = (index < valuesSize) ? index : 0;
    }

    bool hasValue() const {
      return bool(values);
    }

    String value() const {
      if (!hasValue()) {
        return "";
      }
      return values[selectedIdx];
    }

    String getPadding(size_t strLength) const {
      String padding = "";
      for (size_t i = strLength; i < UI_LINE_CHAR_SIZE; i++) {
        padding += " ";
      }
      return padding;
    }

    // Gets the first line. Never indent value.
    String titleLine() const {
      return name + getPadding(name.length());
    }

    // Gets the second line, with indent and padding.
    String valueLine() const {
      if (!hasValue()) {
        return "";
      }
      String line = "  " + value();
      return line + getPadding(line.length());
    }

    void onLeft() {
      if (hasValue()) {
        selectedIdx = (selectedIdx - 1 + valuesSize) % valuesSize;
      }
    }

    void onRight() {
      if (hasValue()) {
        selectedIdx = (selectedIdx + 1) % valuesSize;
      }
    }

    void setOnSelectCallback(CallbackFn fn) {
      onSelectCallback = fn;
    }

    void onSelect() {
      if (onSelectCallback) {
        onSelectCallback();
      }
    }

    ~MenuItem() {
      delete[] values;
    }
};

class Menu {
    using CallbackFn = void (*)();
  public:
    String name = "";
    MenuItem *items = nullptr;
    size_t itemsSize = 0;
    size_t itemsCap = 0;
    size_t cursorIdx = 0;
    CallbackFn awaitResponseCallback;
    bool awaitResponse = false;
    CallbackFn refreshAssetsCallback;
    bool autoRefresh = false;

    Menu(String name_, size_t size) {
      name = name_;
      items = new MenuItem[size];
      itemsCap = size;
    }

    ~Menu() {}

    MenuItem* add(
      const String& name,
      CallbackFn onSelectCallback = nullptr,
      const String& fieldName = "",
      String* values = nullptr,
      size_t valuesSize = 0,
      size_t selectedIdx = 0) {
      if (itemsSize >= itemsCap) {
        return nullptr;
      }
      MenuItem* item = &items[itemsSize++];
      item->name = name;
      item->fieldName = fieldName;
      item->setOnSelectCallback(onSelectCallback);
      if (values) {
        item->setValues(values, valuesSize);
      }
      return item;
    }

    size_t length() const {
      return itemsSize;
    }

    bool cursorOn(size_t index) const {
      return index == cursorIdx;
    }

    void onUp() {
      cursorIdx = (cursorIdx - 1 + itemsSize) % itemsSize;
    }

    void onDown() {
      cursorIdx = (cursorIdx + 1) % itemsSize;
    }

    void onLeft() {
      items[cursorIdx].onLeft();
    }

    void onRight() {
      items[cursorIdx].onRight();
    }

    void onSelect() {
      items[cursorIdx].onSelect();
    }

    void setAwaitResponseCallback(CallbackFn fn) {
      awaitResponseCallback = fn;
    }

    void setAwaitResponse() {
      awaitResponse = true;
    }

    void onAwaitResponse() {
      if (awaitResponse && awaitResponseCallback) {
        awaitResponseCallback();
      }
    }

    void setRefreshAssetsCallback(CallbackFn fn) {
      refreshAssetsCallback = fn;
    }

    void setAutoRefresh(bool autoRefresh_ = true) {
      autoRefresh = autoRefresh_;
    }

    void onRefreshAssets() {
      if (refreshAssetsCallback) {
        refreshAssetsCallback();
      }
    }
};

////////////////////////
// UI Functionalities //
////////////////////////

// All menus in the app. The index of each menu must correspond
// to PAGE_XXX. Note: the size parameter must be same or greater than
// the number of items on the menu.
Menu menus[] = {
  {"main", 11},           /* PAGE_MAIN_MENU = 0 */
  {"pageDeviceInfo", 10}, /* PAGE_DEVICE_INFO = 1 */
  {"createPallet", 8},    /* PAGE_CREATE_PALLET = 2 */
  {"createBucket", 6},    /* PAGE_CREATE_BUCKET = 3 */
  {"fillBucket", 9},      /* PAGE_FILL_BUCKET = 4 */
  {"createRoast", 5},     /* PAGE_CREATE_ROAST = 5 */
  {"inputRoast", 6},      /* PAGE_INPUT_ROAST = 6 */
  {"startRoast", 4},      /* PAGE_START_ROAST = 7 */
  {"endRoast", 4},        /* PAGE_END_ROAST = 8 */
  {"outputRoast", 6},     /* PAGE_OUTPUT_ROAST = 9 */
  {"createLot", 6},       /* PAGE_CREATE_LOT = 10 */
  {"fillLot", 6},         /* PAGE_FILL_LOT = 11 */
};
Menu* pageOnMenu;

void onCmdSend() {
  DynamicJsonDocument cmdSpec(256);
  JsonObject root = cmdSpec.to<JsonObject>();
  root["cmd"] = pageOnMenu->name;
  root["ts"] = millis();
  JsonObject dataNode = root.createNestedObject("data");
  for (int i = 0; i < pageOnMenu->itemsSize; i++) {
    MenuItem* item = &pageOnMenu->items[i];
    if (item->fieldName.length()) {
      dataNode[item->fieldName] = item->value();
    }
  }
  String cmd;
  serializeJson(cmdSpec, cmd);
  enqueueCommRequest(cmd, true);
  pageOnMenu->awaitResponse = true;
  screenToRefresh = true;
}

void onBucketFillStart() {
  Serial.println("TO IMPLEMENT: Bucket fill start");
}

void onBucketFillStop() {
  Serial.println("TO IMPLEMENT: Bucket fill stopped");
}

void onSaveDeviceSettings() {
  Menu* m = &menus[PAGE_DEVICE_INFO];
  if (m->items[1].value() == "IoT") {
    deviceMode = DEVICE_MODE_IOT;
  } else {
    deviceMode = DEVICE_MODE_LIZARD;
  }
  loadCellFillTarget = m->items[5].value().toFloat();
}

void loadAssets() {
  // Actually fetch asset from web, with some defaults.

  // Create menu pages.
  Menu* m = &menus[PAGE_MAIN_MENU];
  m->add("0- Show Device Info", CALLBACK_GOTO_PAGE(PAGE_DEVICE_INFO));
  m->add("1- Add New Pallet",   CALLBACK_GOTO_PAGE(PAGE_CREATE_PALLET));
  m->add("2- Add New Bucket",   CALLBACK_GOTO_PAGE(PAGE_CREATE_BUCKET));
  m->add("3- Fill Bucket",      CALLBACK_GOTO_PAGE(PAGE_FILL_BUCKET));
  m->add("4- Add New Roast",    CALLBACK_GOTO_PAGE(PAGE_CREATE_ROAST));
  m->add("5- Load Roast",       CALLBACK_GOTO_PAGE(PAGE_INPUT_ROAST));
  m->add("6- Start Roast",      CALLBACK_GOTO_PAGE(PAGE_START_ROAST));
  m->add("7- Stop Roast",       CALLBACK_GOTO_PAGE(PAGE_END_ROAST));
  m->add("8- Unload Roast",     CALLBACK_GOTO_PAGE(PAGE_OUTPUT_ROAST));
  m->add("9- Add New Lot",      CALLBACK_GOTO_PAGE(PAGE_CREATE_LOT));
  m->add("10- Fill A Lot",      CALLBACK_GOTO_PAGE(PAGE_FILL_LOT));

  {
    // Menuge pageDevice -- the device settings page.
    // On device assets
    // device status
    String deviceStatuses[] = {deviceStatus};
    String rfidReader1CardIds[] = {rfidReader1CardId};
    String rfidReader2CardIds[] = {rfidReader2CardId};
    String loadCellReadingLbs[] = {String(loadCellReadingLb, 1)};
    String fillTargets[] = {"50", "33", "5"};
    m = &menus[PAGE_DEVICE_INFO];
    m->add("Device Status", CALLBACK_NOOP, "device_status", deviceStatuses, ARRAY_LEN(deviceStatuses));
    m->add("Device Mode", CALLBACK_NOOP, "device_mode", deviceModes, ARRAY_LEN(deviceModes));
    m->add("RFID Reader 1", CALLBACK_NOOP, "rfid1", rfidReader1CardIds, ARRAY_LEN(rfidReader1CardIds));
    m->add("RFID Reader 2", CALLBACK_NOOP, "rfid2", rfidReader2CardIds, ARRAY_LEN(rfidReader2CardIds));
    m->add("Scale (lb)", CALLBACK_NOOP, "weight", loadCellReadingLbs, ARRAY_LEN(loadCellReadingLbs));
    m->add("Fill Target (lb)", CALLBACK_NOOP, "fill_target", fillTargets, ARRAY_LEN(fillTargets));
    m->add("");
    m->add("TARE", []() {
      Serial.println("Taring Load Cell");
    });
    m->add("SAVE", onSaveDeviceSettings);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setRefreshAssetsCallback([]() {
      Menu* m = &menus[PAGE_DEVICE_INFO];
      m->items[0].updateValue(deviceStatus, 0);
      m->items[0].select(deviceMode);
      m->items[2].updateValue(rfidReader1CardId, 0);
      m->items[3].updateValue(rfidReader2CardId, 0);
      m->items[4].updateValue(String(loadCellReadingLb, 1), 0);
    });
    m->setAutoRefresh();
  }

  {
    // Menu createPallet
    // Online assets
    String palletIds[] = {"P00001", "P00002", "P00003"};
    size_t palletIdsSize = 3;
    String origins[] = {"NIC", "HON", "COL", "GUAT"};
    size_t originsSize = 4;
    String vendors[] = {"Royal Coffee"};
    size_t vendorsSize = 1;
    // On device assets
    String pcards[] = {"PCARD001"};
    size_t pcardsSize = 1;
    String received[] = {"2022-08-13", "2022-08-14", "2022-08-15"};
    size_t receivedSize = 3;
    m = &menus[PAGE_CREATE_PALLET];
    m->add("Pallet", CALLBACK_NOOP, "pallet_id", palletIds, palletIdsSize);
    m->add("Origin", CALLBACK_NOOP, "origin", origins, originsSize);
    m->add("Vendor", CALLBACK_NOOP, "vendor", vendors, vendorsSize);
    m->add("Received", CALLBACK_NOOP, "received", received, receivedSize);
    m->add("PCard", CALLBACK_NOOP, "pcard", pcards, pcardsSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
    m->setRefreshAssetsCallback([]() {
      Menu* m = &menus[PAGE_CREATE_PALLET];
      m->items[4].updateValue(rfidReader1CardId, 0);
    });
    m->setAutoRefresh();
  }

  {
    // Menu createBucket
    // On device assets
    String btags[] = {"BTAG0001"};
    size_t btagsSize = 1;
    String colors[] = {"WHITE", "GREY", "BLACK", "BLUE", "GREEN", "YELLLOW", "RED"};
    size_t colorsSize = 7;
    String stages[] = {"EMPTY"};
    size_t stagesSize = 1;
    m = &menus[PAGE_CREATE_BUCKET];
    m->add("BTag", CALLBACK_NOOP, "btag", btags, btagsSize);
    m->add("Color", CALLBACK_NOOP, "color", colors, colorsSize);
    m->add("Stage", CALLBACK_NOOP, "stage", stages, stagesSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
    m->setRefreshAssetsCallback([]() {
      Menu* m = &menus[PAGE_CREATE_BUCKET];
      m->items[0].updateValue(rfidReader2CardId, 0);
    });
    m->setAutoRefresh();
  }

  {
    // Menu fillBucket
    // On device assets
    String btags[] = {"BTAG0001"};
    size_t btagsSize = 1;
    String stages[] = {"GREEN"};
    size_t stagesSize = 1;
    String pcards[] = {"PCARD001"};
    size_t pcardsSize = 1;
    String weights[] = {"0.0"};
    size_t weightsSize = 1;
    m = &menus[PAGE_FILL_BUCKET];
    m->add("BTag", CALLBACK_NOOP, "btag", btags, btagsSize);
    m->add("Stage", CALLBACK_NOOP, "stage", stages, stagesSize);
    m->add("PCard", CALLBACK_NOOP, "pcard", pcards, pcardsSize);
    m->add("Weight (lb)", CALLBACK_NOOP, "weight", weights, weightsSize);
    m->add("");
    m->add("START", onBucketFillStart);
    m->add("STOP", onBucketFillStop);
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
    m->setRefreshAssetsCallback([]() {
      Menu* m = &menus[PAGE_FILL_BUCKET];
      m->items[0].updateValue(rfidReader2CardId, 0);
      m->items[2].updateValue(rfidReader1CardId, 0);
      m->items[3].updateValue(String(loadCellReadingLb, 1), 0);
    });
    m->setAutoRefresh();
  }

  {
    // Menu createRoast
    // Online assets
    String roastIds[] = {"2022-08-17-r-1", "2022-08-17-r-2", "2022-08-17-r-3"};
    size_t roastIdsSize = 3;
    // On device assets
    String roastLevels[] = {"DARK", "MEDIUM", "LIGHT"};
    size_t roastLevelsSize = 3;
    m = &menus[PAGE_CREATE_ROAST];
    m->add("Roast", CALLBACK_NOOP, "roast_id", roastIds, roastIdsSize);
    m->add("Roast Level", CALLBACK_NOOP, "roast_level", roastLevels, roastLevelsSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
  }

  {
    // Menu inputRoast
    // Online assets
    String roastIds[] = {"2022-08-17-r-1", "2022-08-17-r-2", "2022-08-17-r-3"};
    size_t roastIdsSize = 3;
    // On device assets
    String btags[] = {"BTAG0001"};
    size_t btagsSize = 1;
    String weights[] = {"0.0"};
    size_t weightsSize = 1;
    m = &menus[PAGE_INPUT_ROAST];
    m->add("Roast", CALLBACK_NOOP, "roast_id", roastIds, roastIdsSize);
    m->add("BTag", CALLBACK_NOOP, "btag", btags, btagsSize);
    m->add("Weight", CALLBACK_NOOP, "weight", weights, weightsSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
    m->setRefreshAssetsCallback([]() {
      Menu* m = &menus[PAGE_INPUT_ROAST];
      m->items[1].updateValue(rfidReader2CardId, 0);
      m->items[2].updateValue(String(loadCellReadingLb, 1), 0);
    });
    m->setAutoRefresh();
  }

  {
    // Menu startRoast
    // Online assets
    String roastIds[] = {"2022-08-17-r-1", "2022-08-17-r-2", "2022-08-17-r-3"};
    size_t roastIdsSize = 3;
    // On device assets
    m = &menus[PAGE_START_ROAST];
    m->add("Roast", CALLBACK_NOOP, "roast_id", roastIds, roastIdsSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
  }

  {
    // Menu startRoast
    // Online assets
    String roastIds[] = {"2022-08-17-r-1", "2022-08-17-r-2", "2022-08-17-r-3"};
    size_t roastIdsSize = 3;
    // On device assets
    m = &menus[PAGE_END_ROAST];
    m->add("Roast", CALLBACK_NOOP, "roast_id", roastIds, roastIdsSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
  }

  {
    // Menu outputRoast
    // Online assets
    String roastIds[] = {"2022-08-17-r-1", "2022-08-17-r-2", "2022-08-17-r-3"};
    size_t roastIdsSize = 3;
    // On device assets
    String btags[] = {"BTAG0001"};
    size_t btagsSize = 1;
    String weights[] = {"0.0"};
    size_t weightsSize = 1;
    m = &menus[PAGE_OUTPUT_ROAST];
    m->add("Roast", CALLBACK_NOOP, "roast_id", roastIds, roastIdsSize);
    m->add("BTag", CALLBACK_NOOP, "btag", btags, btagsSize);
    m->add("Weight", CALLBACK_NOOP, "weight", weights, weightsSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
    m->setRefreshAssetsCallback([]() {
      Menu* m = &menus[PAGE_OUTPUT_ROAST];
      m->items[1].updateValue(rfidReader2CardId, 0);
      m->items[2].updateValue(String(loadCellReadingLb, 1), 0);
    });
    m->setAutoRefresh();
  }

  {
    // Menu createLot
    // Online assets
    String lotIds[] = {"L00001", "L00002", "L00003"};
    size_t lotIdsSize = 3;
    String products[] = {"Inferno", "Flying Cinder", "Velvet Blaze"};
    size_t productsSize = 3;
    // On device assets
    String lcards[] = {"LCARD01"};
    size_t lcardsSize = 1;
    m = &menus[PAGE_CREATE_LOT];
    m->add("Lot", CALLBACK_NOOP, "lot_id", lotIds, lotIdsSize);
    m->add("LCard", CALLBACK_NOOP, "lcard", lcards, lcardsSize);
    m->add("Product", CALLBACK_NOOP, "product", products, productsSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
    m->setRefreshAssetsCallback([]() {
      Menu* m = &menus[PAGE_CREATE_LOT];
      m->items[1].updateValue(rfidReader1CardId, 0);
    });
    m->setAutoRefresh();
  }

  {
    // Menu fillLot
    // On device assets
    String lcards[] = {"LCARD01"};
    size_t lcardsSize = 1;
    String btags[] = {"BTAG0001"};
    size_t btagsSize = 1;
    String weights[] = {"0.0"};
    size_t weightsSize = 1;
    m = &menus[PAGE_FILL_LOT];
    m->add("LCard", CALLBACK_NOOP, "lcard", lcards, lcardsSize);
    m->add("BTag", CALLBACK_NOOP, "btag", btags, btagsSize);
    m->add("Weight", CALLBACK_NOOP, "weight", weights, weightsSize);
    m->add("");
    m->add("SEND", onCmdSend);
    m->add("CANCEL", CALLBACK_GOTO_PAGE(PAGE_MAIN_MENU));
    m->setAwaitResponseCallback([]() {
      if (commResponseAvailable()) {
        String cmd = readCommResponse(true);
        // TODO: check to see if this is the response you want.
        pageOnMenu->awaitResponse = false;
        gotoPage(PAGE_MAIN_MENU);
      }
    });
    m->setRefreshAssetsCallback([]() {
      Menu* m = &menus[PAGE_FILL_LOT];
      m->items[0].updateValue(rfidReader1CardId, 0);
      m->items[1].updateValue(rfidReader2CardId, 0);
      m->items[2].updateValue(String(loadCellReadingLb, 1), 0);
    });
    m->setAutoRefresh();
  }
}

void drawMenuPage(const Menu& menu) {
  if (menu.awaitResponse) {
    // Draw loading overlay.
    tft.setTextSize(3);
    tft.setCursor(0, tft.height() / 2 - 12);
    tft.setTextColor(ILI9341_BLUE, ILI9341_YELLOW);
    tft.print("   WAITING   ");
  } else {
    // Draw regular menu.
    tft.setCursor(0, 0);
    tft.setTextSize(2);

    for (int i = 0; i < menu.length(); i++) {
      if (menu.cursorOn(i)) {
        tft.setTextColor(ILI9341_YELLOW, ILI9341_NAVY);
      } else {
        tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      }
      if (screenToRefresh) {
        tft.println(menu.items[i].titleLine());
      } else {
        // Partial refresh skips reprinting the menu lines.
        tft.println("");
      }
      if (menu.items[i].hasValue()) {
        tft.println(menu.items[i].valueLine());
      }
    }
  }

  if (menu.autoRefresh) {
    setScreenToPartialRefreshIn(UI_MENU_AUTO_REFRESH_CYCLE);
  }
}

void gotoPage(int page) {
  pageOn = page;
  pageOnMenu = &menus[page];
  screenToClear = true;
  screenToRefresh = true;
}

void setScreenToPartialRefreshIn(long ms) {
  screenToPartialRefreshTimeout = millis() + ms;
}

void screenUpdateCallback() {
  pageOnMenu->onAwaitResponse();
  if (screenToPartialRefreshTimeout &&
      millis() > screenToPartialRefreshTimeout) {
    screenToPartialRefreshTimeout = 0;
    screenToPartialRefresh = true;
  }
  if (!screenToClear && !screenToRefresh && !screenToPartialRefresh) {
    // Lazy update: no redraw until something happens.
    return;
  }
  if (screenToClear) {
    tft.fillScreen(ILI9341_BLACK);
  }
  pageOnMenu->onRefreshAssets();
  drawMenuPage(*pageOnMenu);
  screenToClear = false;
  screenToRefresh = false;
  screenToPartialRefresh = false;
}

void inputDeviceReadCallback() {
  // Done in onButtonPress interrupt.
}

void uiProcessCallback() {
  if (uiInput != UI_INPUT_NULL) {
    if (uiInput == UI_INPUT_UP) {
      pageOnMenu->onUp();
    } else if (uiInput == UI_INPUT_DOWN) {
      pageOnMenu->onDown();
    } else if (uiInput == UI_INPUT_LEFT) {
      pageOnMenu->onLeft();
    } else if (uiInput == UI_INPUT_RIGHT) {
      pageOnMenu->onRight();
    } else if (uiInput == UI_INPUT_SEL) {
      pageOnMenu->onSelect();
    }
    screenToRefresh = true;
    uiInput = UI_INPUT_NULL;
  }
}

void enableInput() {
  inputDeviceReadTask.enable();
}

void disableInput() {
  inputDeviceReadTask.disable();
}

///////////////////////////////////
// Communication Functionalities //
///////////////////////////////////

// Enqueues request and returns true if successful.
bool enqueueCommRequest(const String& req, bool disableInputOnSuccess) {
  if (channelReq.length()) {
    return false;
  }
  if (disableInputOnSuccess) {
    disableInput();
  }
  channelReq = req;
  return true;
}

bool commResponseAvailable() {
  return channelRespReady;
}

// Reads the response and clear the result.
String readCommResponse(bool enableInputOnSuccess) {
  if (channelRespReady) {
    String response = channelResp;
    channelReq = "";
    channelResp = "";
    channelDownReady = false;
    channelReqSent = false;
    channelRespReady = false;
    if (enableInputOnSuccess) {
      enableInput();
    }
    return response;
  }
  return "";
}

// Handles communication. Can only accept one request.
// TODO: Implement FIFO queue.
void commChannelProcessCallback() {
  if (channelReq.length()) {
    if (!channelReqSent) {
      channelDownReady = false;
      channelUp = channelReq;
      channelReqSent = true;
    } else if (channelDownReady) {
      channelResp = channelDown;
      channelDownReady = false;
      channelRespReady = true;
    }
  }
}

void onChannelDownChange()  {
  channelUp = "";
  channelDownReady = true;
}

/////////////////////
// END Actual Code //
/////////////////////

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  while (!Serial);

  pinMode(PIN_UP, INPUT_PULLUP);
  pinMode(PIN_DOWN, INPUT_PULLUP);
  pinMode(PIN_LEFT, INPUT_PULLUP);
  pinMode(PIN_RIGHT, INPUT_PULLUP);
  pinMode(PIN_SEL, INPUT_PULLUP);

  // Initialize TFT display
  Serial.println("Initialize TFT.");
  tft.begin();

  tft.setRotation(0);
  int width = tft.width();
  int height = tft.height();
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
  tft.setTextSize(2);
  String text;
  text = "SUMMERMOON COFFEE";
  tft.setCursor((width - 12 * text.length()) / 2, height / 2 - 12);
  tft.print(text);

  // Set up timed tasks.
  Serial.println("Initialize Task Scheduler.");
  runner.init();
  runner.addTask(screenUpdateTask);
  runner.addTask(inputDeviceReadTask);
  runner.addTask(uiProcessTask);
  runner.addTask(commChannelProcessTask);

  // delay(1000);
  loadAssets();

  // Button interrupt (emulation specific)
  attachInterrupt(digitalPinToInterrupt(PIN_UP), onButtonPress, RISING);
  attachInterrupt(digitalPinToInterrupt(PIN_DOWN), onButtonPress, RISING);
  attachInterrupt(digitalPinToInterrupt(PIN_LEFT), onButtonPress, RISING);
  attachInterrupt(digitalPinToInterrupt(PIN_RIGHT), onButtonPress, RISING);
  attachInterrupt(digitalPinToInterrupt(PIN_SEL), onButtonPress, RISING);

  Serial.println("Ready.");
  screenUpdateTask.enable();
  inputDeviceReadTask.enable();
  uiProcessTask.enable();
  commChannelProcessTask.enable();
  gotoPage(PAGE_MAIN_MENU);
  // gotoPage(PAGE_CREATE_PALLET);
}

void onButtonPress() {
  if (!digitalRead(PIN_UP)) {
    uiInput = UI_INPUT_UP;
  } else if (!digitalRead(PIN_DOWN)) {
    uiInput = UI_INPUT_DOWN;
  } else if (!digitalRead(PIN_LEFT)) {
    uiInput = UI_INPUT_LEFT;
  } else if (!digitalRead(PIN_RIGHT)) {
    uiInput = UI_INPUT_RIGHT;
  } else if (!digitalRead(PIN_SEL)) {
    uiInput = UI_INPUT_SEL;
  }
}

void loop() {
  runner.execute();
  // Mock actions.
  static long lastExecution = 0;
  if (millis() >= lastExecution + 2000) {
    loadCellReadingLb = (millis() % 60000) / 1000;
    if (millis() % 10000 > 5000) {
      rfidReader1CardId = "";
      rfidReader2CardId = "_RFID2_";
    } else {
      rfidReader1CardId = "_RFID1_";
      rfidReader2CardId = "";
    }
    if (channelUp.length()) {
      String down = "{\"cmd\": \"ACK\"}";
      channelDown = down;
      onChannelDownChange();
    }
    lastExecution = millis();
  }
}