// =============================
// Scope, Description, Overview
// =============================
/*
 *  The objective of this program is to make the Arduino Nano generate quadrature 90 degrees offset pulse trains on two channels in order to mimic the output of an
 *  ATEX/intrinsically safe, optical/NAMUR Hohner "SERIE E"/Series E (SPE-15) encoder that we use for depth measurement in EX-zone. (Internal company P/N: 104725).
 *  The user will be able to specify the direction in which the simulated distance will go: "Up" increases elapsed distance,
 *  "Down" decreases elapsed distance (negative if already at zero), and "Both" will have the distance travel up at first, then back down to zero again.
 *  Most important params: distance, speed & direction. Highest speed is 100m/min and 1000m distance. Desirable distances to test are 10, 100, 300, 500 and 1000m.
 *  Code will be run on an Arduino Nano (Atmega 328p) with a 20x4 character LCD display (i2c), and a push-button rotary encoder for user input and control.
 *  https://www.encoder-hohner.com/product-detail/series-e/
 */



// =============
// Preprocessor
// =============
#define MCU 2040 // Specify MCU used, code can be adapted to run on either RPi Pico (#define MCU 2040) or Arduino UNO/Nano (#define MCU 328)

#if MCU == 2040
#define ENC_LIB <pio_encoder.h>
#define RP2040_ENCODER_INIT encoder.begin();
#define ENCODER_OBJ PioEncoder encoder(2); // encoder is connected to GPIO2 and GPIO3
#define CHECK_ENCODER check2040Encoder()
#define READ_ENC encoder.getCount() / 4;
#elif MCU == 328
#define ENC_LIB <Encoder.h>
#define RP2040_ENCODER_INIT nullptr
#define ENCODER_OBJ Encoder Enc(ROTARY_ENCODER_CLK, ROTARY_ENCODER_DT); // Encoder object: "name"(Ch.A pin#, Ch.B pin#); Invoke: "enc.read();", "enc.write();"
#define CHECK_ENCODER checkEncoder
#define READ_ENC Enc.read() / 4; // Read encoder pulses
#else
#error "Invalid MCU!"
#endif

#define LCD_TYPE LCD_TYPE_PARALLEL // TODO <<< Set LCD type (Using parallel in Wokwi, i2c in real world)
#define LCD_TYPE_I2C 1
#define LCD_TYPE_PARALLEL 2
#define LCD_TYPE_WOKWI_I2C 3
#define OPTIONAL_WOKWI_I2C_SETUP nullptr // Can add a couple of lines of code if using Wokwi I2C

#if LCD_TYPE == LCD_TYPE_I2C
#define LCD_LIBRARY <Adafruit_LiquidCrystal.h>
#define LCD_OBJECT_CONFIG Adafruit_LiquidCrystal lcd(LCD_I2C_ADR)
#elif LCD_TYPE == LCD_TYPE_PARALLEL
#define LCD_LIBRARY <LiquidCrystal.h>
#define LCD_OBJECT_CONFIG LiquidCrystal lcd(12, 11, 10, 9, 8, 7)
#elif LCD_TYPE == LCD_TYPE_WOKWI_I2C
#define LCD_LIBRARY <LiquidCrystal_I2C.h>
#define LCD_I2C_ADR 0x27
#define LCD_OBJECT_CONFIG LiquidCrystal_I2C lcd(LCD_I2C_ADR, LCD_COLUMNS, LCD_ROWS);
#define OPTIONAL_WOKWI_I2C_SETUP
lcd.init();
lcd.backlight()
#else
#error "No valid LCD type specified!"
#endif

// ==========
// Libraries
// ==========
#include <Arduino.h>
#include LCD_LIBRARY   /* If i2C: https://github.com/adafruit/Adafruit_LiquidCrystal , If parallel: use std LiquidCrystal.h */
#include ENC_LIB       /* https://github.com/PaulStoffregen/Encoder (ATmega328p) OR https://github.com/gbr1/rp2040-encoder-library (RP2040) */
#include <OneButton.h> /* https://github.com/mathertel/OneButton */
#include <Wire.h>      /* https://github.com/esp8266/Arduino/tree/master/libraries/Wire */
#include <BlockNot.h>  /* https://github.com/EasyG0ing1/BlockNot */
#include <LibPrintf.h> /* https://github.com/embeddedartistry/arduino-printf  */
// #include <TimerOne.h> /* https://github.com/PaulStoffregen/TimerOne */

