#include <Adafruit_ILI9341.h>
#include <TaskScheduler.h>
#include <AccelStepper.h>
#include <MultiStepper.h>
#include "MTDisplayModule.h"

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

MTDisplayModule *mtDisplay = nullptr;

#define PUMP_ENUM_WATER   0
#define PUMP_ENUM_MJ      1
#define PUMP_ENUM_HEAVY   2
#define PUMP_ENUM_WHOLE   3
#define PUMP_ENUM_SKIM    4
#define PUMP_ENUM_OAT     5
#define PUMP_ENUM_ALM     6
#define PUMP_ENUM_CCT     7
#define PUMP_ENUM_SIZE    8
String pumpEnumNames[PUMP_ENUM_SIZE] = {
  "Water",
  "MJ",
  "Heavy",
  "Whole",
  "Skim",
  "Oat",
  "Almond",
  "Coconut"
};

// A4988 Stepper Pump Driver 1 (Water)
// Wiring: D0<>DIR, D1<>STEP
#define PIN_A4988_P1_DIR 0
#define PIN_A4988_P1_STEP 1

// A4988 Stepper Pump Driver 2 (MJ)
// Wiring: D2<>DIR, D3<>STEP
#define PIN_A4988_P2_DIR 2
#define PIN_A4988_P2_STEP 3

// A4988 Stepper Pump Driver 3 (Heavy Cream)
// Wiring: D4<>DIR, D5<>STEP
#define PIN_A4988_P3_DIR 4
#define PIN_A4988_P3_STEP 5

// A4988 Stepper Pump Driver 4 (Whole Milk)
// Wiring: D6<>DIR, D7<>STEP
#define PIN_A4988_P4_DIR 6
#define PIN_A4988_P4_STEP 7

// A4988 Stepper Pump Driver 5 (Skim Milk)
// Wiring: D8<>DIR, D9<>STEP
#define PIN_A4988_P5_DIR 8
#define PIN_A4988_P5_STEP 9

// A4988 Stepper Pump Driver 6 (Oat Milk)
// Wiring: D10<>DIR, D11<>STEP
#define PIN_A4988_P6_DIR 10
#define PIN_A4988_P6_STEP 11

// A4988 Stepper Pump Driver 7 (Almond Milk)
// Wiring: D12<>DIR, D13<>STEP
#define PIN_A4988_P7_DIR 12
#define PIN_A4988_P7_STEP 13

// A4988 Stepper Pump Driver 8 (Coconut Milk)
// Wiring: D14<>DIR, D15<>STEP
#define PIN_A4988_P8_DIR 14
#define PIN_A4988_P8_STEP 15

AccelStepper pumps[PUMP_ENUM_SIZE] = {
  {1, PIN_A4988_P1_STEP, PIN_A4988_P1_DIR},
  {1, PIN_A4988_P2_STEP, PIN_A4988_P2_DIR},
  {1, PIN_A4988_P3_STEP, PIN_A4988_P3_DIR},
  {1, PIN_A4988_P4_STEP, PIN_A4988_P4_DIR},
  {1, PIN_A4988_P5_STEP, PIN_A4988_P5_DIR},
  {1, PIN_A4988_P6_STEP, PIN_A4988_P6_DIR},
  {1, PIN_A4988_P7_STEP, PIN_A4988_P7_DIR},
  {1, PIN_A4988_P8_STEP, PIN_A4988_P8_DIR},
};
MultiStepper pumpAggregator;

#define PUMP_STATE_ENUM_IDLE          0
#define PUMP_STATE_ENUM_PRIMING       1
#define PUMP_STATE_ENUM_FILLING       2
#define PUMP_STATE_ENUM_RETRACTING    3
#define PUMP_STATE_ENUM_DISABLED      4
#define PUMP_STATE_ENUM_SIZE          5
int pumpAggregatorState = PUMP_STATE_ENUM_IDLE;

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

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

