// https://wokwi.com/projects/new/arduino-nano
#include <EEPROM.h>
#include <LiquidCrystal_PCF8574.h>

// function to reset arduino
// -> consider changing to hardware reset!
void(* resetFunc) (void) = 0;

LiquidCrystal_PCF8574 lcd(0x27); //lcd(0x3F);

// Rotary encoder pins
#define PIN_A       3
#define PIN_B       2
#define PUSH_BTN    4

#define BUZZER      5

// switch input pins
#define LIFT_PIN    9
#define START_PIN   10
#define CANCEL_PIN  11

//const int HOME_SCREEN = 0;
const int SETTINGS_SCREEN = 1;
const int SUBMENU_SCREEN = 2;
const int GAME_SCREEN = 3;
const int PRACTICE_SCREEN = 4;
const int TEST_SCREEN = 5;

int currentScreen;

int _BLANK, _MODE, _MOTORS, _HC12W, _BUZON, _LEDON;

// The Settings array is only to help save settings in individual variables.
int * Settings[] = {&_BLANK, &_MODE, &_MOTORS, &_HC12W, &_BUZON, &_LEDON};

// menu, submenu items tracking vars
int menu = 0, subMenu = 0;
byte last_menu = 0;
char prev_dir = '-';
static int8_t prev_rotationValue = 1;

bool btnPushed = false;
bool buzON = false;
bool RElongPress = false;
bool REshortPress = false;

const unsigned int LOWPITCH = 1350;
const unsigned int HIGHPITCH = 1450;
const unsigned long PITCHDURATION = 25;
const unsigned long LONGPITCHDURATION = 150;

unsigned long prev = 0, prevBat = 0;
unsigned long startTime = 0, t1 = 0, t2 = 0;
unsigned long totalTime = 0, prevTimers = 0;

bool timersStarted = false;
bool timersStopped = false;
bool timer1Started = false, timer2Started = false;
bool timer1Stopped = false, timer2Stopped = false;

int winner = 0;

char msg[40];
byte arrow = 126; // right arrow!

// Flag from interrupt routine (moved=true)
volatile bool rotaryEncoder = false;

String settingsMenu[][2] ={
  {"Settings"},
  {" Select Mode        ", " Game: Practice: Test Targets"}, //Practice == Training
  {" Motors Control     ", " Auto: OFF: Auto 1: Auto 2"},
  {" Wireless Control   ", " Tower: Server Room"},
  {" Sound Control      ", " ON: OFF"},
  {" LCD Light Control  ", " ON: OFF"}
};

const int MAX_MENU_ITEMS = sizeof(settingsMenu) / sizeof(settingsMenu[0]);

const byte subMenuItems[MAX_MENU_ITEMS] = {0,3,4,2,2,2};

// settings parameters to be saved...
// each setting menu corresponds to EEPROM location.
// each data represents the actual sub menu setting.
// e.g: {settingsParameters[5] = 3; } means:
// Motors Control (address 5) holds setting Auto 1 (data 3).
int settingsParameters[MAX_MENU_ITEMS];

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  lcd.begin(20,4);
  
  lcd.clear();
  lcd.setCursor(0,1);
  lcd.print("    Initialising    ");
  
  
  // get battery voltage and display in LCD
  int bat = analogRead(A0);

  float v = bat/100.0;

  lcd.setCursor(0,2);
  lcd.print("Bat. voltage: ");
  lcd.print(v);lcd.print("v");

  // The encoder module already has pullup resistors on board
  pinMode(PIN_A, INPUT);
  pinMode(PIN_B, INPUT);
  pinMode(PUSH_BTN, INPUT_PULLUP);

  pinMode(LIFT_PIN, INPUT_PULLUP);
  pinMode(START_PIN, INPUT_PULLUP);
  pinMode(CANCEL_PIN, INPUT_PULLUP);
  
  /* ----- read / set / apply all settings from EEPROM ----- */
  ReadSettings();
  ApplySettings(1);
  
  delay(1250);

  // We need to monitor both pins, rising and falling for all states
  attachInterrupt(digitalPinToInterrupt(PIN_A), rotary, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_B), rotary, CHANGE);

  delay(250);

  printHomeScreen();
  menu = 0;

  Serial.println("Ready");
  if(_BUZON == 1) tone(BUZZER, 2500, 35);
} // end setup