// ================
// Definitions
// ================
// Pins
#define ROTARY_ENCODER_CLK 3        // Rotary encoder pulse input channel A
#define ROTARY_ENCODER_DT 2         // Rotary encoder pulse input channel B
#define ROTARY_ENCODER_PUSHBUTTON 4 // Rotary encoder pushbutton
#define OUTPUT_CH_A 6               // Output channel A
#define OUTPUT_CH_B 5               // Output channel B

// LCD
#define LCD_I2C_ADR 0  // LCD i2c address
#define LCD_COLUMNS 20 // Number of LCD columns
#define LCD_ROWS 4     // Number of LCD rows

// Serial
#define SERIAL_BAUD_RATE 9600 // Serial monitor speed
#define NUMBER_OF_COMMANDS 4  // Total number of commands/size of serial command buffer
#define COMMAND_MAX_LENGTH 6  // Max number of serial command characters

// ========
// Objects
// ========
LCD_OBJECT_CONFIG;                                                // If i2c: "Adafruit_LiquidCrystal lcd(LCD_I2C_ADR)", if parallel: LiquidCrystal lcd(12, 11, 10, 9, 8, 7)
ENCODER_OBJ
OneButton btn = OneButton(ROTARY_ENCODER_PUSHBUTTON, true, true); // Button object = OneButton(pin#, active low?, internal pullup?);
BlockNot nonBlockingTimer(150);                                   // Non-blocking timer object(milliseconds)

// =================
// Global variables
// =================

// LCD & LCD menu variables
int8_t menuCounter = -2;                // Determines the cursor row position in the LCD menu
int8_t currentMenu = 0;                 // The current menu shown: 0 = main, 1 = secondary
int8_t menuCounterMin = 0;              // Menu's uppermost cursor position (LCD row)
int8_t menuCounterMax = (LCD_ROWS - 1); // Menu's lowermost cursor position (LCD row)
const String cursor = ">";              // Menu cursor
bool runFinished = false;               // Flag to indicate if the run is completed

// Serial monitor commands variables
char cmdBuffer[COMMAND_MAX_LENGTH]; // Command buffer

// Other variables
int16_t distElapsed = 0; //

// =======
// Arrays
// =======
// Array structures

struct SerialCommand // Define serial commands structure
{
  const char *cmd;  // Command
  const char *desc; // Command description
};

struct MenuItem // Define menu item array structure
{
  String label; // Menu item name
  String value; // Menu item value
};

// Declare array
SerialCommand serialCommands[NUMBER_OF_COMMANDS] = { // Serial commands array, easily expandable
    {"help", " - Shows all available commands"},
    {"quit", " - Returns to main menu"},
    {"poll", " - Displays multiple variables' current values"}};

MenuItem mainMenuItems[] = { // Main menu array
    {"Pulse pr m: ", ""},
    {"Distance  : ", ""},
    {"Speed     : ", ""},
    {"Continue... ", ""}};

MenuItem secondMenuItems[] = { // Secondary menu array
    {"Direction : ", ""},
    {"Back ", ""},
    {"Start ", ""}};

MenuItem runMenuItems[] = { // Run menu items
    {"Target   : ", ""},
    {"Speed    : ", ""},
    {"Elapsed  : ", ""},
    {"Direction: ", ""}};

// Arrays containing available menu items
const uint16_t pulsesPerMeter[] = {1024, 512};          // Pulses per meter options, encoder type dependant, indexed by "ppmSel"
int8_t ppmSel = 1;                                      // "pulsesPerMeter" index selection factor
const uint16_t distances[] = {10, 100, 300, 500, 1000}; // Distance options, meters, indexed by "distSel"
int8_t distSel = 0;                                     // "distances" index selection factor
const uint8_t speeds[] = {10, 30, 50, 100};             // Speed options, meters per minute, indexed by "speedSel"
int8_t speedSel = 0;                                    // "speeds" index selection factor
const String dir[] = {"Both", "Up", "Down"};            // Direction options, "Both" runs up first then down again, indexed by "dirSel"
int8_t dirSel = 1;                                      // "dir" (direction) index selection factor: 0 = "BOTH", 1 = "UP", 2 = "DOWN"