// UI
#define UI_INPUT_NULL   'N'  // Null
#define UI_INPUT_A      'A'  // Backspace
#define UI_INPUT_B      'B'  // Prep
#define UI_INPUT_C      'C'  // Make
#define UI_INPUT_D      'D'  // Clean
#define UI_INPUT_E      'E'  // Enter; Click
#define UI_INPUT_FAULT  'F'  // Fault
#define UI_INPUT_STAR   '*'  // Star
#define UI_INPUT_POUND  '#'  // Pound
#define UI_INPUT_LEFT   '<'  // Left
#define UI_INPUT_RIGHT  '>'  // Right

int uiInput = UI_INPUT_NULL;
String uiInputChar = "";

#define UI_LINE_HEIGHT 20
#define UI_LINE_CHAR_SIZE 20

String deviceStatus = "READY";
String uiInputAccumulator = "";

#define LINE_HEIGHT 15
#define CHAR_WIDTH 15
#define DEFAULT_TEXT_COLOR ILI9341_WHITE
#define DEFAULT_TEXT_BACKGROUND ILI9341_BLACK
#define HIGHLIGHT_TEXT_COLOR ILI9341_BLUE
#define HIGHLIGHT_TEXT_BACKGROUND ILI9341_YELLOW

#define PAGE_ENUM_UNKNOWN 0
#define PAGE_ENUM_PREP 1
#define PAGE_ENUM_MAKE 2
#define PAGE_ENUM_CLEAN 3
#define PAGE_ENUM_SIZE 4
String pageEnumNames[PAGE_ENUM_SIZE] = {
  "",
  "PREP",
  "MAKE",
  "CLEAN"
};
int pageOn = PAGE_ENUM_UNKNOWN;

#define MILK_ENUM_UNKNOWN 0
#define MILK_ENUM_WATER 1
#define MILK_ENUM_MM 2
#define MILK_ENUM_MJ 3
#define MILK_ENUM_HEAVY 4
#define MILK_ENUM_HH 5
#define MILK_ENUM_WHOLE 6
#define MILK_ENUM_SKIM 7
#define MILK_ENUM_OAT 8
#define MILK_ENUM_ALM 9
#define MILK_ENUM_CCT 10
#define MILK_ENUM_MOON_OAT 11
#define MILK_ENUM_MOON_ALM 12
#define MILK_ENUM_MOON_CCT 13
#define MILK_ENUM_SIZE 14
String milkEnumNames[MILK_ENUM_SIZE] = {
  "Milk  ",
  "Water ",
  "MM    ",
  "MJ    ",
  "Heavy ",
  "H&H   ",
  "Whole ",
  "Skim  ",
  "Oat   ",
  "Alm   ",
  "Cct   ",
  "MM+Oat",
  "MM+Alm",
  "MM+Cct"
};

#define SPINNER_REFRESH_RATE 500
const String spinnerChar[4] = {"|", "/", "-", "\\"};
String overlayLine1 = "";
String overlayLine2 = "";
#define OVERLAY_TEXT_COLOR ILI9341_WHITE
#define OVERLAY_TEXT_BACKGROUND ILI9341_NAVY
bool overlayOn = false;
bool clearScreen = false;

// Records the current absolute position of each pump.
long pumpPositions[PUMP_ENUM_SIZE];
// Records the delta to new target of each pump.
long pumpDeltas[PUMP_ENUM_SIZE];

// TODO: To calibrate.
// Steps per ounce (@speed 1250).
long pumpStepsPerOz = 1425;
// Retract liquid into refridgerated space.
long pumpRetractSteps = -100;
// Dispense limit for priming.
long pumpPrimeOz = 30;

// Receipt
#define MM_RECIPE_MJ          12.0/76.0
#define MM_RECIPE_HEAVY       32.0/76.0
#define MM_RECIPE_WHOLE       32.0/76.0
#define MM_RECIPE_VEGAN_MJ    3.0/16.0
#define MM_RECIPE_VEGAN_MILK  13.0/16.0
#define HH_RECIPE_HEAVY       1.0/3.0
#define HH_RECIPE_SKIM        2.0/3.0

