/*
  Trying out code that can be used for serial commands and/or debugging in future projects
*/

#include <BlockNot.h>   // Used in pseudo-code...
BlockNot timer1(470);   // Used in pseudo-code...
BlockNot timer2(1550);  // Used in pseudo-code...
BlockNot timer3(3020);  // Used in pseudo-code...
BlockNot timer4(7530);  // Used in pseudo-code...

// Serial
#define SERIAL_BAUD_RATE 115200 // Serial monitor speed
#define NUMBER_OF_COMMANDS 12   // Total number of commands/size of serial command buffer
#define COMMAND_MAX_LENGTH 24   // Max number of serial command characters

long distElapsed = 0;

// Pseudo variables
int8_t menuCounter = -1;                // Used in pseudo-code...
int8_t currentMenu = 0;                 // Used in pseudo-code...

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

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

SerialCommand serialCommands[NUMBER_OF_COMMANDS] = { // Serial commands array
  {"help", " - Shows all available commands"},
  {"quit", " - Returns to main menu"},
  {"poll", " - Displays multiple variables' current values"},
  {"menucounter", " - Displays menuCounter's current value"},
  {"currentmenu", " - Displays currentMenu's current value"},
  {"splash", " - Blinks LED and have fun"},
};

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

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

// Compare serial input with command array, return true if matching, false otherwise
bool is_valid_command(const char *command)
{
  for (int 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

// Execute code based on received command
void handle_command(const char *command)
{
  Serial.println("> Command \"" + (String)command + "\" received..."); // Echo last 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
  {
    Serial.println(F("> ")); // TODO: If running: quit, show main menu
  }
  else if (strcmp(command, serialCommands[2].cmd) == 0) // "poll" command
  {
    Serial.println(F("> TODO: Print all variables' values: ppmSel, distSel, speedSel, dirSel")); // TODO: Print all variables' values
    Serial.print(F("> \"menuCounter\" current value: "));
    Serial.println(menuCounter);
    Serial.print(F("> \"currentMenu\" current value: "));
    Serial.println(currentMenu);
    Serial.print(F("> \"distElapsed\" current value: "));
    Serial.println(distElapsed);
  }
  else if (strcmp(command, serialCommands[3].cmd) == 0) // "menucounter" command
  {
    Serial.print(F("> \"menuCounter\" current value: "));
    Serial.print(menuCounter);
  }
  else if (strcmp(command, serialCommands[4].cmd) == 0) // "currentmenu" command
  {
    Serial.print(F("> \"currentMenu\" current value: "));
    Serial.print(currentMenu);
  }
  else if (strcmp(command, serialCommands[5].cmd) == 0) // "splash" command
  {
    splashScreen(500, 500);
  }
} // handle_command --> Used in debug function

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

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

void splashScreen(const unsigned int splashDuration, const unsigned int splashBlinkDelay) // Pseudo function...
{
  delay(splashDuration); // splashDuration = For how many ms splash graphics is displayed
  Serial.println(F("> Do the splash!"));
  // Blink LEDs on startup "splash screen"
  uint8_t initBlink = 0;
  while (initBlink < 5)
  {
    digitalWrite(LED_BUILTIN, HIGH);
    delay(splashBlinkDelay);
    digitalWrite(LED_BUILTIN, LOW);
    Serial.println(F("> Splashing!"));
    delay(splashBlinkDelay);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(splashBlinkDelay);
    digitalWrite(LED_BUILTIN, LOW);
    initBlink++;
  }
  Serial.println(F("> OK, now I'm done! xD"));
} // splashScreen

void loop() {
  // Pseudo-code, simulate arduino doing stuff
  if (timer1.TRIGGERED) {
    menuCounter++;
    digitalWrite(LED_BUILTIN, HIGH);
  }

  if (timer2.TRIGGERED) {
    currentMenu++;
    digitalWrite(LED_BUILTIN, LOW);
  }

  if (timer3.TRIGGERED) {
    distElapsed += 100;
    digitalWrite(LED_BUILTIN, LOW);
  }

  if (timer4.TRIGGERED) {
    Serial.println(F("> Still alive!"));
    distElapsed -= menuCounter;
  }

  if (menuCounter > 199) {
    menuCounter = 5;
  }

  if (currentMenu > 199) {
    currentMenu = 3;
  }

  if (distElapsed < -999) {
    distElapsed = 999;
  }
  else if (distElapsed > 19999) {
    distElapsed = currentMenu;
  }

  // Test serial commands
  debug();
} // Loop