// ==========
// Functions
// ==========

// Functions Declaration (needed for PlatformIO/CPP-file)
void debug();
void handle_command(const char *command);
bool is_valid_command(const char *command);
void splashScreen(const unsigned int splashDuration, const unsigned int splashBlinkDelay);
bool checkEncoder(); // Using this encoder: 62AG22-L5-040C: https://no.mouser.com/datasheet/2/626/grhl_s_a0011394895_1-2289471.pdf (3V3 version: 62VG22-L5-040C)
void lcdClearLn(uint8_t clearRow, uint8_t clearColumn, uint8_t lcdMaxNumOfColumns);
int minMaxReset(int input, int min, int max);
void handleSelection();
void run();
void outputPulses(const uint8_t travelDirection); // Makes quadrature pulses in the specified direction. Takes int argument for direction: 0 = UP, 1 = DOWN
void moveCursor(uint8_t menuPos);
void showMenu(const uint8_t menuType);
void onClick();
void longPress();

// Actual functions:
// Runs once
void setup()
{
  // Config pins
  pinMode(ROTARY_ENCODER_PUSHBUTTON, INPUT_PULLUP); // Rotary encoder momentary push button
  pinMode(ROTARY_ENCODER_CLK, INPUT);               // Rotary encoder input channel A
  pinMode(ROTARY_ENCODER_DT, INPUT);                // Rotary encoder input channel B
  pinMode(OUTPUT_CH_A, OUTPUT);                     // Encoder pulse simulation output channel A
  pinMode(OUTPUT_CH_B, OUTPUT);                     // Encoder pulse simulation output channel B

  // Config objects
  btn.attachClick(onClick);            // Fires as soon as a single click is detected
  btn.attachLongPressStart(longPress); // Fires as soon as the button is held down for 1 second

  // Initialize LCD, other devices, objects etc.
  RP2040_ENCODER_INIT
  OPTIONAL_WOKWI_I2C_SETUP;
  lcd.begin(LCD_COLUMNS, LCD_ROWS); // Initialize LCD, 20 columns, 4 rows
  delay(100);

  // Put values into arrays
  mainMenuItems[0].value = pulsesPerMeter[1];
  mainMenuItems[1].value = (String)distances[0] + "m";
  mainMenuItems[2].value = (String)speeds[0] + "m/mi";

  secondMenuItems[0].value = dir[1];

  runMenuItems[0].value = (String)distances[4] + "m";
  runMenuItems[1].value = (String)speeds[0] + "m/mi";
  runMenuItems[2].value = (String)distElapsed + "m ";
  runMenuItems[3].value = dir[0];

  splashScreen(500, 100); // Display splashscreen/"welcome" greet on startup, because we're polite.

  showMenu(currentMenu); // Show main menu

  delay(100);

  // Set up serial monitor for debugging and testing
  Serial.begin(SERIAL_BAUD_RATE);
  Serial.println(F("DEEPBox CLI Initiated!"));
  Serial.println(F("Enter a valid command to get started"));
  Serial.println(F("Or enter command 'help' to see a list of all available commands..."));
} // Setup

// Take and process incoming commands from serial monitor
void debug()
{
  static uint8_t cmdBufferIndex = 0; // Index of command buffer

  while (Serial.available() > 0)
  {
    char received_char = Serial.read();
    if (received_char == '\n') // If a newline ("enter") is detected
    {
      cmdBuffer[cmdBufferIndex] = '\0'; // null terminate the C string
      if (is_valid_command(cmdBuffer))
      {
        handle_command(cmdBuffer);
      }
      else
      {
        Serial.println(F("Invalid command!"));
        Serial.println(F("Type \"help\" to see all available commands."));
      }
      cmdBufferIndex = 0; // reset index
    }
    else if (cmdBufferIndex < COMMAND_MAX_LENGTH - 1)
    {
      cmdBuffer[cmdBufferIndex] = received_char;
      cmdBufferIndex++;
    }
  }
} // debug