///////////////////////////
// Common util Functions //
///////////////////////////

String trimPadStr(const String& str, bool center = true, uint8_t size = 20) {
  String s = "";
  uint8_t l = str.length();
  if (l == size) {
    return str;
  } else if (l > size) {
    return str.substring(0, size);
  }
  if (center) {
    for (int i = 0; i < (size - l) / 2; i++) {
      s += " ";
    }
  }
  s += str;
  for (int i = s.length(); i < size; i++) {
    s += " ";
  }
  return s;
}

//////////////////
// UI Functions //
//////////////////

void gotoPage(int page) {
  clearScreen = true;
  overlayOn = false;
  pageOn = page;
}

String getSpinner() {
  int t = millis() % (SPINNER_REFRESH_RATE * 4);
  return spinnerChar[t / SPINNER_REFRESH_RATE];
}

void drawOverlayScreen() {
  const String& blankLine = trimPadStr("");
  const String& spinner = trimPadStr(getSpinner());
  tft.setTextSize(2);
  tft.setTextColor(OVERLAY_TEXT_COLOR, OVERLAY_TEXT_BACKGROUND);
  tft.setCursor(0, 9 * LINE_HEIGHT);
  tft.println(blankLine);
  tft.println(spinner);
  tft.println(trimPadStr(overlayLine1));
  tft.println(trimPadStr(overlayLine2));
  tft.println(blankLine);
}

// Draws the top nav bar.
void drawNavBar() {
  tft.setTextColor(ILI9341_BLACK, ILI9341_GREEN);
  tft.setTextSize(1);
  tft.setCursor(0, 0);
  String displayString = " [" + pageEnumNames[pageOn] + "] " + deviceStatus;
  tft.print(trimPadStr(displayString, false, 40));
}

// Sets TFT text color for a given line number. Determines whether text is highlighted.
void setLineTextColor(uint8_t lineNum) {
  uint8_t acc_length = uiInputAccumulator.length();
  int offset = (lineNum - 1) * 2;
  if (acc_length >= offset + 1 &&
      acc_length <= offset + 2) {
    tft.setTextColor(HIGHLIGHT_TEXT_COLOR, HIGHLIGHT_TEXT_BACKGROUND);
  } else {
    tft.setTextColor(DEFAULT_TEXT_COLOR, DEFAULT_TEXT_BACKGROUND);
  }
}

// Stitches the string for a line of text for display, from accumulated input.
String getLineText(uint8_t lineNum, String spacer = " ") {
  const String& acc = uiInputAccumulator;
  uint8_t acc_length = acc.length();
  String line = "";
  int offset = (lineNum - 1) * 2;
  if (acc_length > offset + 1) {
    line += uiInputAccumulator.charAt(offset + 1);
  } else {
    line += "_";
  }
  line += spacer;
  if (acc_length > offset + 2) {
    line += uiInputAccumulator.charAt(offset + 2);
  } else {
    line += "_";
  }
  return line;
}

// Parses characters from the input accumulator int.
int parseInputToInt(uint8_t start, uint8_t end_, int default_ = -1) {
  const String& acc = uiInputAccumulator;
  if (acc.length() < end_) {
    return default_;
  }
  String sel = acc.substring(start, end_);
  for (int i = 0; i < sel.length(); i++) {
    if (!isDigit(sel.charAt(i))) {
      return default_;
    }
  }
  return sel.toInt();
}

// Gets the milk selection.
// By default, expected input: *____MM#
int getMilkSelection(int start = 5, int end_ = 7) {
  int sel = parseInputToInt(start, end_);
  if (sel < 0 || sel >= MILK_ENUM_SIZE) {
    return MILK_ENUM_UNKNOWN;
  }
  return sel;
}