void loop() {
  // Has rotary encoder moved?
  if(rotaryEncoder){
    // Get the movement (if valid)
    int8_t rotationValue = checkRotaryEncoder();

    // If valid movement (l=-1, r=1), do something
    if (rotationValue != 0){

      // RE button not pushed!
      btnPushed = false;
      //rotationCounter += rotationValue * 1;

      // are we in HomeScreen, settingsScreen, or submenuScreen?
      switch (currentScreen){
        case GAME_SCREEN:
          if(_BUZON == 1){
            rotationValue < 1 ? tone(BUZZER, HIGHPITCH, PITCHDURATION) :
            tone(BUZZER, LOWPITCH, PITCHDURATION);
          }
          menu = subMenu = 0;
          // rotationValue < 1 ? StartTimers() : StopTimers();
          break;
        case PRACTICE_SCREEN:
        case TEST_SCREEN:
          if(_BUZON == 1){
            rotationValue < 1 ? tone(BUZZER, HIGHPITCH, PITCHDURATION) :
            tone(BUZZER, LOWPITCH, PITCHDURATION);
          }
          menu = subMenu = 0;
          return;
          break;

        case SETTINGS_SCREEN:
          menu += rotationValue * 1;
          if(_BUZON == 1){
            rotationValue < 1 ? tone(BUZZER, HIGHPITCH, PITCHDURATION) : 
            tone(BUZZER, LOWPITCH, PITCHDURATION);
          }
          if(menu > MAX_MENU_ITEMS-1){
            if(_BUZON == 1) {
              noTone(BUZZER);
              tone(BUZZER, LOWPITCH, LONGPITCHDURATION);
            }
            menu = MAX_MENU_ITEMS-1;
          }
          if(menu < 1){
            if(_BUZON == 1) {
              noTone(BUZZER);
              tone(BUZZER, HIGHPITCH, LONGPITCHDURATION);
            }
            menu = 1;
          }
          prev_rotationValue = rotationValue;
          printMenu(rotationValue);
          break;

        case SUBMENU_SCREEN:
          subMenu += rotationValue * 1;
          if(_BUZON == 1){
            rotationValue < 1 ? tone(BUZZER, HIGHPITCH, PITCHDURATION) : 
            tone(BUZZER, LOWPITCH, PITCHDURATION);
          }
          if(subMenu > subMenuItems[menu]){
            if(_BUZON == 1) {
              noTone(BUZZER);
              tone(BUZZER, LOWPITCH, LONGPITCHDURATION);
            }
            subMenu = subMenuItems[menu];
          }
          if(subMenu < 1){
            if(_BUZON == 1) {
              noTone(BUZZER);
              tone(BUZZER, HIGHPITCH, LONGPITCHDURATION);
            }
            subMenu = 1;
          }
          scrollSubMenu();
          break;
      }      

      //doActiveWorks();

      // Serial.print(rotationValue < 1 ? "L" : "R");
      // Serial.println(menu);
    }
  }

  if(digitalRead(PUSH_BTN) == LOW){

    //doActiveWorks();
        
    btnPushed = true;
    RElongPress = false;
    REshortPress = true;
    
    // Wait until button released (Blocking call!)
    prev = millis();
    while(digitalRead(PUSH_BTN) == LOW && !RElongPress){
      /* --- long press --- */
      if(millis() - prev > 800){
        RElongPress = true;
        REshortPress = false;
      }
      delay(10);
    }

    // long press...
    if(RElongPress){
      if(_BUZON == 1) tone(BUZZER, 3800, 80);
      switch (currentScreen){
        case GAME_SCREEN:
        case PRACTICE_SCREEN:
        case TEST_SCREEN:
          lcd.clear();
          lcd.setCursor(0,1);
          lcd.print("     Resetting!     ");
          Serial.println("Long Press, RESETTING...!");
          delay(2000);
          resetFunc();
          break;
        case SETTINGS_SCREEN:
          switch (_MODE){
            case 1:
              currentScreen = GAME_SCREEN;
              break;
            case 2:
              currentScreen = PRACTICE_SCREEN;
              break;
            case 3:
              currentScreen = TEST_SCREEN;
              break;
          }
          printHomeScreen();
          menu = subMenu = 0;
          break;
        case SUBMENU_SCREEN:
          currentScreen = SETTINGS_SCREEN;
          subMenu = 0;
          printMenu(prev_rotationValue);
          break;
        // Serial.println("Going back to Home Screen!");
      }
    } // end longpress...

    // short press...
    if(REshortPress){
      if(_BUZON == 1) tone(BUZZER, 2400, 25);
      switch (currentScreen){
        case GAME_SCREEN:
        case PRACTICE_SCREEN:
        case TEST_SCREEN:
          // go to settingsMenu / menu screen
          // Serial.println("Going to Settings Screen!");
          menu = 1;
          printMenu(1);
          currentScreen = SETTINGS_SCREEN;
          break;
        case SETTINGS_SCREEN:
          // go to sub menu screen, and mark '>' current setting
          // subMenu = 1;
          subMenu = settingsParameters[menu];
          printSubMenu();
          scrollSubMenu();
          currentScreen = SUBMENU_SCREEN;
          break;
        case SUBMENU_SCREEN:
          // save the setting, in EEPROM, Controller and server!
          // apply the setting!
          SaveSettings();
          ApplySettings(0); // 0 = one setting at a time!
          
          // go to prev screen. If mode is set, then go to home screen!
          if(menu == 1){
            switch (_MODE){
              case 1:
                currentScreen = GAME_SCREEN;
                break;
              case 2:
                currentScreen = PRACTICE_SCREEN;
                break;
              case 3:
                currentScreen = TEST_SCREEN;
                break;
            }
            printHomeScreen();
            menu = subMenu = 0;
          }else{
            currentScreen = SETTINGS_SCREEN;
            subMenu = 0;
            printMenu(prev_rotationValue);
          }
          break;
      } // end switch currentScreen
    }else{
      while(digitalRead(PUSH_BTN) == LOW){
        delay(10);
      }
    } // end if REshortPress
  } // end if Encoder butten pressed


  if(timersStarted && currentScreen == GAME_SCREEN){
    if(millis() - prevTimers > 100){
      unsigned long totalTime = millis() - startTime;

      if(!timer2Stopped){
        t2 = totalTime;

        int t2inMS = (t2 / 10) % 100;
        int t2inS  = (t2 / 1000) % 60;
        int t2inM  = (t2 / 60000) % 60;

        lcd.setCursor(0,2);
        lcd.print(" ");
        if(t2inM < 10) lcd.print("0");
        lcd.print(t2inM);
        lcd.print(":");
        if(t2inS < 10) lcd.print("0");
        lcd.print(t2inS);
        lcd.print(":");
        if(t2inMS < 10) lcd.print("0");
        lcd.print(t2inMS);
        lcd.print("  ");
      }

      if(!timer1Stopped){
        t1 = totalTime;

        int t1inMS = (t1 / 10) % 100;
        int t1inS  = (t1 / 1000) % 60;
        int t1inM  = (t1 / 60000) % 60;
        
        lcd.setCursor(9,2);
        lcd.print("  ");
        if(t1inM < 10) lcd.print("0");
        lcd.print(t1inM);
        lcd.print(":");
        if(t1inS < 10) lcd.print("0");
        lcd.print(t1inS);
        lcd.print(":");
        if(t1inMS < 10) lcd.print("0");
        lcd.print(t1inMS);
        lcd.print(" ");
      }

      if(winner == 2){
        lcd.setCursor(0,2);
        lcd.write('*');
      }

      if(winner == 1){
        lcd.setCursor(19,2);
        lcd.write('*');
      }

      prevTimers = millis();
    }
    // t1 = t2 = millis();
    //lcd.print(" 00:00:00  00:00:00 ");
  }
  

  if(digitalRead(LIFT_PIN) == LOW){
    Serial.println("LIFT");
    delay(50);
    timer1Stopped = true;
    if(winner == 0) winner = 1;
    while(digitalRead(LIFT_PIN) == LOW)
      delay(10);
  } // end digitalRead(LIFT_PIN)

  if(digitalRead(START_PIN) == LOW){
    // unsigned int 
    Serial.println("STRT");
    delay(50);
    while(digitalRead(START_PIN) == LOW)
      delay(10);
    // send start command to Controller, then wait for start signal
    // Once start signal received, start the timer and show in LCD
    StartTimers();

  } // end digitalRead(START_PIN)

  if(digitalRead(CANCEL_PIN) == LOW){
    Serial.println("CANCEL");
    delay(50);
    timer2Stopped = true;
    if(winner == 0) winner = 2;
    while(digitalRead(CANCEL_PIN) == LOW)
      delay(10);
    
    // send cancel/abort command to Controller, then wait for aborted signal
    // once aborted signal received, stop timers
    //StopTimers();
  } // end digitalRead(CANCEL_PIN)


  // update battery & signal status every .. sec.
  if(millis() - prevBat > 500){
      // get battery voltage and display in LCD
    int bat = analogRead(A0);
    
    float v = bat/100.0;

    // char batV[2], batmV[4];
    
    // sprintf(batV, "%i", bat/100);
    // sprintf(batmV, ".%i", bat)

    lcd.setCursor(13,0);
    lcd.print(v);
    //lcd.print(bat/100);lcd.print(".");
    //lcd.print("10");
    lcd.print("v");


    prevBat = millis();
  }

} // end loop