// Execute code based on received command
void handle_command(const char *command)
{
  Serial.println("> Command \"" + (String)command + "\" received..."); // Echo last command
  /*
  ******************************************************************************************
  ************************** ADD COMMAND ACTIONS BELOW HERE ********************************
  **** FORMAT: "else if (strcmp(command, serialCommands[x].cmd) == 0) // "cmd" command" ****
  ******************************************************************************************
  */
  if (strcmp(command, serialCommands[0].cmd) == 0) // "help" command
  {
    for (int i = 0; i < NUMBER_OF_COMMANDS; i++) // Print all valid commands and their descriptions to serial monitor
    {
      if (serialCommands[i].cmd == nullptr || strlen(serialCommands[i].cmd) == 0) // Check if the cmd field is empty
      {
        break; // If the cmd field is empty, break the loop
      }
      Serial.print(F("> "));
      Serial.print(serialCommands[i].cmd);
      Serial.println(serialCommands[i].desc);
    }
  }
  else if (strcmp(command, serialCommands[1].cmd) == 0) // "quit" command
  {
    longPress(); // btn longPress function currently resets the program, reusing this code for this quit-command
  }
  else if (strcmp(command, serialCommands[2].cmd) == 0) // "poll" command
  {
    Serial.print(F("> menuCounter: ")); Serial.println(menuCounter);
    Serial.print(F("> currentMenu: ")); Serial.println(currentMenu);
    Serial.print(F("> distElapsed: ")); Serial.println((String)distElapsed + "m");
    Serial.print(F("> ppmSel.....: ")); Serial.print((String)ppmSel + " >> "); Serial.println((String)pulsesPerMeter[ppmSel] + " pulses/m");
    Serial.print(F("> distSel....: ")); Serial.print((String)distSel + " >> "); Serial.println((String)distances[distSel] + "m");
    Serial.print(F("> speedSel...: ")); Serial.print((String)speedSel + " >> "); Serial.println((String)speeds[speedSel] + "m/min");
    Serial.print(F("> dirSel.....: ")); Serial.print((String)dirSel + " >> "); Serial.println("'" + dir[dirSel] + "'");
    Serial.print(F("> runFinished: ")); Serial.println((String)runFinished);
  }
} // handle_command --> Used in debug function

// Compare serial input with command array, return true if matching, false otherwise
bool is_valid_command(const char *command)
{
  for (uint8_t i = 0; i < NUMBER_OF_COMMANDS; i++)
  {
    if (strcmp(command, serialCommands[i].cmd) == 0)
    {
      return true;
    }
  }
  return false;
} // is_valid_command --> Used in debug function

// Creates a welcome message on the display, and briefly flashes the LEDs
void splashScreen(const unsigned int splashDuration, const unsigned int splashBlinkDelay)
{
  lcd.clear();

  // Charlimit--------------------
  lcd.println(F("  === DEEPBox  ==="));
  lcd.setCursor(0, 1); // Column 1, Row 2
  // Charlimit--------------------
  lcd.println(F("  === V.0.0.0  ==="));
  lcd.setCursor(0, 2); // Column 1, Row 3
  // Charlimit--------------------
  lcd.println(F("  ===  BAKER   ==="));
  lcd.setCursor(0, 3); // Column 1, Row 4
  // Charlimit--------------------
  lcd.println(F("  ===   2023   ==="));
  delay(splashDuration); // splashDuration = For how many ms splash graphics is displayed

  // Blink LEDs on startup "splash screen"
  uint8_t initBlink = 0;
  while (initBlink < 5)
  {
    digitalWrite(OUTPUT_CH_A, HIGH);
    delay(splashBlinkDelay);
    digitalWrite(OUTPUT_CH_A, LOW);
    digitalWrite(OUTPUT_CH_B, HIGH);
    delay(splashBlinkDelay);
    digitalWrite(OUTPUT_CH_B, LOW);
    initBlink++;
  }

  lcd.clear();
} // splashScreen