// Gets the size selection.
// By default, expected input: *SS____#
int getSizeSelection(int start = 1, int end_ = 3) {
  return parseInputToInt(start, end_);
}
// Gets the fraction selection.
// By default, expected input: *__FF__#
int getFractionSelection(int start = 3, int end_ = 5) {
  return parseInputToInt(start, end_);
}

float getFractional(int fraction) {
  switch (fraction) {
    case 00:
    case 01:
      return 0.0;
    case 18:
      return 1.0 / 8.0;
    case 14:
      return 1.0 / 4.0;
    case 13:
      return 1.0 / 3.0;
    case 12:
      return 1.0 / 2.0;
    case 34:
      return 3.0 / 4.0;
    case 11:
      return 1.0;
  }
  return -1;
}

// Validates the input accumulator for the Prep Page.
// Expected input format: *MM#
bool validateInputPrep() {
  const String& acc = uiInputAccumulator;
  if (acc.length() != 4) {
    return false;
  }
  if (acc.charAt(0) != '*' && acc.charAt(3) != '#') {
    return false;
  }
  int milk = getMilkSelection(1, 3);
  if (milk == MILK_ENUM_UNKNOWN) {
    return false;
  }
  return true;
}

// Validates the input accumulator for the Make Page.
// Expected input format: *SSFFMM#
bool validateInputMake() {
  const String& acc = uiInputAccumulator;
  if (acc.length() != 8) {
    return false;
  }
  if (acc.charAt(0) != '*' && acc.charAt(7) != '#') {
    return false;
  }
  int size = getSizeSelection();
  if (size < 1 || size > 20) {
    return false;
  }
  if (getFractional(getFractionSelection()) < 0) {
    return false;
  }
  int milk = getMilkSelection();
  if (milk == MILK_ENUM_UNKNOWN) {
    return false;
  }
  return true;
}

bool validateInput() {
  if (pageOn == PAGE_ENUM_PREP) {
    return validateInputPrep();
  } else if (pageOn == PAGE_ENUM_MAKE) {
    return validateInputMake();
  } else if (pageOn == PAGE_ENUM_CLEAN) {
    Serial.println("Validate input for clean page");
  }
  return false;
}

void drawMakeScreen() {
}

void drawPrepScreen() {
}

void commitInputAccumulator() {
  Serial.println("Committing input: " + uiInputAccumulator);
  if (validateInput()) {
    if (pageOn == PAGE_ENUM_PREP) {
      Serial.println("Commit OK");
      int milk = getMilkSelection(1, 3);
      primePump(milk);
      overlayLine1 = "PRIMING";
      overlayLine2 = "Press A to stop";
      overlayOn = true;
    } else if (pageOn == PAGE_ENUM_MAKE) {
      Serial.println("Commit OK");
      int size = getSizeSelection();
      int fraction = getFractionSelection();
      int milk = getMilkSelection();
      startPumps(size, fraction, milk);
      overlayLine1 = "DISPENSING";
      overlayLine2 = "Press A to stop";
      overlayOn = true;
    }
  } else {
    Serial.println("Invalid");
    overlayLine1 = "INVALID INPUT";
    overlayLine2 = "Press A to retry";
    overlayOn = true;
  }
}