void StartTimers(){
  // start and show timer in LCD home screen.
  startTime = millis();
  timersStarted = true;
  timer1Stopped = false;
  timer2Stopped = false;
  winner = 0;
} // end StartTimers()




void StopTimers(){
  // start and show timer in LCD home screen.
  timersStarted = false;

} // end StopTimers()



void clearLCD(){
  lcd.setCursor(0,0);
  lcd.print("                  ");
  lcd.setCursor(0,1);
  lcd.print("                    ");
  lcd.setCursor(0,2);
  lcd.print("                    ");
  lcd.setCursor(0,3);
  lcd.print("                    ");
}




void printHomeScreen(){
  switch (currentScreen){
    case GAME_SCREEN:
      //clearLCD();
      lcd.home();
      lcd.print("                  SB"); // HC signal and battery statuses are displayed in pos 19 & 20
      lcd.setCursor(0, 1);
      lcd.print("  Team 2    Team 1  ");
      lcd.setCursor(0,2);
      lcd.print(" 00:00:00  00:00:00 ");
      lcd.setCursor(0,3);
      // lcd.write(arrow); //right arrow!
      // lcd.write(byte(219)); //empty square!
      // lcd.write(char(255)); //filled square!
      lcd.print("                    ");
      break;
    
    case TEST_SCREEN:
      //clearLCD();
      lcd.home();
      lcd.print("0987654321        SB");
      lcd.setCursor(0, 1);
      lcd.print("            Target 1");
      lcd.setCursor(0,2);
      lcd.print("0987654321          ");
      lcd.setCursor(0,3);
      lcd.print("            Target 2");
      // lcd.write(arrow); //right arrow!
      // lcd.write(byte(219)); //empty square!
      // lcd.write(char(255)); //filled square!
      break;

    case PRACTICE_SCREEN:
      //clearLCD();
      lcd.home();
      lcd.print("                  SB");
      lcd.setCursor(0, 1);
      lcd.print("  Practice Screen   ");
      lcd.setCursor(0,2);
      lcd.print("                    ");
      lcd.setCursor(0,3);
      // lcd.write(arrow); //right arrow!
      // lcd.write(byte(219)); //empty square!
      // lcd.write(char(255)); //filled square!
      lcd.print("                    ");
      break;
  } // end switch currentScreen
} // end printHomeScreen()