// Read the rotary encoder's current position, detect position change and in which direction. Update menuCounter val accordingly.
bool checkEncoder()
{
  static bool encoderPositionChanged = false; // Used to flag encoder position change
  static long oldPos = -999;                  // Rotary encoder start position
  static long newPos;                         // Encoder position when doing Enc.read()

  newPos = READ_ENC

  newPos > oldPos ? menuCounter++, oldPos = newPos, encoderPositionChanged = true :     // Did the encoder turn right? Decrement menuCounter
      newPos < oldPos ? menuCounter--, oldPos = newPos, encoderPositionChanged = true : // Did the encoder turn left? Increment menuCounter
      encoderPositionChanged = false;                                                   // Encoder position hasn't changed, do nothing

  btn.tick(); // Check button state (Ref. OneButton lib)

  return encoderPositionChanged;
} // checkEncoder

bool check2040Encoder() {
  static bool encoderPositionChanged = false; // Used to flag encoder position change
  static long oldPos = -999;                  // Rotary encoder starting position
  static long newPos;                         // Encoder positoin retreived from encoder.getCount()

  newPos = READ_ENC

  if (newPos > oldPos) {
    menuCounter--;
    encoderPositionChanged = true;
    oldPos = newPos;
    Serial.println(F("> Enoder position changed!"));
    Serial.print(F("> Enoder.read(): ")); Serial.println(newPos);
  }

  if (newPos < oldPos) {
    menuCounter++;
    encoderPositionChanged = true;
    oldPos = newPos;
    Serial.println(F("> Enoder position changed!"));
    Serial.print(F("> Enoder.read(): ")); Serial.println(newPos);
  }

  btn.tick(); // Check button state (Ref. OneButton lib)

  return encoderPositionChanged;
} // check2040Encoder

// Sets the cursor on a specific point on LCD, and clears everything to the right of it. Useful for refreshing parts of LCD.
void lcdClearLn(uint8_t clearRow, uint8_t clearColumn, uint8_t lcdMaxNumOfColumns)
{
  lcd.setCursor(clearRow, clearColumn);
  for (uint8_t i = clearColumn; i < lcdMaxNumOfColumns; i++)
  {
    lcd.setCursor(i, clearRow);
    lcd.print((" "));
  }
} // lcdClearLn

// Ensures that a value (int input) is kept between a (int) min and (int) max value
int minMaxReset(int input, int min, int max)
{
  input > max ? input = min :      // If input is greater than max, reset input to min
      input < min ? input = max :  // Else, If input is lower than min, reset input to max
      input = input;               // Else, no need to reset input
  return input;
} // minMaxReset

// Takes action depending on the current menu and cursor position
void handleSelection()
{
  switch (currentMenu)
  {
  case 0: // If main menu
    switch (menuCounter)
    {
    case 0: // Set pulses per meter!
      ppmSel++;
      ppmSel = minMaxReset(ppmSel, 0, 1);
      mainMenuItems[0].value = pulsesPerMeter[ppmSel];
      lcdClearLn(menuCounter, 12, (LCD_COLUMNS - 1)); // Clear everything from row 1, column 12 and out
      showMenu(0);                                    // #0 - Main Menu
      break;

    case 1: // Set distance!
      distSel++;
      distSel = minMaxReset(distSel, 0, 4);
      mainMenuItems[1].value = (String)distances[distSel] + "m";
      lcdClearLn(menuCounter, 12, (LCD_COLUMNS - 1)); // Clear everything from row 2, column 12 and out
      showMenu(0);                                    // #0 - Main Menu
      break;

    case 2: // Set speed!
      speedSel++;
      speedSel = minMaxReset(speedSel, 0, 3); // Keeps speed selection factor between 0 and 2
      mainMenuItems[2].value = (String)speeds[speedSel] + "m/mi ";
      showMenu(0); // #0 - Main Menu
      break;

    case 3: // Continue to second menu/"page 2"
      lcd.clear();
      currentMenu = 1; // Set current menu to second
      showMenu(1);     // #1 - Secondary Menu
      break;
    }
    break;

  case 1: // If second menu page
    switch (menuCounter)
    {
    case 0: // Set direction!
      dirSel++;
      dirSel = minMaxReset(dirSel, 0, 2); // Keeps direction selection factor between 0 and 2
      secondMenuItems[0].value = dir[dirSel];
      lcdClearLn(menuCounter, 12, (LCD_COLUMNS - 1)); // Clear everything from row 0, column 12 and out
      showMenu(1);                                    // #1 - Secondary Menu
      break;

    case 1: // Back to main menu
      lcd.clear();
      currentMenu = 0; // #0 - Main Menu
      showMenu(currentMenu);
      break;

    case 2: // Start
      runMenuItems[0].value = (String)distances[distSel] + "m";
      runMenuItems[1].value = (String)speeds[speedSel] + "m/mi";
      runMenuItems[3].value = dir[dirSel];
      uint8_t runDirection = 0; // 0 = Not moving, 1 = Up, 2 = Down
      lcd.clear();
      currentMenu = 2; // #2 - Run-menu
      showMenu(currentMenu);
      run();
      break;
    }
    break;
  }

} // handleSelection