// Returns amount of ingredients in ounces.
void calculateIngredients(int size_, int fraction, int milk, float* amounts) {
  float size = size_;
  float main_multiplier = getFractional(fraction);
  float other_multiplier = 1.0 - main_multiplier;
  if (main_multiplier < 0) {
    Serial.println("Invalid fractional amount. Abort!");
    return;
  }
  switch (milk) {
    case MILK_ENUM_WATER:
      amounts[PUMP_ENUM_WATER] = size;
      break;
    case MILK_ENUM_MJ:
      amounts[PUMP_ENUM_MJ] = size;
      break;
    case MILK_ENUM_MM:
    case MILK_ENUM_SKIM:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_MJ * size;
      amounts[PUMP_ENUM_HEAVY] = main_multiplier * MM_RECIPE_HEAVY * size;
      amounts[PUMP_ENUM_WHOLE] = main_multiplier * MM_RECIPE_WHOLE * size;
      amounts[PUMP_ENUM_SKIM] += other_multiplier * size;
      break;
    case MILK_ENUM_HEAVY:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_MJ * size;
      amounts[PUMP_ENUM_HEAVY] = main_multiplier * MM_RECIPE_HEAVY * size;
      amounts[PUMP_ENUM_WHOLE] = main_multiplier * MM_RECIPE_WHOLE * size;
      amounts[PUMP_ENUM_HEAVY] += other_multiplier * size;
      break;
    case MILK_ENUM_WHOLE:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_MJ * size;
      amounts[PUMP_ENUM_HEAVY] = main_multiplier * MM_RECIPE_HEAVY * size;
      amounts[PUMP_ENUM_WHOLE] = main_multiplier * MM_RECIPE_WHOLE * size;
      amounts[PUMP_ENUM_WHOLE] += other_multiplier * size;
      break;
    case MILK_ENUM_MOON_OAT:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_MJ * size;
      amounts[PUMP_ENUM_HEAVY] = main_multiplier * MM_RECIPE_HEAVY * size;
      amounts[PUMP_ENUM_WHOLE] = main_multiplier * MM_RECIPE_WHOLE * size;
      amounts[PUMP_ENUM_OAT] = other_multiplier * size;
      break;
    case MILK_ENUM_MOON_ALM:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_MJ * size;
      amounts[PUMP_ENUM_HEAVY] = main_multiplier * MM_RECIPE_HEAVY * size;
      amounts[PUMP_ENUM_WHOLE] = main_multiplier * MM_RECIPE_WHOLE * size;
      amounts[PUMP_ENUM_ALM] = other_multiplier * size;
      break;
    case MILK_ENUM_MOON_CCT:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_MJ * size;
      amounts[PUMP_ENUM_HEAVY] = main_multiplier * MM_RECIPE_HEAVY * size;
      amounts[PUMP_ENUM_WHOLE] = main_multiplier * MM_RECIPE_WHOLE * size;
      amounts[PUMP_ENUM_CCT] = other_multiplier * size;
      break;
    case MILK_ENUM_HH:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_MJ * size;
      amounts[PUMP_ENUM_HEAVY] = main_multiplier * MM_RECIPE_HEAVY * size;
      amounts[PUMP_ENUM_WHOLE] = main_multiplier * MM_RECIPE_WHOLE * size;
      amounts[PUMP_ENUM_HEAVY] += other_multiplier * HH_RECIPE_HEAVY * size;
      amounts[PUMP_ENUM_SKIM] += other_multiplier * HH_RECIPE_SKIM * size;
      break;
    case MILK_ENUM_OAT:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_VEGAN_MJ * size;
      amounts[PUMP_ENUM_OAT] = main_multiplier * MM_RECIPE_VEGAN_MILK * size;
      amounts[PUMP_ENUM_OAT] += other_multiplier * size;
      break;
    case MILK_ENUM_ALM:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_VEGAN_MJ * size;
      amounts[PUMP_ENUM_ALM] = main_multiplier * MM_RECIPE_VEGAN_MILK * size;
      amounts[PUMP_ENUM_ALM] += other_multiplier * size;
      break;
    case MILK_ENUM_CCT:
      amounts[PUMP_ENUM_MJ] = main_multiplier * MM_RECIPE_VEGAN_MJ * size;
      amounts[PUMP_ENUM_CCT] = main_multiplier * MM_RECIPE_VEGAN_MILK * size;
      amounts[PUMP_ENUM_CCT] += other_multiplier * size;
      break;
    default:
      Serial.println("Unrecognized milk selection.");
  }
}

//////////////////////////
// Timed Task Callbacks //
//////////////////////////