void printMenu(int8_t rotationDir){
  // rotationDir: L=U=-1 / R=D=1

  if(currentScreen != SETTINGS_SCREEN){
    clearLCD();
    lcd.home();
    lcd.print(settingsMenu[0][0]);
  }

  if(rotationDir == 1){ // direction RIGHT = DOWN
    switch (menu){
      case 1:
        lcd.setCursor(0,1);lcd.print(settingsMenu[1][0]);
        lcd.setCursor(0,1);lcd.write(arrow); //right arrow!
        lcd.setCursor(0,2);lcd.print(settingsMenu[2][0]);
        lcd.setCursor(0,3);lcd.print(settingsMenu[3][0]);
        last_menu = 3;
        break;

      case 2:
        lcd.setCursor(0,1);lcd.print(settingsMenu[1][0]);
        lcd.setCursor(0,2);lcd.print(settingsMenu[2][0]);
        lcd.setCursor(0,2);lcd.write(arrow); //right arrow!
        lcd.setCursor(0,3);lcd.print(settingsMenu[3][0]);
        last_menu = 3;
        break;
      
      case 3:
        if(prev_dir == 'D'){
          if(last_menu == 4){
            lcd.setCursor(0,1);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,2);lcd.write(arrow); //right arrow!
            lcd.setCursor(0,3);lcd.print(settingsMenu[4][0]);
            last_menu = 4;
          }else{
            lcd.setCursor(0,1);lcd.print(settingsMenu[1][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,3);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,3);lcd.write(arrow); //right arrow!
            last_menu = 3;
          }
        }
        if(prev_dir == 'U'){
          if(last_menu == 4){
            lcd.setCursor(0,1);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,2);lcd.write(arrow); //right arrow!
            lcd.setCursor(0,3);lcd.print(settingsMenu[4][0]);
            last_menu = 4;
          }
          else{
            lcd.setCursor(0,1);lcd.print(settingsMenu[1][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,3);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,3);lcd.write(arrow); //right arrow!
            last_menu = 3;
          }
        }
        break;

      case 4:
        if(prev_dir == 'D'){
          if(last_menu == 5){
            lcd.setCursor(0,1);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[4][0]);
            lcd.setCursor(0,2);lcd.write(arrow); //right arrow!
            lcd.setCursor(0,3);lcd.print(settingsMenu[5][0]);
            last_menu = 5;
          }else{
            lcd.setCursor(0,1);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,3);lcd.print(settingsMenu[4][0]);
            lcd.setCursor(0,3);lcd.write(arrow); //right arrow!
            last_menu = 4;
          }
        }
        if(prev_dir == 'U'){
          if(last_menu == 4){
            lcd.setCursor(0,1);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,3);lcd.print(settingsMenu[4][0]);
            lcd.setCursor(0,3);lcd.write(arrow); //right arrow!
            last_menu = 4;
          }else{
            lcd.setCursor(0,1);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[4][0]);
            lcd.setCursor(0,2);lcd.write(arrow); //right arrow!
            lcd.setCursor(0,3);lcd.print(settingsMenu[5][0]);
            last_menu = 5;
          }
        }
        break;

      case 5:
        lcd.setCursor(0,1);lcd.print(settingsMenu[3][0]);
        lcd.setCursor(0,2);lcd.print(settingsMenu[4][0]);
        lcd.setCursor(0,3);lcd.print(settingsMenu[5][0]);
        lcd.setCursor(0,3);lcd.write(arrow); //right arrow!
        last_menu = 5;
        break;
    } // end switch

    prev_dir = 'D';
    // lcd.setCursor(15,0);
    // lcd.print(menu);

    // lcd.setCursor(18,1);
    // lcd.write(prev_dir);

    // lcd.setCursor(19,1);
    // lcd.print(last_menu);

  } // end if rotationDir == DOWN

  if(rotationDir == -1){ // direction LEFT = UP
    switch (menu){
      case 1:
        lcd.setCursor(0,1);lcd.print(settingsMenu[1][0]);
        lcd.setCursor(0,1);lcd.write(arrow);
        lcd.setCursor(0,2);lcd.print(settingsMenu[2][0]);
        lcd.setCursor(0,3);lcd.print(settingsMenu[3][0]);
        last_menu = 3;
        break;

      case 2:
        if(prev_dir == 'D'){
          if(last_menu == 4){
            lcd.setCursor(0,1);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,1);lcd.write(arrow);
            lcd.setCursor(0,2);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,3);lcd.print(settingsMenu[4][0]);
            last_menu = 4;
          }
          else{
            lcd.setCursor(0,1);lcd.print(settingsMenu[1][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,2);lcd.write(arrow);
            lcd.setCursor(0,3);lcd.print(settingsMenu[3][0]);
            last_menu = 3;
          }
        }
        if(prev_dir == 'U'){
          if(last_menu == 3){
            lcd.setCursor(0,1);lcd.print(settingsMenu[1][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,2);lcd.write(arrow);
            lcd.setCursor(0,3);lcd.print(settingsMenu[3][0]);
            last_menu = 3;
          }else{
            lcd.setCursor(0,1);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,1);lcd.write(arrow);
            lcd.setCursor(0,2);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,3);lcd.print(settingsMenu[4][0]);
            last_menu = 4;
          }
        }
        break;
      
      case 3:
        if(prev_dir == 'D'){
          if(last_menu == 5){
            lcd.setCursor(0,1);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,1);lcd.write(arrow);
            lcd.setCursor(0,2);lcd.print(settingsMenu[4][0]);
            lcd.setCursor(0,3);lcd.print(settingsMenu[5][0]);
            last_menu = 5;
          }
          else{
            lcd.setCursor(0,1);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,2);lcd.write(arrow);
            lcd.setCursor(0,3);lcd.print(settingsMenu[4][0]);
            last_menu = 4;
          }
        }
        if(prev_dir == 'U'){
          if(last_menu == 4){
            lcd.setCursor(0,1);lcd.print(settingsMenu[2][0]);
            lcd.setCursor(0,2);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,2);lcd.write(arrow);
            lcd.setCursor(0,3);lcd.print(settingsMenu[4][0]);
            last_menu = 4;
          }else{
            lcd.setCursor(0,1);lcd.print(settingsMenu[3][0]);
            lcd.setCursor(0,1);lcd.write(arrow);
            lcd.setCursor(0,2);lcd.print(settingsMenu[4][0]);
            lcd.setCursor(0,3);lcd.print(settingsMenu[5][0]);
            last_menu = 5;
          }
        }
        break;

      case 4:
        lcd.setCursor(0,1);lcd.print(settingsMenu[3][0]);
        lcd.setCursor(0,2);lcd.print(settingsMenu[4][0]);
        lcd.setCursor(0,2);lcd.write(arrow);
        lcd.setCursor(0,3);lcd.print(settingsMenu[5][0]);
        last_menu = 5;
        break;

      case 5:
        lcd.setCursor(0,1);lcd.print(settingsMenu[3][0]);
        lcd.setCursor(0,2);lcd.print(settingsMenu[4][0]);
        lcd.setCursor(0,3);lcd.print(settingsMenu[5][0]);
        lcd.setCursor(0,3);lcd.write(arrow);
        last_menu = 5;
        break;
    } // end switch

    prev_dir = 'U';    
    // lcd.setCursor(15,0);
    // lcd.print(menu);

    // lcd.setCursor(18,1);
    // lcd.write(prev_dir);

    // lcd.setCursor(19,1);
    // lcd.print(last_menu);

  } // end if rotationDir == UP
}