// What to do when btn is pressed
void onClick()
{
  Serial.println(F("> Button press detected!"));
  switch (currentMenu)
  {
  case 0:
    handleSelection();
    break;

  case 1:
    handleSelection();
    break;

  case 2:
    break;
  }
} // onClick

// When longPress is called by btn, currently used to cancel run and/or restart the application
void longPress()
{
  Serial.println(F("> Button long-press detected!"));
  runFinished = false; // Breaks the completed run while loop, in case it's active
  digitalWrite(OUTPUT_CH_A, LOW);
  digitalWrite(OUTPUT_CH_B, LOW);

  dirSel = 1; // Reset selections/settings-related variables
  ppmSel = 1;
  distSel = 0;
  speedSel = 0;
  currentMenu = 0;
  menuCounter = 0;
  distElapsed = 0; // Reset elapsed distance

  mainMenuItems[0].value = pulsesPerMeter[ppmSel]; // Reset menu items do defaults
  mainMenuItems[1].value = (String)distances[distSel] + "m";
  mainMenuItems[2].value = (String)speeds[speedSel] + "m/mi";

  secondMenuItems[0].value = dir[dirSel];

  runMenuItems[0].value = (String)distances[4] + "m";
  runMenuItems[1].value = (String)speeds[speedSel] + "m/mi";
  runMenuItems[2].value = (String)distElapsed + "m ";
  runMenuItems[3].value = dir[dirSel];

  splashScreen(500, 100);
  showMenu(currentMenu);
} // longPress

// Draws menus on LCD, takes argument: menuType 0 = main, 1 = secondary, 2 = run-menu
void showMenu(const uint8_t menuType)
{
  switch (menuType)
  {
  case 0: // Main Menu
    menuCounterMin = 0;
    menuCounterMax = 3;

    for (uint8_t i = 0; i < 4; i++) // i = Number of menu items
    {
      lcd.setCursor(1, i);
      lcd.print(mainMenuItems[i].label);
      lcd.print(mainMenuItems[i].value);
    }
    moveCursor(menuCounter);
    break;
  case 1: // Second Menu
    menuCounterMin = 0;
    menuCounterMax = 2;

    for (uint8_t i = 0; i < 3; i++) // i = Number of menu items
    {
      lcd.setCursor(1, i);
      lcd.print(secondMenuItems[i].label);
      lcd.print(secondMenuItems[i].value);
    }
    moveCursor(0);
    break;
  case 2:                           // Run-Menu
    for (uint8_t i = 0; i < 4; i++) // i = Number of menu items
    {
      lcd.setCursor(0, i);
      lcd.print(runMenuItems[i].label);
      lcd.print(runMenuItems[i].value);
    }
    break;
  }

  menuCounter = minMaxReset(menuCounter, menuCounterMin, menuCounterMax); // Reset menu selector cursor position when reaching either end
} // showMenu

// Moves the selector cursor up/down on the LCD, takes row# as argument
void moveCursor(uint8_t menuPos)
{
  menuPos = minMaxReset(menuPos, 0, (LCD_ROWS - 1)); // Make sure the cursor can't move past LCD row count

  for (uint8_t i = 0; i < LCD_ROWS; i++) // Clean the cursor rows, i = Number of menu items
  {
    lcd.setCursor(0, i); // Column 1, Row 1-4
    lcd.print(F(" "));
  }

  lcd.setCursor(0, menuPos); // Print a cursor on the LCD according to menuCounter's value, Column 1, Row 1-4
  lcd.print(cursor);         // Print an indicator showing current selection
} // moveCursor

