#include <Arduino.h>

#define BUTTON_PIN 12

// To store the last time mode was printed
unsigned long lastPrintTime = 0;

enum MODES
{
  REMOTE_CONTROLLED, // Bluetooth (mobile) or remote controller
  LINE_FOLLOWER,
  SUMO,
  HUMAN_FOLOWER,
};
int mode, oldMode;

void setupMode()
{
  // change mode on button press
  if (readButton(BUTTON_PIN))
  {
    mode++;
    if (mode > HUMAN_FOLOWER)
    {
      mode = REMOTE_CONTROLLED;
    }
  }

  // change mode on serial input or bluetooth command
  if (Serial.available())
  {
    char input = Serial.read();
    if (input >= '0' && input <= '3')
    {
      mode = input - '0';
      // ASCCI table:
      // '0' = 48
      // '1' = 49
      // '2' = 50
      // '3' = 51

      // mode = '0' - '0' = 0 = REMOTE_CONTROLLED
      // mode = 48 -48 = 0 = REMOTE_CONTROLLED
      // mode = '1' - '0' = 1 = LINE_FOLLOWER
      // mode = '2' - '0' = 2 = SUMO
      // mode = '3' - '0' = 3 = HUMAN_FOLOWER
    }
  }
}

void printMode()
{
  Serial.print("Current mode: ");
  switch (mode)
  {
  case REMOTE_CONTROLLED:
    Serial.println("REMOTE_CONTROLLED");
    break;
  case LINE_FOLLOWER:
    Serial.println("LINE_FOLLOWER");
    break;
  case SUMO:
    Serial.println("SUMO");
    break;
  case HUMAN_FOLOWER:
    Serial.println("HUMAN_FOLOWER");
    break;
  default:
    Serial.println("UNKNOWN");
    break;
  }
}

bool readButton(int pin)
{
  bool buttonState = !digitalRead(pin);

  if (buttonState)
    delay(100); // Debounce delay

  // Serial.print("Button state: ");
  // Serial.println(buttonState ? "PRESSED" : "RELEASED");

  return buttonState;
}

void setup()
{
  Serial.begin(9600);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  mode = REMOTE_CONTROLLED; // Default mode
  printMode();
}

void loop()
{
  setupMode();

  // if (millis() - lastPrintTime >= 1000)
  // { // Print mode every second
  //   printMode();
  //   lastPrintTime = millis();
  // }

  if (mode != oldMode)
  {
    printMode();
  }

  oldMode = mode;
}