void screenUpdateCallback() {
  mtDisplay->draw();
  /*if (clearScreen) {
    tft.fillScreen(ILI9341_BLACK);
    clearScreen = false;
  }
  if (overlayOn) {
    drawOverlayScreen();
  } else {
    if (pageOn == PAGE_ENUM_PREP) {
      drawPrepScreen();
    } else if (pageOn == PAGE_ENUM_MAKE) {
      drawMakeScreen();
    } else if (pageOn == PAGE_ENUM_CLEAN) {
      // drawCleanScreen();
    }
  }*/
}

void inputDeviceReadCallback() {
  if (Serial.available() > 0) {
    String s = Serial.readStringUntil('\n');
    s.toUpperCase();
    char c = s.charAt(0);
    switch(c) {
      case '0':
      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
      case '6':
      case '7':
      case '8':
      case '9':
      case 'A':
      case 'B':
      case 'C':
      case 'D':
      case 'E':
      case 'F':
      case '*':
      case '#':
      case '<':
      case '>':
      case 'N':
        Serial.println("Input device read got: " + String(c));
        return mtDisplay->handleInput(c);
      default:
        Serial.println("Unrecognized: " + String(c));
    }
  }
}

void uiProcessCallback() {
  if (uiInput != UI_INPUT_NULL &&
      uiInput != UI_INPUT_FAULT) {
    if (uiInput == UI_INPUT_STAR) {
      uiInputAccumulator = "*";
    } else if (uiInput == UI_INPUT_POUND) {
      uiInputAccumulator += uiInputChar;
      commitInputAccumulator();
    } else if (uiInput == UI_INPUT_A) {
      // Back.
      if (pumpAggregatorState != PUMP_STATE_ENUM_IDLE) {
        // Stop all pumps.
        stopPumps();
      }
      if (overlayOn) {
        uiInputAccumulator = "";
        overlayOn = false;
        clearScreen = true;
      } else {
        uint8_t length = uiInputAccumulator.length();
        if (length) {
          uiInputAccumulator = uiInputAccumulator.substring(0, length - 1);
        }
      }
    } else if (uiInput == UI_INPUT_B) {
      // TODO: check if there are unstoppable tasks.
      gotoPage(PAGE_ENUM_PREP);
    } else if (uiInput == UI_INPUT_C) {
      // TODO: check if there are unstoppable tasks.
      gotoPage(PAGE_ENUM_MAKE);
    } else if (uiInput == UI_INPUT_D) {
      // TODO: check if there are unstoppable tasks.
      gotoPage(PAGE_ENUM_CLEAN);
    } else {
      // Numbers.
      if (uiInputAccumulator.length() >= 1 &&
          uiInputAccumulator.length() <= 6 &&
          uiInputAccumulator.charAt(0) == '*') {
        uiInputAccumulator += uiInputChar;
        Serial.println("Accumulator: " + uiInputAccumulator);
      } else {
        Serial.println("Ignore invalid input");
      }
    }
    uiInputChar = "";
    uiInput = UI_INPUT_NULL;
    screenUpdateCallback();
  }
}

void pumpControlCallback() {
  // Manage pump states.
  bool completed = true;
  for (int i = 0; i < PUMP_ENUM_SIZE; i++) {
    if (pumps[i].isRunning()) {
      completed = false;
      break;
    }
  }
  if (completed) {
    // Advance to retracting liquid if necessary.
    if (pumpAggregatorState == PUMP_STATE_ENUM_FILLING ||
        pumpAggregatorState == PUMP_STATE_ENUM_PRIMING) {
      startPumpsRetract();
    } else if (pumpAggregatorState == PUMP_STATE_ENUM_RETRACTING) {
      stopPumps();
    }
  }
}

//////////////////
// Pump Actions //
//////////////////

// Resets the position of all pumps, making current position 0.
void resetPumpPositions() {
  for(int i = 0; i < PUMP_ENUM_SIZE; i++) {
    pumps[i].setCurrentPosition(0);
  }
}