// Starts the simulation
void run()
{
  runFinished = false;
  currentMenu = 3; // Setting menu to 3; for some reason it gets set to 0 if only specified in "handleSelection"...

  switch (dirSel)
  {
  case 0:                                 // "BOTH"
    lcdClearLn(3, 12, (LCD_COLUMNS - 1)); // Clear everything from row 2, column 12 and out
    dirSel = 1;                           // Set current direction to "UP"
    runMenuItems[3].value = dir[dirSel];  // Update current travel direction in menu items
    showMenu(2);                          // Refresh menu on LCD
    outputPulses(0);                      // "UP"
    delay(1000);                          // Using a small, blocking delay here because nothing else should occur between the changing of direction
    dirSel = 2;                           // Set current direction to "DOWN"
    runMenuItems[3].value = dir[dirSel];  // Update current travel direction in menu items
    showMenu(2);                          // Refresh menu on LCD
    outputPulses(1);                      // "DOWN"
    break;
  case 1:            // "UP"
    outputPulses(0); // "UP"
    break;
  case 2:            // "DOWN"
    outputPulses(1); // "DOWN"
    break;
  }

  if (runFinished)
  {
    dirSel = 1; // Reset selections/settings-related variables
    ppmSel = 1;
    distSel = 0;
    speedSel = 0;
    currentMenu = 0;
    menuCounter = -2;
    distElapsed = 0; // Reset elapsed distance
    BlockNot endBlinkTimer(1000);
    bool endBlinkState = false;
    lcd.setCursor(11, 3); lcd.print(F("N/A "));
    while (runFinished)
    {
      if (endBlinkTimer.TRIGGERED)
      {
        if (endBlinkState)
        {
          digitalWrite(OUTPUT_CH_A, LOW);
          digitalWrite(OUTPUT_CH_B, LOW);
          lcdClearLn(2, 11, (LCD_COLUMNS - 1)); // Clear everything from row 2, column 12 and out
          endBlinkState = false;
        }
        else if (!endBlinkState)
        {
          lcd.setCursor(11, 2); lcd.print(F("DONE!"));
          digitalWrite(OUTPUT_CH_A, HIGH);
          digitalWrite(OUTPUT_CH_B, HIGH);
          endBlinkState = true;
        }
      }
      checkEncoder();
      debug(); // TODO: Only for feedback during test, remove before production!
    }
  }

  if (nonBlockingTimer.TRIGGERED) // 150ms
  {
    debug();
  }

} // run