void printSubMenu(){
  if(currentScreen != SUBMENU_SCREEN){
    clearLCD();
    lcd.home();
    lcd.print(settingsMenu[0][0]);
    lcd.setCursor(0,1);
    lcd.print(settingsMenu[menu][0]);
  }

  Serial.print('"');
  Serial.print(settingsMenu[menu][1]);
  Serial.println("\"");

  int to = settingsMenu[menu][1].indexOf(':');
  int from = 0;

  // sprintf(msg, " - x = %i\tidx = %i", x, idx);
  // Serial.println(msg);


  switch (menu){
    case 1:
      // Select Mode "Game: Practice: Test Targets"
      lcd.setCursor(0,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(0,2);lcd.write(arrow);

      from = to+1;
      to = settingsMenu[menu][1].indexOf(':', to+2);
      lcd.setCursor(11,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(11,2);lcd.write(' ');

      from = to+1;
      to = settingsMenu[menu][1].indexOf(':', to+2);
      lcd.setCursor(0,3);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(0,3);lcd.write(' ');


      // while(from > -1){
      //   Serial.print('"');
      //   Serial.print(settingsMenu[menu][1].substring(from, to));
      //   Serial.print("\", ");
        
      //   from = to+1;
      //   to = settingsMenu[menu][1].indexOf(':', to+2);
      //   if(from == 0) from = -1;

      //   // sprintf(msg, " - x = %i\tidx = %i\t", x, idx);
      //   // Serial.println(msg);
      // }
      // Serial.println();
      break;

    case 2:
      // Motors Control " Auto: OFF: Auto 1: Auto 2"
      lcd.setCursor(0,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(0,2);lcd.write(arrow);

      from = to+1;
      to = settingsMenu[menu][1].indexOf(':', to+2);
      lcd.setCursor(13,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(13,2);lcd.write(' ');

      from = to+1;
      to = settingsMenu[menu][1].indexOf(':', to+2);
      lcd.setCursor(0,3);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(0,3);lcd.write(' ');

      from = to+1;
      to = settingsMenu[menu][1].indexOf(':', to+2);
      lcd.setCursor(13,3);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(13,3);lcd.write(' ');
      break;

    case 3:
      // Wireless Control " Tower: Server Room"
      lcd.setCursor(0,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(0,2);lcd.write(arrow);

      from = to+1;
      to = settingsMenu[menu][1].indexOf(':', to+2);
      lcd.setCursor(8,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(8,2);lcd.write(' ');
      break;

    case 4:
      // Sound Control " ON: OFF"
      lcd.setCursor(0,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(0,2);lcd.write(arrow);

      from = to+1;
      to = settingsMenu[menu][1].indexOf(':', to+2);
      lcd.setCursor(16,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      // lcd.setCursor(16,2);lcd.write(' ');
      break;

    case 5:
      // LCD Light Control " ON: OFF"
      lcd.setCursor(0,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      //lcd.setCursor(0,2);lcd.write(arrow);

      from = to+1;
      to = settingsMenu[menu][1].indexOf(':', to+2);
      lcd.setCursor(16,2);
      lcd.print(settingsMenu[menu][1].substring(from, to));
      //lcd.setCursor(16,2);lcd.write(' ');
      break;
  }
} // end printSubMenu



void scrollSubMenu(){
  switch (menu){
    case 1:
      switch (subMenu){
        case 1:
          lcd.setCursor(0,2);lcd.write(arrow);
          lcd.setCursor(11,2);lcd.write(' ');
          lcd.setCursor(0,3);lcd.write(' ');
          break;
        case 2:
          lcd.setCursor(0,2);lcd.write(' ');
          lcd.setCursor(11,2);lcd.write(arrow);
          lcd.setCursor(0,3);lcd.write(' ');
          break;
        case 3:
          lcd.setCursor(0,2);lcd.write(' ');
          lcd.setCursor(11,2);lcd.write(' ');
          lcd.setCursor(0,3);lcd.write(arrow);
          break;
      }
      break;
      
    case 2:
      switch (subMenu){
        case 1:
          lcd.setCursor(0,2);lcd.write(arrow);
          lcd.setCursor(13,2);lcd.write(' ');
          lcd.setCursor(0,3);lcd.write(' ');
          lcd.setCursor(13,3);lcd.write(' ');
          break;
        case 2:
          lcd.setCursor(0,2);lcd.write(' ');
          lcd.setCursor(13,2);lcd.write(arrow);
          lcd.setCursor(0,3);lcd.write(' ');
          lcd.setCursor(13,3);lcd.write(' ');
          break;
        case 3:
          lcd.setCursor(0,2);lcd.write(' ');
          lcd.setCursor(13,2);lcd.write(' ');
          lcd.setCursor(0,3);lcd.write(arrow);
          lcd.setCursor(13,3);lcd.write(' ');
          break;
        case 4:
          lcd.setCursor(0,2);lcd.write(' ');
          lcd.setCursor(13,2);lcd.write(' ');
          lcd.setCursor(0,3);lcd.write(' ');
          lcd.setCursor(13,3);lcd.write(arrow);
          break;
      }
      break;

    case 3:
      switch (subMenu){
        case 1:
          lcd.setCursor(0,2);lcd.write(arrow);
          lcd.setCursor(8,2);lcd.write(' ');
          break;
        case 2:
          lcd.setCursor(0,2);lcd.write(' ');
          lcd.setCursor(8,2);lcd.write(arrow);
          break;
      }
      break;

    case 4:
      switch (subMenu){
        case 1:
          lcd.setCursor(0,2);lcd.write(arrow);
          lcd.setCursor(16,2);lcd.write(' ');
          break;
        case 2:
          lcd.setCursor(0,2);lcd.write(' ');
          lcd.setCursor(16,2);lcd.write(arrow);
          break;
      }
      break;
    
    case 5:
      switch (subMenu){
        case 1:
          lcd.setCursor(0,2);lcd.write(arrow);
          lcd.setCursor(16,2);lcd.write(' ');
          break;
        case 2:
          lcd.setCursor(0,2);lcd.write(' ');
          lcd.setCursor(16,2);lcd.write(arrow);
          break;
      }
      break;
  }
} // end scrollSubMenu()




// Interrupt routine just sets a flag when rotation is detected
void rotary(){
  rotaryEncoder = true;
}



// Rotary encoder has moved (interrupt tells us) but what happened?
// See https://www.pinteric.com/rotary.html
int8_t checkRotaryEncoder(){
  // Reset the flag that brought us here (from ISR)
  rotaryEncoder = false;

  static uint8_t lrmem = 3;
  static int lrsum = 0;
  static int8_t TRANS[] = {0, -1, 1, 14, 1, 0, 14, -1, -1, 14, 0, 1, 14, 1, -1, 0};

  // Read BOTH pin states to deterimine validity of rotation (ie not just switch bounce)
  int8_t l = digitalRead(PIN_A);
  int8_t r = digitalRead(PIN_B);

  // Move previous value 2 bits to the left and add in our new values
  lrmem = ((lrmem & 0x03) << 2) + 2 * l + r;

  // Convert the bit pattern to a movement indicator (14 = impossible, ie switch bounce)
  lrsum += TRANS[lrmem];

  /* encoder not in the neutral (detent) state */
  if (lrsum % 4 != 0){
      return 0;
  }

  /* encoder in the neutral state - clockwise rotation*/
  if (lrsum == 4){
      lrsum = 0;
      return 1;
  }

  /* encoder in the neutral state - anti-clockwise rotation*/
  if (lrsum == -4){
      lrsum = 0;
      return -1;
  }

  // An impossible rotation has been detected - ignore the movement
  lrsum = 0;
  return 0;
}


// Read and Apply the settings
void ReadSettings(){
  for(int i = 1; i < MAX_MENU_ITEMS; i++){
    // read setting from EEPROM, save in array, then
    // check if EEPROM location is new or unset(255) -> 
    //    set to first setting (subMenu 1).
    // apply the setting.

    if((settingsParameters[i] = (int)EEPROM.read(i)) == 255){
      settingsParameters[i] = 1; // default setting == ON
      EEPROM.update(i, settingsParameters[i]);
      // delay(10); // give some time to complete EEPROM instruction.
    }

    *Settings[i] = settingsParameters[i];
      
    // sprintf(msg, "Address %i , Data %i, Setting %i.", i, settingsParameters[i], *Settings[i]);
    // Serial.println(msg);
  }
} //ReadSettings()




// Save and Apply the settings
void SaveSettings(){
  // sprintf(msg, "Saving settings for menu %i, submenu %i.", menu, subMenu);
  // Serial.println(msg);
  settingsParameters[menu] = subMenu;
  *Settings[menu] = settingsParameters[menu];
  EEPROM.update(menu, subMenu);
} // end SaveSettings()



void ApplySettings(bool all){
  switch (all){
    case 1: // apply all settings..
      /* --- HC12 Module settings --- */
      setHC12();

      /* --- LCD Backlight LED --- */
      if(_LEDON == 1) lcd.setBacklight(1);
      else lcd.setBacklight(0);

      /* --- Mode (Home Screen) settings --- */
      switch (_MODE){
        case 1: // GAME
          currentScreen = GAME_SCREEN;
          break;
        case 2: // Practice
          currentScreen = PRACTICE_SCREEN;
          break;
        case 3: // Test Targets
          currentScreen = TEST_SCREEN;
          break;      
      }
      break;
    
    case 0: // apply individual setting using 'menu'
      switch (menu){
        case 1:
          /* --- Mode (Home Screen) settings --- */
          switch (_MODE){
            case 1: // GAME
              currentScreen = GAME_SCREEN;
              break;
            case 2: // Practice
              currentScreen = PRACTICE_SCREEN;
              break;
            case 3: // Test Targets
              currentScreen = TEST_SCREEN;
              break;      
          }
          break;
        
        case 2:
          /* --- Motor Control settings --- */
          break;

        case 3:
          /* --- HC12 Wirelss settings --- */
          setHC12();
          break;

        case 4:
          /* --- Sound settings --- */
          // applied immediately...!
          break;

        case 5:
          /* --- LCD Backlight settings --- */
          /* --- LCD Backlight LED --- */
          if(_LEDON == 1) lcd.setBacklight(1);
          else lcd.setBacklight(0);
          break;
      }
      break;
  }
} // end ApplySettings()




void setHC12(){
  switch (_HC12W){
    case 1: // Tower
      // set HC12 channel for Tower i.e. channel 010?
      Serial.println("HC12 is set to Tower!");
      break;
    
    case 2: // Server Room
      // set HC12 channel for Server Room i.e. channel 020?
      Serial.println("HC12 is set to Server Room!");
      break;
  }
} // end setHC12()