void primePump(int milk) {
  Serial.println("Prime pumps for " + milkEnumNames[milk]);
  for (int i = 0; i < PUMP_ENUM_SIZE; i++) {
    pumpDeltas[i] = 0;
    if (i == milk) {
      pumpDeltas[i] = pumpPrimeOz * pumpStepsPerOz;
    }
  }
  resetPumpPositions();
  pumpAggregatorState = PUMP_STATE_ENUM_PRIMING;
  pumpAggregator.moveTo(pumpDeltas);
}

void stopPumps() {
  Serial.println("Stopping all pumps");
  for (int i = 0; i < PUMP_ENUM_SIZE; i++) {
    if (pumpDeltas[i]) {
      // Do something to pumps that moved.
    }
    pumps[i].stop();
  }
  if(pumpAggregatorState == PUMP_STATE_ENUM_FILLING ||
     pumpAggregatorState == PUMP_STATE_ENUM_PRIMING) {
    // Manually stopped flow requires retraction.
    Serial.println("Manually stopped filling / priming.");
    startPumpsRetract();
  } else {
    pumpAggregatorState = PUMP_STATE_ENUM_IDLE;
  }
}

void startPumps(int size, int fraction, int milk) {
  Serial.println("Start pumps to dispense: " +
                 String(size) + "oz of " +
                 String(fraction) + " " +
                 milkEnumNames[milk]);
  float ingredients[PUMP_ENUM_SIZE];
  calculateIngredients(size, fraction, milk, ingredients);
  Serial.println("Recipe: ");
  for (int i = 0; i < PUMP_ENUM_SIZE; i++) {
    pumpDeltas[i] = 0;
    if (ingredients[i] > 0.01) {
      Serial.println(" " + pumpEnumNames[i] + ": " + String(ingredients[i]) + "oz");
      pumpDeltas[i] = ingredients[i] * pumpStepsPerOz;
    }
  }
  resetPumpPositions();
  pumpAggregatorState = PUMP_STATE_ENUM_FILLING;
  pumpAggregator.moveTo(pumpDeltas);
}

void startPumpsRetract() {
  Serial.println("Start pumps retraction");
  for (int i = 0; i < PUMP_ENUM_SIZE; i++) {
    if (pumpDeltas[i]) {
      pumpDeltas[i] = pumpRetractSteps;
    }
  }
  resetPumpPositions();
  pumpAggregatorState = PUMP_STATE_ENUM_RETRACTING;
  pumpAggregator.moveTo(pumpDeltas);
}

void startSanitaryCycle() {
  Serial.println("Start sanitary cycle");
}

//////////////////
// Staging Code //
//////////////////

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

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

  mtDisplay = new MTDisplayModule(&tft);
  mtDisplay->setTitle("LOW: Heavy Alm Oat Cct");

  // mtDisplay->drawSplashScreen();
  /*
  mtDisplay->setInputAcc("*1234");
  mtDisplay->setHighlightLineNo(5);
  mtDisplay->drawMakeScreen();
  */
  /*
  mtDisplay->setInputAcc("*");
  mtDisplay->setHighlightLineNo(3);
  mtDisplay->drawPrepScreen();
  */

  // while(1) {}
  mtDisplay->loadPage(PAGE_SPLASH);

  // Initialize pumps.
  for (int i = 0; i < PUMP_ENUM_SIZE; i++) {
    pumps[i].stop();
    pumps[i].setMaxSpeed(800);
    pumpAggregator.addStepper(pumps[i]);
  }

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

  gotoPage(PAGE_ENUM_MAKE);

  screenUpdateTask.enable();
  inputDeviceReadTask.enable();
  uiProcessTask.enable();
  pumpControlTask.enable();

  Serial.println("Ready.");
}

void loop() {
  runner.execute();
  pumpAggregator.run();
}