// Makes quadrature pulses in the specified direction. Takes int argument for direction: 0 = UP, 1 = DOWN
void outputPulses(const uint8_t travelDirection)
{
  runFinished = false;                                         // TODO: Only for test purposes, remove before production!
  unsigned int totalPulses = distances[distSel] * pulsesPerMeter[ppmSel]; // Total number of pulses to be generated
  unsigned int pulseCycleCount = 0;                                       // Count of pulses generated
  uint8_t pulseIntervalArrayIndex = 0;                                    // Index used to select the pulse interval from array
  const uint16_t pulseIntervalsArray[8] = {11718, 3906, 2343, 1171,       // Array of possible intervals (μs) between two pulses, derived from selected speed
                                           5859, 1953, 1171, 585};
  // Interval formula: 60'000'000 / (speed * pulses/meter) >> https://docs.google.com/spreadsheets/d/1iSJ3GRwYg6v-2EAzArJU7HahjW1eNBRwfr_UCq-fvGs/edit?usp=sharing

  switch (ppmSel) // Determine interval array index
  {
  case 1:                               // 512
    pulseIntervalArrayIndex = speedSel; // First 4 entries in interval array are for 512 pulses per meter
    break;
  case 0:                                   // 1024
    pulseIntervalArrayIndex = speedSel + 4; // Last 4 entries in interval array are for 1024 pulses per meter
    break;
  } // switch (ppmSel)

  uint8_t pulseSequenceFlag = 0;                                                 // Flag used to track current sequence (ab,Ab,AB,aB,ab etc..)
  const uint16_t pulseInterval = (pulseIntervalsArray[pulseIntervalArrayIndex]); // Interval: full period
  const uint16_t pulseQuarterInterval = (pulseInterval / 4);                     // Interval: 1/4 period
  BlockNot intervalTimer((pulseInterval), MICROSECONDS);                         // Set a microsecond timer, used to time the pulses
  BlockNot intervalQuarterTimer((pulseQuarterInterval), MICROSECONDS);           // Set a microsecond timer, used to time the pulses

  // Spit out some data before emitting pulses. // TODO: Only for test purposes, remove before production!
  Serial.print(F("> 'totalPulses'.....: ")); Serial.println(totalPulses);
  Serial.print(F("> 'Interval'........: ")); Serial.println((String)pulseInterval + "μs");
  Serial.print(F("> 'QuarterInterval'.: ")); Serial.println((String)pulseQuarterInterval + "μs");
  Serial.print(F("> 'currentMenu'.....: ")); Serial.println(currentMenu);

  digitalWrite(OUTPUT_CH_A, LOW); // Ensure both outputs are low before starting
  digitalWrite(OUTPUT_CH_B, LOW);

  // Generate pulses, good visualization here: https://www.usdigital.com/media/qktgqmkj/blog011-image007-pulse-multiplication.png?width=1000&height=660
  while (pulseCycleCount <= totalPulses && !runFinished)
  {
    switch (travelDirection)
    {
      case 0: // UP

        if (intervalQuarterTimer.TRIGGERED) // Check if the desired pulse interval has elapsed
        {
          switch (pulseSequenceFlag)
          {
            case 0:
              digitalWrite(OUTPUT_CH_A, HIGH);
              pulseSequenceFlag++;
              break;
            case 1:
              digitalWrite(OUTPUT_CH_B, HIGH);
              pulseSequenceFlag++;
              break;
            case 2:
              digitalWrite(OUTPUT_CH_A, LOW);
              pulseSequenceFlag++;
              break;
            case 3:
              digitalWrite(OUTPUT_CH_B, LOW);
              pulseSequenceFlag = 0;
              pulseCycleCount++;
              break;
          }
        }
      break;

      case 1: // DOWN

        if (intervalQuarterTimer.TRIGGERED) // Check if the desired pulse interval has elapsed
        {
          switch (pulseSequenceFlag)
          {
            case 0:
              digitalWrite(OUTPUT_CH_B, HIGH);
              pulseSequenceFlag++;
              break;
            case 1:
              digitalWrite(OUTPUT_CH_A, HIGH);
              pulseSequenceFlag++;
              break;
            case 2:
              digitalWrite(OUTPUT_CH_B, LOW);
              pulseSequenceFlag++;
              break;
            case 3:
              digitalWrite(OUTPUT_CH_A, LOW);
              pulseSequenceFlag = 0;
              pulseCycleCount++;
              break;
          }
        }
      break;
      default: break;
    }

/*
    if (intervalTimer.TRIGGERED) // Check if the pulse interval has fully elapsed
    {
      pulseCycleCount++;
    }
*/

    if (pulseCycleCount % pulsesPerMeter[ppmSel] == 0) // Increment meters traveled every 512 or 1024 pulse cycles
    {
      switch (travelDirection)
      {
      case 0: // UP
        distElapsed++;
        break;
      case 1: // DOWN
        distElapsed--;
        break;
      default: break;
      }
      runMenuItems[2].value = (String)distElapsed + "m ";
      showMenu(2);
    }

    if (nonBlockingTimer.TRIGGERED) // 150ms
    {
      debug();
      checkEncoder();
    }
  }

  runFinished = true; // Indicate that the run finished

} // outputPulses

// Runs continuously
void loop()
{
  

  if (nonBlockingTimer.TRIGGERED) // 150ms non-blocking timer
  {
    if (CHECK_ENCODER == true) // Has the encoder changed position?
    {
      showMenu(currentMenu); // Refresh menu upon encoder position change

      if (currentMenu == 0 || currentMenu == 1) // "If in main OR secondary menu..."
      {
        moveCursor(menuCounter); // Move cursor, it we're not inside the run-menu
      }
    }

    debug();
  }
} // Loop