// Line counter
static const int handleCredits_codeLinesCounter_firstLine = __LINE__;
// CoolTerm Settings for HC-06
  // Port (generally) : COM5 (Standard Serial over Bluetooth link)
  // Baud rate : 9600
  // Data Bits : 8
  // Parity : None
  // Stop bits : 1
  // Flow Control : None
  // Software Supported Flow Control : TRUE
  // Block Keystrokes while flow is halted : TRUE
  // Initial Line States when port opens : DTR On : TRUE / RTS On : TRUE
// Libraries
  // New libraries
#include <Keypad.h> // "Keypad" by Marl Stanley, Alexander Brevig.
#include <LiquidCrystal_I2C.h>  // "LiquidCrystal I2C" by Frank de Brabander.
  // Built-in libraries
#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include <EEPROM.h>
// Pinout
  // Arduino MEGA 2560
  // L298N
#define servoMotor 4
#define motors_ENA 10
#define motors_ENB 11
#define buzzer 49
#define ultrasonicSensor_trigger 43
#define ultrasonicSensor_echo 44
#define motors_IN1 50
#define motors_IN2 51
#define motors_IN3 52
#define motors_IN4 53
#define LED_red 7
#define LED_green 8
#define LED_blue 12
#define joystick_button 41
#define joystick_VRY A8
#define joystick_VRX A12
#define potentiometer A13
// Definitions
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo myServo;
// Components
int sevenSegmentPin[7] = {22, 23, 24, 25, 26, 27, 28};
byte keypadRowPin[4] = {35, 36, 37, 38};
byte keypadColumnPin[4] = {34, 32, 31, 30};
char keypadKey[4][4] = {
    {'1', '2', '3', 'A'},
    {'4', '5', '6', 'B'},
    {'7', '8', '9', 'C'},
    {'*', '0', '#', 'D'}};
Keypad keypad = Keypad(makeKeymap(keypadKey), keypadRowPin, keypadColumnPin, 4, 4);
// Function Definitions
void updateMovementState(String state, bool forceUpdate = false);
// DC Motors
void motorsSpeed();
void motorsForward();
void motorsBackward();
void motorsLeft();
void motorsRight();
void motorsStop();
// Buzzer
void buzzer_activeBeep(int duration = 120);
// Ultrasonic Sensor
long ultrasonicSensor_measureDistance();
// Reset mode
void handleReset(int handleReset_delay = 50);
// Emergency mode
void handleEmergency();
void handleEmergency_buzzer_passiveBeep();
// Bluetooth mode
void handleBluetooth();
// Maze mode
void handleMaze();
void handleMaze_motorsTurn(String direction);
void handleMaze_buzzer_passiveBeep();
// Morse Communication mode
void handleMorseCommunication();
void handleMorseCommunication_element(char element);
void handleMorseCommunication_character(char character);
// Follow Me mode
void handleFollowMe();
// Dice mode
void handleDice();
// Credits mode
void handleCredits();
void handleCredits_servoMotorSweep();
// Chrome Dinausor Game mode
void handleChromeDinausorGame();
// Stopwatch And Timer mode
void handleStopwatchAndTimer();
// Calculator mode
void handleCalculator();
// Simon Says mode
void handleSimonSays();
// Sleepy Radar mode
void handleSleepyRadar();
// Melodies mode
void handleMelodies();
// Reaction mode
void handleReaction();
// Off mode
void handleOFF_buzzer_passiveBeep();
void handleOff();
// Manual Test mode
void handleManualTest(char customKey);
// Automatic Test mode
void handleAutomaticTest();
// Joystick Test mode
void handleJoystickTest();
// LCD
void LCD_displayMessageCentered(String msg, int row);
void LCD_welcomeMessage();
void LCD_welcomeMessage_buzzer_passiveBeep();
void LCD_menuMessage_buzzer_passiveBeep();
void LCD_welcomeMenuMessage_buzzer_passiveBeep();
void LCD_menuMessage();
void LCD_menuMessage_animation();
// 7 Segments Display
void sevenSegment_clearAllSegments();
void sevenSegment_animation();
void sevenSegment_displayDigit(int digit);
void sevenSegment_displayLetter(char letter);
// Modes
enum Mode
{
  NONE,
  BLUETOOTH,
  MAZE,
  TESTING,
  CREDITS,
  FOLLOW_ME,
  MAN_TST,
  MORSE_COM,
  JOYSTICK_TEST,
  DICE,
  CHROME_DINAUSOR_GAME,
  STOPWATCH_AND_TIMER,
  CALCULATOR,
  SIMON_SAYS,
  SLEEPY_RADAR,
  MELODIES,
  REACTION,
  OFF
};
enum SubModeState {
  SUBMODE_SELECT,
  STOPWATCH_RUNNING,
  TIMER_CONFIG,
  TIMER_RUNNING,
  TIMER_FINISHED
};
enum Mode_noneState
{
  NONE_STATE_WAITING,
  NONE_STATE_BT_PRESSED_FIRST
};
enum handleMaze_followState
{
  FOLLOW_STATE_WAITING,
  FOLLOW_STATE_FOLLOWING
};
enum handleMaze_mazeState
{
  MAZE_STATE_MOVING_FORWARD,
  MAZE_STATE_OBSTACLE_DETECTED,
  MAZE_STATE_SCANNING,
  MAZE_STATE_TURNING_LEFT,
  MAZE_STATE_TURNING_RIGHT,
  MAZE_STATE_TURNING_AROUND,
};
enum handleAutomaticTest_testState
{
  TEST_STATE_INIT,
  TEST_STATE_FORWARD,
  TEST_STATE_BACKWARD,
  TEST_STATE_TURN_LEFT,
  TEST_STATE_TURN_RIGHT,
  TEST_STATE_SERVO_LEFT,
  TEST_STATE_SERVO_CENTER,
  TEST_STATE_SERVO_RIGHT,
  TEST_STATE_CYCLE_COMPLETE
};
// Logic variables
  // Integers
int menuPage = 1;
int btBuzzerPeriod = 0; 
int FOLLOW_TOLERANCE_CM;
int EEPROMHighScore = 0;
int EEPROMHighScoreSimon = 200;
int EEPROMHighScoreReactionMinutes = 10;
int EEPROMHighScoreReactionSeconds = 15;
int EEPROMHighScoreReactionMilliseconds = 20;
int EEPROMVIN = 100;
int HighScore = 0;
int HighScoreSimon = 0;
int HighScoreReactionMinutes = 0;
int HighScoreReactionSeconds = 0;
int HighScoreReactionMilliseconds = 0;
int servoPos = 90;
int ledBrightness = 0;
int fadeAmount = 2;
int lcdAnimationPhase = 0;
int ensureSingleModeAccess = 1;
int currentCreditMessage = 0;
int morseRepeater = 1;
int servoPosition = 0;
int servoDirection = 1;
const int NOTE_REST = 0;
const int NOTE_C0 = 16;
const int NOTE_CS0 = 17;
const int NOTE_D0 = 18;
const int NOTE_DS0 = 19;
const int NOTE_E0 = 21;
const int NOTE_F0 = 22;
const int NOTE_FS0 = 23;
const int NOTE_G0 = 25;
const int NOTE_GS0 = 26;
const int NOTE_A0 = 28;
const int NOTE_AS0 = 29;
const int NOTE_B0 = 31;
const int NOTE_C1 = 33;
const int NOTE_CS1 = 35;
const int NOTE_D1 = 37;
const int NOTE_DS1 = 39;
const int NOTE_E1 = 41;
const int NOTE_F1 = 44;
const int NOTE_FS1 = 46;
const int NOTE_G1 = 49;
const int NOTE_GS1 = 52;
const int NOTE_A1 = 55;
const int NOTE_AS1 = 58;
const int NOTE_B1 = 62;
const int NOTE_C2 = 65;
const int NOTE_CS2 = 69;
const int NOTE_D2 = 73;
const int NOTE_DS2 = 78;
const int NOTE_E2 = 82;
const int NOTE_F2 = 87;
const int NOTE_FS2 = 93;
const int NOTE_G2 = 98;
const int NOTE_GS2 = 104;
const int NOTE_A2 = 110;
const int NOTE_AS2 = 117;
const int NOTE_B2 = 123;
const int NOTE_C3 = 131;
const int NOTE_CS3 = 139;
const int NOTE_D3 = 147;
const int NOTE_DS3 = 156;
const int NOTE_E3 = 165;
const int NOTE_F3 = 175;
const int NOTE_FS3 = 185;
const int NOTE_G3 = 196;
const int NOTE_GS3 = 208;
const int NOTE_A3 = 220;
const int NOTE_AS3 = 233;
const int NOTE_B3 = 247;
const int NOTE_C4 = 262;
const int NOTE_CS4 = 277;
const int NOTE_D4 = 294;
const int NOTE_DS4 = 311;
const int NOTE_E4 = 330;
const int NOTE_F4 = 349;
const int NOTE_FS4 = 370;
const int NOTE_G4 = 392;
const int NOTE_GS4 = 415;
const int NOTE_A4 = 440;
const int NOTE_AS4 = 466;
const int NOTE_B4 = 494;
const int NOTE_C5 = 523;
const int NOTE_CS5 = 554;
const int NOTE_D5 = 587;
const int NOTE_DS5 = 622;
const int NOTE_E5 = 659;
const int NOTE_F5 = 698;
const int NOTE_FS5 = 740;
const int NOTE_G5 = 784;
const int NOTE_GS5 = 831;
const int NOTE_A5 = 880;
const int NOTE_AS5 = 932;
const int NOTE_B5 = 988;
const int NOTE_C6 = 1047;
const int NOTE_CS6 = 1109;
const int NOTE_D6 = 1175;
const int NOTE_DS6 = 1245;
const int NOTE_E6 = 1319;
const int NOTE_F6 = 1397;
const int NOTE_FS6 = 1480;
const int NOTE_G6 = 1568;
const int NOTE_GS6 = 1661;
const int NOTE_A6 = 1760;
const int NOTE_AS6 = 1865;
const int NOTE_B6 = 1976;
const int NOTE_C7 = 2093;
const int NOTE_CS7 = 2217;
const int NOTE_D7 = 2349;
const int NOTE_DS7 = 2489;
const int NOTE_E7 = 2637;
const int NOTE_F7 = 2794;
const int NOTE_FS7 = 2960;
const int NOTE_G7 = 3136;
const int NOTE_GS7 = 3322;
const int NOTE_A7 = 3520;
const int NOTE_AS7 = 3733;
const int NOTE_B7 = 3951;
const int NOTE_C8 = 4186;
const int NOTE_CS8 = 4435;
const int NOTE_D8 = 4699;
const int NOTE_DS8 = 4978;
const int NOTE_E8 = 5274;
const int NOTE_F8 = 5588;
const int NOTE_FS8 = 5920;
const int NOTE_G8 = 6272;
const int NOTE_GS8 = 6645;
const int NOTE_A8 = 7040;
const int NOTE_AS8 = 7466;
const int NOTE_B8 = 7902;
const int UNIT_TIME = 100;
const int DOT_DURATION = UNIT_TIME;
const int DASH_DURATION = UNIT_TIME * 3;
const int ELEMENT_SPACE = UNIT_TIME;
const int CHAR_SPACE = UNIT_TIME * 3;
const int DISTANCE_UPDATE_INTERVAL = 200;
const int SWEEP_SPEED_DELAY = 20;
const int TURN_CLEARANCE_DISTANCE = 18;
const int MAZE_TURN_LEFT_SPEED = 255;
const int MAZE_TURN_RIGHT_SPEED = 255;
const int FOLLOW_DISTANCE_CM = 20;
const int MAZE_TURN_LEFT_DURATION = 310;
const int MAZE_TURN_RIGHT_DURATION = 330;
const int TEST_ACTION_DURATION = 1000;
const int LCD_ANIMATION_DELAY = 300;
extern const int handleCredits_codeLinesCounter_endLine;
extern const int handleCredits_codeLinesCounter_calculator;
static int lastPrintedSec;
static int timerHours;
static int timerMinutes;
static int timerSeconds;
static int activeUnit;
  // Longs
long distLeft = 0;
long distRight = 0;
const long OBSTACLE_DISTANCE = 15;
unsigned long lastDistanceUpdateTime = 0;
unsigned long lastSweepTime = 0;
unsigned long lastPauseBlinkTime = 0;
unsigned long lastMessageTime = 0;
unsigned long stateStartTime = 0;
unsigned long lastLcdAnimationUpdate = 0;
unsigned long testStateStartTime = 0;
unsigned long btBuzzerLastToggle = 0;
unsigned long lastMotorMicros = 0;
const unsigned long motorPeriod = 5000;
const unsigned long MAZE_PAUSE_BLINK_INTERVAL = 300;
static unsigned long modeStartTime;
static unsigned long lastJoystickAction;
static unsigned long lastDisplayUpdate;
  // Booleans
bool isCreditsActive = false;
bool isServoMovementEnabled = false;
bool manTstJustEntered = false;
bool isMazePaused = false;
bool sevenSegIsOn = false;
bool buzzerIsOn = false;
bool isPauseBlinkOn = true;
bool buzzerEnabled = false;
bool btModeJustEntered = false;
bool hasOperator = false;
bool showingResult = false;
bool counterStopwatchAndTimer = false;
bool btBuzzerState = false;
bool offTriggered=false;
static bool firstRun = true;
  // Statics
static SubModeState subState;
static Mode lastMode;
  // Chars
char num1[8] = "";      
char num2[8] = "";      
char activeOp = ' ';
char resultStr[12] = "";
  // Strings
String mazeMovementState = "Moving";
  // Modes
Mode currentMode = NONE;
  // Customs
handleMaze_followState currenthandleMaze_followState = FOLLOW_STATE_WAITING;
handleMaze_mazeState currenthandleMaze_mazeState = MAZE_STATE_MOVING_FORWARD;
handleAutomaticTest_testState currenthandleAutomaticTest_testState = TEST_STATE_INIT;
Mode_noneState currentMode_noneState = NONE_STATE_WAITING;
// Void setup
void setup()
{
  Serial.begin(9600);
  Serial1.begin(9600);
  EEPROM.get(EEPROMHighScore, HighScore);
  EEPROM.get(EEPROMHighScoreSimon, HighScoreSimon);
  EEPROM.get(EEPROMHighScoreReactionMinutes, HighScoreReactionMinutes);
  EEPROM.get(EEPROMHighScoreReactionSeconds, HighScoreReactionSeconds);
  EEPROM.get(EEPROMHighScoreReactionMilliseconds, HighScoreReactionMilliseconds);
  saveVIN();
  lcd.init();
  lcd.backlight();
  myServo.attach(servoMotor);
  for (int i = 0; i < 7; i++)
  {
    pinMode(sevenSegmentPin[i], OUTPUT);
  }
  pinMode(joystick_button, INPUT_PULLUP);
  pinMode(joystick_VRX, INPUT);
  pinMode(joystick_VRY, INPUT);
  pinMode(LED_red, OUTPUT);
  pinMode(LED_green, OUTPUT);
  pinMode(LED_blue, OUTPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(motors_IN1, OUTPUT);
  pinMode(motors_IN2, OUTPUT);
  pinMode(motors_IN3, OUTPUT);
  pinMode(motors_IN4, OUTPUT);
  pinMode(motors_ENA, OUTPUT);
  pinMode(motors_ENB, OUTPUT);
  pinMode(potentiometer, INPUT);
  pinMode(ultrasonicSensor_trigger, OUTPUT);
  pinMode(ultrasonicSensor_echo, INPUT);
  LCD_welcomeMessage();
  LCD_menuMessage();
}
// Void loop
void loop()
{
  motorsSpeed();
  char customKey = keypad.getKey();
  if (customKey == '#')
  {
    if (currentMode == MAZE || currentMode == BLUETOOTH || currentMode == FOLLOW_ME || currentMode == CHROME_DINAUSOR_GAME)
    {
      buzzer_activeBeep(50);
      isMazePaused = !isMazePaused;
      if (isMazePaused)
      {
        motorsStop();
        lcd.clear();
        lcd.setCursor(2, 0);
        LCD_displayMessageCentered("ROBOT PAUSED", 0);
        lcd.setCursor(0, 1);
        LCD_displayMessageCentered("~S15 TO CONTINUE", 1);
        sevenSegment_displayLetter('P');
        lastPauseBlinkTime = millis();
        isPauseBlinkOn = true;
        digitalWrite(LED_green, LOW);
      }
      else
      {
        lcd.clear();
        updateMovementState(mazeMovementState, true);
        digitalWrite(LED_red, LOW);
        lcd.clear();
      }
    }
  }
  if ((currentMode == MAZE || currentMode == BLUETOOTH || currentMode == FOLLOW_ME || currentMode == CHROME_DINAUSOR_GAME) && isMazePaused)
  {
    unsigned long currentTime = millis();
    digitalWrite(LED_blue, LOW);
    digitalWrite(LED_green, LOW);
    if (currentTime - lastPauseBlinkTime >= MAZE_PAUSE_BLINK_INTERVAL)
    {
      lastPauseBlinkTime = currentTime;
      isPauseBlinkOn = !isPauseBlinkOn;
      if (isPauseBlinkOn)
      {
        sevenSegment_displayLetter('P');
        digitalWrite(LED_red, HIGH);
      }
      else
      {
        sevenSegment_clearAllSegments();
        digitalWrite(LED_red, LOW);
      }
    }
  }
  if (currentMode == NONE && menuPage!=1 && (analogRead(joystick_VRX) > 800))
  {
    sevenSegment_clearAllSegments();
    LCD_menuMessage_buzzer_passiveBeep();
    tone(buzzer, NOTE_C6);
    digitalWrite(LED_green, HIGH);
    menuPage=1;
    LCD_menuMessage();
    noTone(buzzer);
    digitalWrite(LED_green, LOW);
    delay(200);
    return;
  }
  else if (currentMode == NONE && menuPage!=2 && (analogRead(joystick_VRX) < 250))
  {
    sevenSegment_clearAllSegments();
    LCD_menuMessage_buzzer_passiveBeep();
    tone(buzzer, NOTE_C4);
    digitalWrite(LED_green, HIGH);
    menuPage=2;
    LCD_menuMessage();
    noTone(buzzer);
    digitalWrite(LED_green, LOW);
    delay(200);
    return;
  }
  else if ((currentMode != NONE && currentMode != OFF) && customKey == 'D')
  {
    handleEmergency();
    return;
  }
  if (currentMode == NONE)
  {
    if (customKey == '1' || (Serial1.available() && Serial1.peek() == '1'))
    {
      if (Serial1.available() && Serial1.peek() == '1')
      {
        Serial1.read();
      }
      Serial.println(" -- BLUETOOTH MODE TRIGGERED !");
      Serial1.println(" -- BLUETOOTH MODE TRIGGERED !");
      buzzer_activeBeep();
      sevenSegment_displayDigit(1);
      lcd.clear();
      currentMode = BLUETOOTH;
      btModeJustEntered = true;
      digitalWrite(LED_red, HIGH);
      digitalWrite(LED_green, LOW);
      currentMode_noneState = NONE_STATE_WAITING;
    }
    else if (customKey == '2' || (Serial1.available() && Serial1.peek() == '2'))
    {
      if (Serial1.available() && Serial1.peek() == '2')
      {
        Serial1.read();
      }
      sevenSegment_clearAllSegments();
      digitalWrite(sevenSegmentPin[4], HIGH);
      digitalWrite(sevenSegmentPin[5], HIGH);
      digitalWrite(sevenSegmentPin[6], HIGH);
      lcd.clear();
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, HIGH);
      Serial.println(" -- DAMI 3000 uses a Right-Hand Algorithm !");
      Serial1.println(" -- DAMI 3000 uses a Right-Hand Algorithm !");
      LCD_displayMessageCentered("DAMI 3000 uses a", 0);
      LCD_displayMessageCentered("Right-Hand Alg !", 1);
      motorsStop();
      myServo.write(45);
      handleMaze_buzzer_passiveBeep();
      delay(1500);
      myServo.write(90);
      digitalWrite(LED_blue, LOW);
      lcd.clear();
      LCD_displayMessageCentered("Starting in...", 0);
      Serial.println(" -- Starting in 3, 2, 1...");
      Serial1.println(" -- Starting in 3, 2, 1...");
      for (int count = 3; count >= 1; count--)
      {
        LCD_displayMessageCentered(String(count), 1);
        digitalWrite(LED_red, HIGH);
        sevenSegment_displayDigit(count);
        if (count == 3)
        {
          tone(buzzer, NOTE_C3);
          delay(800);
        }
        if (count == 2)
        {
          tone(buzzer, NOTE_C4);
          delay(800);
        }
        if (count == 1)
        {
          tone(buzzer, NOTE_C5);
          delay(800);
        }
        sevenSegment_clearAllSegments();
        digitalWrite(LED_red, LOW);
      }
      sevenSegment_clearAllSegments();
      lcd.clear();
      sevenSegment_displayDigit(0);
      Serial.println(" -- GOOO !");
      Serial1.println(" -- GOOO !");
      LCD_displayMessageCentered("GOOO !", 0);
      digitalWrite(LED_green, HIGH);
      tone(buzzer, NOTE_C6);
      delay(1000);
      noTone(buzzer);
      currentMode = MAZE;
      currenthandleMaze_mazeState = MAZE_STATE_MOVING_FORWARD;
      stateStartTime = millis();
      updateMovementState("");
      currentMode_noneState = NONE_STATE_WAITING;
      delay(300);
      sevenSegment_clearAllSegments();
      digitalWrite(LED_green, LOW);
      lcd.clear();
    }
    else if (customKey == 'B' || (Serial1.available() && Serial1.peek() == 'B'))
    {
      if (Serial1.available() && Serial1.peek() == 'B')
      {
        Serial1.read();
      }
      currentMode = TESTING;
      currenthandleAutomaticTest_testState = TEST_STATE_INIT;
      testStateStartTime = millis();
      updateMovementState("Testing: Initializing");
      sevenSegment_displayDigit(8);
      lcd.clear();
      buzzer_activeBeep();
      delay(500);
    }
    else if (customKey == 'A' || (Serial1.available() && Serial1.peek() == 'A'))
    {
      if (Serial1.available() && Serial1.peek() == 'A')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      currentMode = FOLLOW_ME;
      currenthandleMaze_followState = FOLLOW_STATE_FOLLOWING;
      motorsStop();
    }
    else if (customKey == '6' || (Serial1.available() && Serial1.peek() == '6'))
    {
      if (Serial1.available() && Serial1.peek() == '6')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      lcd.clear();
      currentMode = MAN_TST;
      customKey = '5';
      manTstJustEntered = true;
      updateMovementState("MANUAL TEST READY");
    }
    else if (customKey == '5' || (Serial1.available() && Serial1.peek() == '5'))
    {
      if (Serial1.available() && Serial1.peek() == '5')
      {
        Serial1.read();
      }
      if (currentMode == NONE)
      {
        buzzer_activeBeep();
        lcd.clear();
        currentMode = CREDITS;
        isCreditsActive = true;
        currentCreditMessage = 0;
        lastMessageTime = millis();
        updateMovementState(" ");
      }
      else if (currentMode == CREDITS)
      {
        isServoMovementEnabled = !isServoMovementEnabled;
        if (isServoMovementEnabled)
        {
          Serial.println("Servo movement ON");
          myServo.write(45);
        }
        else
        {
          Serial.println("Servo movement OFF");
          myServo.write(0);
        }
      }
    }
  }
  if (((customKey == '7') || (Serial1.available() && Serial1.peek() == '7')) && currentMode != JOYSTICK_TEST && currentMode == NONE)
  {
    currentMode = JOYSTICK_TEST;
    lcd.clear();
    buzzer_activeBeep();
    sevenSegment_clearAllSegments();
    motorsStop();
  }
  if (currentMode == JOYSTICK_TEST)
  {
    handleJoystickTest();
  }
  else if ((customKey == '3' || (Serial1.available() && Serial1.peek() == '3')) && currentMode == NONE)
  {
    if (Serial1.available() && Serial1.peek() == '3')
    {
      Serial1.read();
    }
    currentMode = MORSE_COM;
    handleMorseCommunication();
  }
  else if (currentMode == MORSE_COM)
  {
    handleMorseCommunication();
  }
  else if ((customKey == '4' || (Serial1.available() && Serial1.peek() == '4')) && currentMode == NONE)
  {
    if (Serial1.available() && Serial1.peek() == '4')
    {
      Serial1.read();
    }
    currentMode = DICE;
    handleDice();
  }
  else if (currentMode == DICE)
  {
    handleDice();
  }
    else if ((customKey == '8' || (Serial1.available() && Serial1.peek() == '8')) && currentMode == NONE)
    {
      if (Serial1.available() && Serial1.peek() == '8')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      lcd.clear();
      currentMode = CHROME_DINAUSOR_GAME;
    }
    else if ((customKey == '9' || (Serial1.available() && Serial1.peek() == '9')) && currentMode == NONE)
    {
      if (Serial1.available() && Serial1.peek() == '9')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      lcd.clear();
      currentMode = STOPWATCH_AND_TIMER;
    }
    else if ((customKey == 'C' || (Serial1.available() && Serial1.peek() == 'C')) && currentMode == NONE)
    {
      if (Serial1.available() && Serial1.peek() == 'C')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      lcd.clear();
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      sevenSegment_clearAllSegments();
      currentMode = CALCULATOR;
    }
    else if ((customKey == '*' || (Serial1.available() && Serial1.peek() == '*')) && currentMode == NONE)
    {
      if (Serial1.available() && Serial1.peek() == '*')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      lcd.clear();
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      sevenSegment_clearAllSegments();
      currentMode = SIMON_SAYS;
    }
    else if ((customKey == '0' || (Serial1.available() && Serial1.peek() == '0')) && currentMode == NONE)
    {
      if (Serial1.available() && Serial1.peek() == '0')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      lcd.clear();
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      sevenSegment_clearAllSegments();
      currentMode = SLEEPY_RADAR;
    }
    else if ((customKey == '#' || (Serial1.available() && Serial1.peek() == '#')) && currentMode == NONE)
    {
      if (Serial1.available() && Serial1.peek() == '#')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      lcd.clear();
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      sevenSegment_clearAllSegments();
      currentMode = MELODIES;
    }
    else if ((customKey == 'D' || (Serial1.available() && Serial1.peek() == 'D')) && currentMode == NONE)
    {
      if (Serial1.available() && Serial1.peek() == 'D')
      {
        Serial1.read();
      }
      buzzer_activeBeep();
      lcd.clear();
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      sevenSegment_clearAllSegments();
      currentMode = REACTION;
    }
    else if ((digitalRead(joystick_button) == LOW || (Serial1.available() && Serial1.peek() == 'O')) && currentMode == NONE)
    {
      if (Serial1.available() && Serial1.peek() == 'O')
      {
        Serial1.read();
      }
      lcd.clear();
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      sevenSegment_clearAllSegments();
      currentMode = OFF;
    }
  else if (currentMode == BLUETOOTH)
  {
    if (!isMazePaused)
    {
      handleBluetooth();
    }
  }
  else if (currentMode == MAZE)
  {
    if (!isMazePaused)
    {
      handleMaze();
    }
  }
  else if (currentMode == MAN_TST)
  {
    handleManualTest(customKey);
  }
  else if (currentMode == TESTING)
  {
    handleAutomaticTest();
  }
  else if (currentMode == CREDITS)
  {
    handleCredits();
  }
  else if (currentMode == FOLLOW_ME)
  {
    if (!isMazePaused)
    {
      handleFollowMe();
    }
  }
  else if (currentMode == OFF)
  {
    handleOff();
  }
  else if (currentMode == REACTION)
  {
    handleReaction();
  }
  else if (currentMode == MELODIES)
  {
    handleMelodies();
  }
  else if (currentMode == SLEEPY_RADAR)
  {
    handleSleepyRadar();
  }
  else if (currentMode == SIMON_SAYS)
  {
    handleSimonSays();
  }
  else if (currentMode == CALCULATOR)
  {
    handleCalculator(customKey);
  }
  else if (currentMode == STOPWATCH_AND_TIMER)
  {
    handleStopwatchAndTimer();
  }
  else if (currentMode == CHROME_DINAUSOR_GAME)
  {
    if (!isMazePaused)
    {
      handleChromeDinausorGame();
    }
  }
  if (currentMode == BLUETOOTH)
  {
    if (!isMazePaused)
    {
      static unsigned long lastBlinkTime = 0;
      static bool showDigit = true;
      if (millis() - lastBlinkTime >= 300)
      {
        lastBlinkTime = millis();
        showDigit = !showDigit;
        if (!isMazePaused)
        {
          if (showDigit)
          {
            sevenSegment_displayDigit(1);
          }
          else
          {
            sevenSegment_clearAllSegments();
          }
        }
      }
      lcd.setCursor(0, 0);
      lcd.print("Distance : ");
      long distance = ultrasonicSensor_measureDistance();
      if (distance < 10)
        lcd.print("0");
      if (distance < 100)
        lcd.print(distance);
      else
        lcd.print("99");
      lcd.print(" cm");
      if (distance <= 99 && distance != 0)
      {
        Serial1.print("Distance : ");
        Serial1.print(distance);
        Serial1.println(" cm");
        Serial.print("Distance : ");
        Serial.print(distance);
      }
      else if (distance == 0)
      {
        Serial1.print("Distance : MIN\n");
        Serial.print("Distance : MIN\n");
      }
      else
      {
        Serial1.print("Distance : MAX\n");
        Serial.print("Distance : MAX\n");
      }
    }
  }
  else if (currentMode == NONE)
  {
    sevenSegment_animation();
    LCD_menuMessage_animation();
  }
}
// Functions Definitions
void updateMovementState(String state, bool forceUpdate = false)
{
  if (isMazePaused)
  {
    return;
  }
  static String lastDisplayedState = "";
  if (forceUpdate || state != lastDisplayedState)
  {
    LCD_displayMessageCentered(state, 1);
    lastDisplayedState = state;
    if (currentMode == BLUETOOTH)
    {
      bool motorsAreMoving = (state.indexOf("Forward") != -1 ||
                              state.indexOf("Backward") != -1 ||
                              state.indexOf("Left") != -1 ||
                              state.indexOf("Right") != -1);
      if (motorsAreMoving)
      {
        digitalWrite(LED_red, LOW);
        digitalWrite(LED_green, HIGH);
        digitalWrite(LED_blue, LOW);
      }
      else if (state.indexOf("Idle") != -1)
      {
        digitalWrite(LED_red, HIGH);
        digitalWrite(LED_green, LOW);
        digitalWrite(LED_blue, LOW);
      }
    }
    else if (currentMode == MAZE)
    {
      if (state.indexOf("Obstacle!") != -1)
      {
        digitalWrite(LED_red, HIGH);
        digitalWrite(LED_green, LOW);
        digitalWrite(LED_blue, LOW);
      }
      else
      {
        digitalWrite(LED_red, LOW);
        digitalWrite(LED_green, HIGH);
        digitalWrite(LED_blue, LOW);
      }
    }
    else if (currentMode == TESTING)
    {
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
    }
  }
}
// DC Motors
void motorsSpeed(){
    int potentiometerValue = analogRead(potentiometer); 
    int speed = map(potentiometerValue, 0, 1023, 0, 255);
    analogWrite(motors_ENB, speed); 
    unsigned long currentMicros = micros();
    if (currentMicros - lastMotorMicros >= motorPeriod) {
      lastMotorMicros = currentMicros;
    }
    unsigned long highTime = ((unsigned long)speed * motorPeriod) / 255;
    if (currentMicros - lastMotorMicros < highTime) {
      digitalWrite(motors_ENA, HIGH);
    } else {
      digitalWrite(motors_ENA, LOW);
    }
}
void motorsForward()
{
  digitalWrite(motors_IN1, HIGH);
  digitalWrite(motors_IN2, LOW);
  digitalWrite(motors_IN3, LOW);
  digitalWrite(motors_IN4, HIGH);
}
void motorsBackward()
{
  digitalWrite(motors_IN1, LOW);
  digitalWrite(motors_IN2, HIGH);
  digitalWrite(motors_IN3, HIGH);
  digitalWrite(motors_IN4, LOW);
}
void motorsLeft()
{
  digitalWrite(motors_IN1, HIGH);
  digitalWrite(motors_IN2, LOW);
  digitalWrite(motors_IN3, HIGH);
  digitalWrite(motors_IN4, LOW);
}
void motorsRight()
{
  digitalWrite(motors_IN1, LOW);
  digitalWrite(motors_IN2, HIGH);
  digitalWrite(motors_IN3, LOW);
  digitalWrite(motors_IN4, HIGH);
}
void motorsStop()
{
  analogWrite(motors_ENA, 0);
  analogWrite(motors_ENB, 0);
  digitalWrite(motors_IN1, LOW);
  digitalWrite(motors_IN2, LOW);
  digitalWrite(motors_IN3, LOW);
  digitalWrite(motors_IN4, LOW);
}
// Buzzer
void buzzer_activeBeep(int duration)
{
  tone(buzzer, NOTE_C5, 100);
  delay(duration);
  noTone(buzzer);
}
// Ultrasonic Sensor
long ultrasonicSensor_measureDistance()
{
  digitalWrite(ultrasonicSensor_trigger, LOW);
  delayMicroseconds(2);
  digitalWrite(ultrasonicSensor_trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(ultrasonicSensor_trigger, LOW);
  long duration = pulseIn(ultrasonicSensor_echo, HIGH, 25000);
  long distance = duration * 0.0343 / 2;
  return distance;
}
// Reset mode
void handleReset(int handleReset_delay)
{
  lcd.clear();
  offTriggered=false;
  firstRun = true;
  menuPage = 1;
  counterStopwatchAndTimer=false;
  lastMode = NONE;
  timerHours = 0;
  timerMinutes = 0;
  timerSeconds = 0;
  activeUnit = 2;
  lastPrintedSec = -1;
  motorsStop();
  myServo.write(90);
  sevenSegment_clearAllSegments();
  digitalWrite(LED_red, LOW);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_blue, LOW);
  servoPos = 90;
  isMazePaused = false;
  ensureSingleModeAccess = 1;
  currentMode = NONE;
  LCD_menuMessage();
  delay(handleReset_delay);
}
// Emergency mode
void handleEmergency()
{
  lcd.clear();
  offTriggered=false;
  firstRun = true;
  menuPage = 1;
  counterStopwatchAndTimer=false;
  lastMode = NONE;
  timerHours = 0;
  timerMinutes = 0;
  timerSeconds = 0;
  activeUnit = 2;
  lastPrintedSec = -1;
  motorsStop();
  myServo.write(90);
  sevenSegment_clearAllSegments();
  digitalWrite(LED_red, LOW);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_blue, LOW);
  servoPos = 90;
  isMazePaused = false;
  ensureSingleModeAccess = 1;
  Serial.println(" -- EMERGENCY MODE TRIGGERED !");
  Serial1.println(" -- EMERGENCY MODE TRIGGERED !");
  lcd.setCursor(0, 0);
  lcd.print(" EMERGENCY MODE");
  lcd.setCursor(0, 1);
  lcd.print(" TRIGGERED !!!!");
  for (int i = 0; i < 7; i++)
  {
    if(i%2==0){
      lcd.backlight();
    }else{
      lcd.noBacklight();
    }
    sevenSegment_displayLetter('E');
    digitalWrite(LED_red, HIGH);
    handleEmergency_buzzer_passiveBeep();
    sevenSegment_clearAllSegments();
    digitalWrite(LED_red, LOW);
    delay(50);
  }
  delay(1000);
  lcd.clear();
  currentMode = NONE;
  LCD_menuMessage();
}
void handleEmergency_buzzer_passiveBeep()
{
  tone(buzzer, NOTE_C6, 100);
  delay(70);
  noTone(buzzer);
}
// Bluetooth mode
void handleBluetooth()
{
  digitalWrite(LED_red , LOW);
  digitalWrite(LED_green , LOW);
  digitalWrite(LED_blue , LOW);
  static bool isForwardToggled = false;
  static bool isBackwardToggled = false;
  static bool isLeftToggled = false;
  static bool isRightToggled = false;
  static bool isServoToggled = false;
  static String lastNonMovementState = "";
  auto resetMotorsToggle = [&]()
  {
    isForwardToggled = false;
    isBackwardToggled = false;
    isLeftToggled = false;
    isRightToggled = false;
    motorsStop();
    lastNonMovementState = "";
  };
  if (btModeJustEntered)
  {
    digitalWrite(LED_red , HIGH);
  digitalWrite(LED_green , LOW);
  digitalWrite(LED_blue , LOW);
    resetMotorsToggle();
    updateMovementState("Waiting for inpt", true);
    btModeJustEntered = false;
  }
  char keyboardPress = 0;
  while (Serial1.available())
  {
    keyboardPress = Serial1.read();
  }
  if (keyboardPress != 0 && keyboardPress != '\n' && keyboardPress != '\r')
  {
    switch (keyboardPress)
    {
    // DC Motors
    case 'Z':
      if (isForwardToggled)
      {
        resetMotorsToggle();
      }
      else
      {
        resetMotorsToggle();
        isForwardToggled = true;
        motorsForward();
        updateMovementState("Motors Forward");
        lastNonMovementState = "Motors Forward";
      }
      break;
    case 'S':
      if (isBackwardToggled)
      {
        resetMotorsToggle();
      }
      else
      {
        motorsBackward();
        isBackwardToggled = true;
        motorsBackward();
        updateMovementState("Motors Backward");
        lastNonMovementState = "Motors Backward";
      }
      break;
    case 'Q':
      if (isLeftToggled)
      {
        resetMotorsToggle();
      }
      else
      {
        resetMotorsToggle();
        isLeftToggled = true;
        motorsLeft();
        updateMovementState("Motors Left");
        lastNonMovementState = "Motors Left";
      }
      break;
    case 'D':
      if (isRightToggled)
      {
        resetMotorsToggle();
      }
      else
      {
        resetMotorsToggle();
        isRightToggled = true;
        motorsRight();
        updateMovementState("Motors Right");
        lastNonMovementState = "Motors Right";
      }
      break;
    case ' ':
      resetMotorsToggle();
      updateMovementState("Motors Idle");
      lastNonMovementState = "Motors Idle";
      break;
    // Servo Motor
    case 'M':
      if (isServoToggled)
      {
        digitalWrite(LED_red , HIGH);
        myServo.write(90);
        servoPos = 90;
        updateMovementState("Servo MIDDLE");
        lastNonMovementState = "Servo MIDDLE";
      }
      else
      {
        digitalWrite(LED_green , HIGH);
        myServo.write(0);
        servoPos = 0;
        updateMovementState("Servo RIGHT");
        lastNonMovementState = "Servo RIGHT";
      }
      break;
    case 'K':
      if (isServoToggled)
      {
        digitalWrite(LED_red , HIGH);
        myServo.write(90);
        servoPos = 90;
        updateMovementState("Servo MIDDLE");
        lastNonMovementState = "Servo MIDDLE";
      }
      else
      {
        digitalWrite(LED_green , HIGH);
        myServo.write(180);
        servoPos = 180;
        updateMovementState("Servo LEFT");
        lastNonMovementState = "Servo LEFT";
      }
      break;
    case 'O':
    case 'L':
      digitalWrite(LED_red , HIGH);
      myServo.write(90);
      servoPos = 90;
      updateMovementState("Servo MIDDLE");
      lastNonMovementState = "Servo MIDDLE";
      break;
    case 'I':
      if (isServoToggled)
      {
        digitalWrite(LED_red , HIGH);
        myServo.write(90);
        servoPos = 90;
        updateMovementState("Servo MIDDLE");
        lastNonMovementState = "Servo MIDDLE";
      }
      else
      {
        digitalWrite(LED_green , HIGH);
        myServo.write(135);
        servoPos = 135;
        updateMovementState("Servo left");
        lastNonMovementState = "Servo left";
      }
      break;
    case 'P':
      if (isServoToggled)
      {
        digitalWrite(LED_red , HIGH);
        myServo.write(90);
        servoPos = 90;
        updateMovementState("Servo MIDDLE");
        lastNonMovementState = "Servo MIDDLE";
      }
      else
      {
        digitalWrite(LED_green , HIGH);
        myServo.write(45);
        servoPos = 45;
        updateMovementState("Servo right");
        lastNonMovementState = "Servo right";
      }
      break;
    // Buzzer
    case 'B':
      buzzerEnabled = !buzzerEnabled;
      if (buzzerEnabled)
      {   
        digitalWrite(LED_green , HIGH);
        tone(buzzer, NOTE_E5);
        updateMovementState("Buzzer ON");
        lastNonMovementState = "Buzzer ON";
      }
      else
      {
        digitalWrite(LED_red , HIGH);
        noTone(buzzer);
        updateMovementState("Buzzer OFF");
        lastNonMovementState = "Buzzer OFF";
      }
      break;
    // Emergency mode
    case 'X':
      handleEmergency();
      return;
    }
  }
}
// Maze mode
void handleMaze()
{
  if (ensureSingleModeAccess == 1)
  {
    ensureSingleModeAccess = 0;
    Serial.println(" -- MAZE MODE TRIGGERED !");
    Serial1.println(" -- MAZE MODE TRIGGERED !");
  }
  static unsigned long lastSegmentBlinkTime = 0;
  static bool showSegmentDigit = true;
  static unsigned long lastBeepTimeMoving = 0;
  digitalWrite(LED_blue, LOW);
  if (millis() - lastSegmentBlinkTime >= 300)
  {
    lastSegmentBlinkTime = millis();
    showSegmentDigit = !showSegmentDigit;
    if (showSegmentDigit)
    {
      sevenSegment_displayDigit(2);
    }
    else
    {
      sevenSegment_clearAllSegments();
    }
  }
  lcd.setCursor(0, 0);
  lcd.print("Distance : ");
  long currentDistance = ultrasonicSensor_measureDistance();
  if (currentDistance < 10)
    lcd.print("0");
  if (currentDistance < 100)
    lcd.print(currentDistance);
  else
    lcd.print("99");
  lcd.print(" cm      ");
  switch (currenthandleMaze_mazeState)
  {
  case MAZE_STATE_MOVING_FORWARD:
    motorsForward();
    updateMovementState("Forward...");
    if (millis() - lastBeepTimeMoving >= 800)
    {
      lastBeepTimeMoving = millis();
      buzzer_activeBeep(50);
    }
    if (ultrasonicSensor_measureDistance() <= OBSTACLE_DISTANCE)
    {
      currenthandleMaze_mazeState = MAZE_STATE_OBSTACLE_DETECTED;
      stateStartTime = millis();
      motorsStop();
      updateMovementState("Obstacle!!!!");
      for (int m = 0; m < 5; m++)
      {
        tone(buzzer, NOTE_B3);
        delay(100);
        noTone(buzzer);
      }
      delay(1000);
    }
    break;
  case MAZE_STATE_OBSTACLE_DETECTED:
    currenthandleMaze_mazeState = MAZE_STATE_SCANNING;
    stateStartTime = millis();
    break;
  case MAZE_STATE_SCANNING:
    updateMovementState("Scanning...");
    myServo.write(0);
    delay(300);
    distRight = ultrasonicSensor_measureDistance();
    Serial.print("Right: ");
    Serial.println(distRight);
    myServo.write(180);
    delay(300);
    distLeft = ultrasonicSensor_measureDistance();
    Serial.print("Left: ");
    Serial.println(distLeft);
    myServo.write(90);
    delay(100);
    if (distRight > TURN_CLEARANCE_DISTANCE)
    {
      currenthandleMaze_mazeState = MAZE_STATE_TURNING_RIGHT;
    }
    else if (distLeft > TURN_CLEARANCE_DISTANCE)
    {
      currenthandleMaze_mazeState = MAZE_STATE_TURNING_LEFT;
    }
    else
    {
      currenthandleMaze_mazeState = MAZE_STATE_TURNING_AROUND;
    }
    stateStartTime = millis();
    break;
  case MAZE_STATE_TURNING_LEFT:
    tone(buzzer, NOTE_C7);
    updateMovementState("Turning Left");
    handleMaze_motorsTurn("left");
    currenthandleMaze_mazeState = MAZE_STATE_MOVING_FORWARD;
    stateStartTime = millis();
    break;
  case MAZE_STATE_TURNING_RIGHT:
    tone(buzzer, NOTE_C7);
    updateMovementState("Turning Right");
    handleMaze_motorsTurn("right");
    currenthandleMaze_mazeState = MAZE_STATE_MOVING_FORWARD;
    stateStartTime = millis();
    break;
  case MAZE_STATE_TURNING_AROUND:
    tone(buzzer, NOTE_C7);
    updateMovementState("Turning Around");
    handleMaze_motorsTurn("around");
    currenthandleMaze_mazeState = MAZE_STATE_MOVING_FORWARD;
    stateStartTime = millis();
    break;
  }
  noTone(buzzer);
}
void handleMaze_motorsTurn(String direction)
{
  if (direction == "left")
  {
    motorsLeft();
    delay(MAZE_TURN_LEFT_DURATION);
  }
  else if (direction == "right")
  {
    motorsRight();
    delay(MAZE_TURN_RIGHT_DURATION);
  }
  else if (direction == "around")
  {
    motorsLeft();
    delay(MAZE_TURN_LEFT_DURATION * 2);
  }
  motorsStop();
  delay(100);
}
void handleMaze_buzzer_passiveBeep()
{
  const int LEVEL_CLEAR_TEMPO = 150;
  int creditsMelody[] = {
      NOTE_C5, NOTE_G4, NOTE_E4, NOTE_A4, NOTE_B4, NOTE_A4, NOTE_G4,
      NOTE_REST,
      NOTE_E5, NOTE_C5};
  int creditsDurations[] = {
      8, 8, 8, 8, 8, 8, 8,
      8,
      4, 2};
  int notes = sizeof(creditsMelody) / sizeof(creditsMelody[0]);
  for (int i = 0; i < notes; i++)
  {
    long duration = (LEVEL_CLEAR_TEMPO * 4) / creditsDurations[i];
    if (creditsMelody[i] != NOTE_REST)
    {
      tone(buzzer, creditsMelody[i], duration);
    }
    long pauseBetweenNotes = duration * 1.30;
    delay(pauseBetweenNotes);
    noTone(buzzer);
  }
}
// Morse Communication mode
void handleMorseCommunication()
{
  if (ensureSingleModeAccess == 1)
  {
    ensureSingleModeAccess = 0;
    Serial.println(" -- MORSE COMMUNICATION MODE TRIGGERED !");
    Serial1.println(" -- MORSE COMMUNICATION MODE TRIGGERED !");
    Serial.println(" -- Press [SPACE] to clear message.");
    Serial1.println(" -- Press [SPACE] to clear message.");
    lcd.clear();
    sevenSegment_clearAllSegments();
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    buzzer_activeBeep(100);
    sevenSegment_displayLetter('M');
    digitalWrite(LED_blue, HIGH);
    delay(100);
    digitalWrite(LED_blue, LOW);
    sevenSegment_clearAllSegments();
    buzzer_activeBeep(100);
    digitalWrite(LED_blue, HIGH);
    sevenSegment_displayLetter('M');
    motorsStop();
    Serial.println("\n -- Ready for Serial Input or Keypad Command !\n");
    Serial1.println("\n -- Ready for Serial Input or Keypad Command !\n");
    Serial.println(" -- Transmitted Message :\n");
    Serial1.println(" -- Transmitted Message :\n");
    lcd.setCursor(0, 0);
    lcd.print("Morse Commt Mode");
    lcd.setCursor(0, 1);
    lcd.print(" \x7EUse CoolTerm\x7F");
    delay(1000);
    digitalWrite(LED_blue, LOW);
    sevenSegment_clearAllSegments();
    delay(1000);
    lcd.clear();
    buzzer_activeBeep(100);
    sevenSegment_displayLetter('8');
    digitalWrite(LED_red, HIGH);
    delay(100);
    sevenSegment_clearAllSegments();
    lcd.setCursor(0, 0);
    lcd.print("Transmitted msg:");
    lcd.setCursor(0, 1);
    lcd.print("~");
  }
  char keyboardPress = 0;
  while (Serial1.available())
  {
    keyboardPress = Serial1.read();
    char receivedChar = Serial1.read();
    Serial.println(keyboardPress);
    switch (keyboardPress)
    {
    case 'A' : case 'a':
      handleMorseCommunication_character('A');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('A');
      morseRepeater++;
      break;
    case 'B': case 'b':
      handleMorseCommunication_character('B');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('B');
      morseRepeater++;
      break;
    case 'C': case 'c':
      handleMorseCommunication_character('C');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('C');
      morseRepeater++;
      break;
    case 'D': case 'd':
      handleMorseCommunication_character('D');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('D');
      morseRepeater++;
      break;
    case 'E': case 'e':
      handleMorseCommunication_character('E');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('E');
      morseRepeater++;
      break;
    case '-':
      handleMorseCommunication_character('-');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('-');
      morseRepeater++;
      break;
    case 'F': case 'f':
      handleMorseCommunication_character('F');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('F');
      morseRepeater++;
      break;
    case 'G': case 'g':
      handleMorseCommunication_character('G');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('G');
      morseRepeater++;
      break;
    case 'H': case 'h':
      handleMorseCommunication_character('H');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('H');
      morseRepeater++;
      break;
    case 'I': case 'i':
      handleMorseCommunication_character('I');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('I');
      morseRepeater++;
      break;
    case 'J': case 'j':
      handleMorseCommunication_character('J');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('J');
      morseRepeater++;
      break;
    case 'K': case 'k':
      handleMorseCommunication_character('K');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('K');
      morseRepeater++;
      break;
    case 'L': case 'l':
      handleMorseCommunication_character('L');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('L');
      morseRepeater++;
      break;
    case '!':
      handleMorseCommunication_character('!');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('!');
      morseRepeater++;
      break;
    case '?':
      handleMorseCommunication_character('?');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('?');
      morseRepeater++;
      break;
    case '.':
      handleMorseCommunication_character('.');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('.');
      morseRepeater++;
      break;
    case ',':
      handleMorseCommunication_character(',');
      lcd.setCursor(morseRepeater, 1);
      lcd.print(',');
      morseRepeater++;
      break;
    case ':':
      handleMorseCommunication_character(':');
      lcd.setCursor(morseRepeater, 1);
      lcd.print(':');
      morseRepeater++;
      break;
    case 'M': case 'm':
      handleMorseCommunication_character('M');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('M');
      morseRepeater++;
      break;
    case 'N': case 'n':
      handleMorseCommunication_character('N');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('N');
      morseRepeater++;
      break;
    case 'O': case 'o':
      handleMorseCommunication_character('O');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('O');
      morseRepeater++;
      break;
    case 'P': case 'p':
      handleMorseCommunication_character('P');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('P');
      morseRepeater++;
      break;
    case 'Q': case 'q':
      handleMorseCommunication_character('Q');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('Q');
      morseRepeater++;
      break;
    case 'R': case 'r':
      handleMorseCommunication_character('R');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('R');
      morseRepeater++;
      break;
    case 'S': case 's':
      handleMorseCommunication_character('S');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('S');
      morseRepeater++;
      break;
    case 'T': case 't':
      handleMorseCommunication_character('T');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('T');
      morseRepeater++;
      break;
    case 'U': case 'u':
      handleMorseCommunication_character('U');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('U');
      morseRepeater++;
      break;
    case 'V': case 'v':
      handleMorseCommunication_character('V');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('V');
      morseRepeater++;
      break;
    case 'W': case 'w':
      handleMorseCommunication_character('W');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('W');
      morseRepeater++;
      break;
    case 'X': case 'x':
      handleMorseCommunication_character('X');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('X');
      morseRepeater++;
      break;
    case 'Y': case 'y':
      handleMorseCommunication_character('Y');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('Y');
      morseRepeater++;
      break;
    case 'Z': case 'z':
      handleMorseCommunication_character('Z');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('Z');
      morseRepeater++;
      break;
    case '0':
      handleMorseCommunication_character('0');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('0');
      morseRepeater++;
      break;
    case '1':
      handleMorseCommunication_character('1');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('1');
      morseRepeater++;
      break;
    case '2':
      handleMorseCommunication_character('2');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('2');
      morseRepeater++;
      break;
    case '3':
      handleMorseCommunication_character('3');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('3');
      morseRepeater++;
      break;
    case '4':
      handleMorseCommunication_character('4');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('4');
      morseRepeater++;
      break;
    case '5':
      handleMorseCommunication_character('5');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('5');
      morseRepeater++;
      break;
    case '6':
      handleMorseCommunication_character('6');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('6');
      morseRepeater++;
      break;
    case '7':
      handleMorseCommunication_character('7');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('7');
      morseRepeater++;
      break;
    case '8':
      handleMorseCommunication_character('8');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('8');
      morseRepeater++;
      break;
    case '9':
      handleMorseCommunication_character('9');
      lcd.setCursor(morseRepeater, 1);
      lcd.print('9');
      morseRepeater++;
      break;
    case ' ':
      handleMorseCommunication_character(' ');
      break;
    default:
      break;
    }
  }
  delay(50);
}
void handleMorseCommunication_element(char element)
{
  int duration;
  if (element == '.')
  {
    duration = DOT_DURATION;
    tone(buzzer, NOTE_A3);
  }
  else if (element == '-')
  {
    duration = DASH_DURATION;
    tone(buzzer, NOTE_E3);
  }
  else
  {
    return;
  }
  digitalWrite(LED_green, HIGH);
  digitalWrite(LED_red, LOW);
  delay(duration);
  noTone(buzzer);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_red, HIGH);
}
void handleMorseCommunication_character(char character)
{
  char upperChar = character;
  sevenSegment_displayLetter(upperChar);
  if (upperChar == 'A' || upperChar == 'a')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("A");
    Serial1.println("A");
  }
  else if (upperChar == '-')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("-");
    Serial1.println("-");
  }
  else if (upperChar == 'B' || upperChar == 'b')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("B");
    Serial1.println("B");
  }
  else if (upperChar == 'C' || upperChar == 'c')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("C");
    Serial1.println("C");
  }
  else if (upperChar == 'D' || upperChar == 'd')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("D");
    Serial1.println("D");
  }
  else if (upperChar == 'E' || upperChar == 'e')
  {
    handleMorseCommunication_element('.');
    Serial.println("E");
    Serial1.println("E");
  }
  else if (upperChar == 'F' || upperChar == 'F')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("F");
    Serial1.println("F");
  }
  else if (upperChar == 'G' || upperChar == 'g')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("G");
    Serial1.println("G");
  }
  else if (upperChar == 'H' || upperChar == 'h')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("H");
    Serial1.println("H");
  }
  else if (upperChar == 'I' || upperChar == 'i')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("I");
    Serial1.println("I");
  }
  else if (upperChar == 'J'  || upperChar == 'j')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("J");
    Serial1.println("J");
  }
  else if (upperChar == 'K' || upperChar == 'k')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("K");
    Serial1.println("K");
  }
  else if (upperChar == 'L' || upperChar == 'l')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("L");
    Serial1.println("L");
  }
  else if (upperChar == 'M' || upperChar == 'm')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("M");
    Serial1.println("M");
  }
  else if (upperChar == 'N' || upperChar == 'n')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("N");
    Serial1.println("N");
  }
  else if (upperChar == 'O' || upperChar == 'o')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("O");
    Serial1.println("O");
  }
  else if (upperChar == 'P' || upperChar == 'p')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("P");
    Serial1.println("P");
  }
  else if (upperChar == 'Q' || upperChar == 'q')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("Q");
    Serial1.println("Q");
  }
  else if (upperChar == 'R' || upperChar == 'r')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("R");
    Serial1.println("R");
  }
  else if (upperChar == 'S' || upperChar == 's')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("S");
    Serial1.println("S");
  }
  else if (upperChar == 'T' || upperChar == 'T')
  {
    handleMorseCommunication_element('-');
    Serial.println("T");
    Serial1.println("T");
  }
  else if (upperChar == 'U' || upperChar == 'u')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("U");
    Serial1.println("U");
  }
  else if (upperChar == 'V' || upperChar == 'v')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("V");
    Serial1.println("V");
  }
  else if (upperChar == 'W' || upperChar == 'w')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("W");
    Serial1.println("W");
  }
  else if (upperChar == 'X' || upperChar == 'x')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("X");
    Serial1.println("X");
  }
  else if (upperChar == 'Y' || upperChar == 'y')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("Y");
    Serial1.println("Y");
  }
  else if (upperChar == 'Z' || upperChar == 'z')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("Z");
    Serial1.println("Z");
  }
  else if (upperChar == '1')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("1");
    Serial1.println("1");
  }
  else if (upperChar == '2')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("2");
    Serial1.println("2");
  }
  else if (upperChar == '3')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("3");
    Serial1.println("3");
  }
  else if (upperChar == '4')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("4");
    Serial1.println("4");
  }
  else if (upperChar == '5')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("5");
    Serial1.println("5");
  }
  else if (upperChar == '6')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("6");
    Serial1.println("6");
  }
  else if (upperChar == '7')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("7");
    Serial1.println("7");
  }
  else if (upperChar == '8')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("8");
    Serial1.println("8");
  }
  else if (upperChar == '9')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("9");
    Serial1.println("9");
  }
  else if (upperChar == '0')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("0");
    Serial1.println("0");
  }
  else if (upperChar == '!')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println("!");
    Serial1.println("!");
  }
  else if (upperChar == '?')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println("?");
    Serial1.println("?");
  }
  else if (upperChar == '.')
  {
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println(".");
    Serial1.println(".");
  }
  else if (upperChar == ',')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    Serial.println(",");
    Serial1.println(",");
  }
  else if (upperChar == ':')
  {
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('-');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    delay(ELEMENT_SPACE);
    handleMorseCommunication_element('.');
    Serial.println(":");
    Serial1.println(":");
  }
  else if (upperChar == ' ')
  {
    Serial.println("\n -End of message- \n");
    Serial1.println("\n -End of message- \n");
    lcd.setCursor(0, 1);
    lcd.print("~                ");
    morseRepeater = 1;
  }
  delay(CHAR_SPACE);
}
// Follow Me mode
void handleFollowMe()
{
  if (ensureSingleModeAccess == 1)
  {
    FOLLOW_TOLERANCE_CM=10;
    ensureSingleModeAccess = 0;
    Serial.println(" -- FOLLOW ME MODE TRIGGERED !");
    Serial1.println(" -- FOLLOW ME MODE TRIGGERED !");
  }
  unsigned long currentTime = millis();
  if (currentTime - lastJoystickAction >= 250) {
    int vrx = analogRead(joystick_VRX);
    if(vrx > 800 && (FOLLOW_TOLERANCE_CM<=40 && FOLLOW_TOLERANCE_CM>0)){
      FOLLOW_TOLERANCE_CM-=5;
      buzzer_activeBeep(50);
      delay(250);
    }
    else if(vrx<250 && (FOLLOW_TOLERANCE_CM>=0 && FOLLOW_TOLERANCE_CM<40)){
      FOLLOW_TOLERANCE_CM+=5;
      buzzer_activeBeep(50);
      delay(250);
    }
  }
  digitalWrite(LED_red, LOW);
  digitalWrite(LED_green, LOW);
  if (currentTime - lastPauseBlinkTime >= MAZE_PAUSE_BLINK_INTERVAL)
  {
    lastPauseBlinkTime = currentTime;
    isPauseBlinkOn = !isPauseBlinkOn;
    if (isPauseBlinkOn)
    {
      sevenSegment_displayLetter('F');
      digitalWrite(LED_blue, HIGH);
    }
    else
    {
      sevenSegment_clearAllSegments();
      digitalWrite(LED_blue, LOW);
    }
  }
  long currentDistance = ultrasonicSensor_measureDistance();
  lcd.setCursor(0, 0);
  lcd.print("Distance : ");
  if (currentDistance < 10)
    lcd.print("0");
  if (currentDistance < 100)
    lcd.print(currentDistance);
  else
    lcd.print("99");
  lcd.print(" cm");
  lcd.setCursor(0, 1);
  lcd.print("Threshold: " + String(FOLLOW_TOLERANCE_CM) + " cm      ");
  const int MIN_DISTANCE = FOLLOW_DISTANCE_CM - FOLLOW_TOLERANCE_CM;
  const int MAX_DISTANCE = FOLLOW_DISTANCE_CM + FOLLOW_TOLERANCE_CM;
  switch (currenthandleMaze_followState)
  {
  case FOLLOW_STATE_WAITING:
    motorsStop();
    break;
  case FOLLOW_STATE_FOLLOWING:
    if (currentDistance < MIN_DISTANCE)
    {
      motorsBackward();
    }
    else if (currentDistance > MAX_DISTANCE)
    {
      motorsForward();
    }
    else
    {
      motorsStop();
    }
    break;
  }
}
void saveVIN() {
  String VIN = "Robot DAMI 3000. Created & assembled by Nizar EL IDRYSY. Last code change was made on 01 July 2026 at 10:39AM.";
  char buffer[65]; 
  VIN.toCharArray(buffer, sizeof(buffer));
  EEPROM.put(EEPROMVIN, buffer);
  Serial.println(" -- " + String(VIN));
  Serial1.println(" -- " + String(VIN));
}
// Dice mode
void handleDice()
{
  static enum DiceState { WAITING,
                          ROLLING,
                          RESULT } currentDiceState = WAITING;
  static unsigned long rollStartTime = 0;
  const unsigned long ROLL_DURATION = 2500;
  if (ensureSingleModeAccess == 1)
  {
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    Serial.println(" -- DICE MODE TRIGGERED !");
    Serial1.println(" -- DICE MODE TRIGGERED !");
    lcd.clear();
    sevenSegment_clearAllSegments();
    motorsStop();
    LCD_displayMessageCentered("Dice Mode:", 0);
    LCD_displayMessageCentered("\x7EJoystick\x7F", 1);
    buzzer_activeBeep(100);
    sevenSegment_displayLetter('D');
    digitalWrite(LED_blue, HIGH);
    delay(100);
    digitalWrite(LED_blue, LOW);
    sevenSegment_clearAllSegments();
    buzzer_activeBeep(100);
    digitalWrite(LED_blue, HIGH);
    sevenSegment_displayLetter('D');
    motorsStop();
    ensureSingleModeAccess = 0;
    currentDiceState = WAITING;
  }
  char customKey = keypad.getKey();
  if (customKey == 'D')
  {
    handleEmergency();
    currentMode = NONE;
    return;
  }
  switch (currentDiceState)
  {
  case WAITING:
  {
    if ((digitalRead(joystick_button) == LOW) || (customKey == '4'))
    {
      currentDiceState = ROLLING;
      rollStartTime = millis();
      randomSeed(analogRead(A0));
      buzzer_activeBeep(50);
    }
    break;
  }
  case ROLLING:
  {
    Serial.println("\n -- Rolling...");
    Serial1.println("\n -- Rolling...");
    unsigned long currentTime = millis();
    if (currentTime - rollStartTime < ROLL_DURATION)
    {
      int currentDigit = (currentTime / 50) % 10;
      sevenSegment_displayDigit(currentDigit);
      buzzer_activeBeep();
      if ((currentTime / 100) % 2 == 0)
      {
        lcd.setCursor(0, 1);
        lcd.print("   ROLLING...   ");
        digitalWrite(LED_blue, HIGH);
      }
      else
      {
        lcd.setCursor(0, 1);
        lcd.print("                ");
        digitalWrite(LED_blue, LOW);
      }
    }
    else
    {
      digitalWrite(LED_blue, LOW);
      int final_roll = random(0, 10);
      Serial.println("\n -- Result is : \n");
      Serial1.println("\n -- Result is : \n");
      Serial.println(final_roll);
      Serial1.println(final_roll);
      sevenSegment_displayDigit(final_roll);
      lcd.clear();
      LCD_displayMessageCentered("ROLL COMPLETE!", 0);
      LCD_displayMessageCentered("Result: " + String(final_roll), 1);
      digitalWrite(LED_green, HIGH);
      tone(buzzer, NOTE_C6);
      currentDiceState = RESULT;
      rollStartTime = millis();
      delay(1000);
    }
    noTone(buzzer);
    break;
  }
  case RESULT:
  {
    if (millis() - rollStartTime >= 3000)
    {
      sevenSegment_clearAllSegments();
      lcd.clear();
      Serial.println("\n -- DICE MODE READY !");
      Serial1.println("\n -- DICE MODE READY !");
      LCD_displayMessageCentered("Dice Mode:", 0);
      LCD_displayMessageCentered("\x7EJoystick \x7ERun", 1);
      digitalWrite(LED_green, LOW);
      buzzer_activeBeep(100);
      sevenSegment_displayLetter('D');
      digitalWrite(LED_blue, HIGH);
      delay(100);
      digitalWrite(LED_blue, LOW);
      sevenSegment_clearAllSegments();
      buzzer_activeBeep(100);
      digitalWrite(LED_blue, HIGH);
      sevenSegment_displayLetter('D');
      currentDiceState = WAITING;
    }
    break;
  }
  }
}
// Credits mode
void handleCredits()
{
  if (ensureSingleModeAccess == 1)
  {
    lcd.clear();
    LCD_displayMessageCentered("Showing credits", 0);
    LCD_displayMessageCentered("of DAMI 3000 \x7E", 1);
    ensureSingleModeAccess = 0;
    Serial.println(" -- CREDITS MODE TRIGGERED !");
    Serial1.println(" -- CREDITS MODE TRIGGERED !");
  }
  sevenSegment_clearAllSegments();
  motorsStop();
  myServo.write(90);
  digitalWrite(LED_red, LOW);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_blue, LOW);
  String creditsMessages[] = {
      "DAMI3000 Made by\nNizar EL IDRYSY",
      "Last change on\n01/7/26-10:39AM",
      "Contains the\nv2.0.ino code",
      String(handleCredits_codeLinesCounter_calculator) + " lines of\ndedicated coding",
      "Use CoolTerm on\nWindows for HC06",
      "Shout out to my\nbro Gemini ;)",
      "1500+ MAD of\nmaterials",
      "Presented at 9AM\n18 June 2025",
      "EMSI-T - 2AP-G3\nGraded 17/20",
      "Rated the best\nrobot among all",
      "Funded by aunt\nHanane <3",
      "Named after my\ndad Imad XD",
      "Jury Members :",
      "Pr. Bouazizi\nand",
      "Pr. Bendaoud\nKaoutar and",
      "Pr. Errezgouny\nAbderrachid.",
      "Shout out to my\nparents for",
      "their support &\nhelp & care <3",
      "More on LinkedIn :\n@Nizar EL IDRYSY",
      "DAMI 3000 !\nLoves you ALL :D"};
  const int numMessages = sizeof(creditsMessages) / sizeof(creditsMessages[0]);
  if (millis() - lastMessageTime >= 2500)
  {
    lastMessageTime = millis();
    lcd.clear();
    lcd.setCursor(0, 0);
    int newlinePos = creditsMessages[currentCreditMessage].indexOf('\n');
    if (newlinePos != -1)
    {
      lcd.print(creditsMessages[currentCreditMessage].substring(0, newlinePos));
      lcd.setCursor(0, 1);
      lcd.print(creditsMessages[currentCreditMessage].substring(newlinePos + 1));
      Serial.println(creditsMessages[currentCreditMessage].substring(0, newlinePos));
      Serial.println(creditsMessages[currentCreditMessage].substring(newlinePos + 1));
      Serial1.println(creditsMessages[currentCreditMessage].substring(0, newlinePos));
      Serial1.println(creditsMessages[currentCreditMessage].substring(newlinePos + 1));
    }
    else
    {
      LCD_displayMessageCentered(creditsMessages[currentCreditMessage], 0);
      Serial.println(creditsMessages[currentCreditMessage]);
      Serial1.println(creditsMessages[currentCreditMessage]);
    }
    currentCreditMessage++;
    if (currentCreditMessage >= numMessages)
    {
      currentCreditMessage = 0;
    }
  }
}
// Chrome Dinausor Game mode
void handleChromeDinausorGame()
{
  static byte dinoChar[8] = {0b00111, 0b00101, 0b00111, 0b00110, 0b10111, 0b11111, 0b01010, 0b01010};
  static byte cactusChar[8] = {0b00100, 0b00101, 0b10101, 0b10101, 0b11111, 0b00100, 0b00100, 0b00100};
  sevenSegment_clearAllSegments();
  static bool gameOver = false;
  static int score = 0;
  static int obstaclePos = 15;
  static bool isJumping = false;
  static unsigned long jumpStartTime = 0;
  static unsigned long lastObstacleMoveTime = 0;
  static int lastDinoRow = 1;
  digitalWrite(LED_red, LOW);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_blue, LOW);
  if (ensureSingleModeAccess == 1)
  {
    lcd.createChar(0, dinoChar);
    lcd.createChar(1, cactusChar);
    sevenSegment_clearAllSegments();
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    lcd.clear();
    gameOver = false;
    score = 0;
    obstaclePos = 15;
    isJumping = false;
    lastDinoRow = 1;
    lastObstacleMoveTime = millis();
    ensureSingleModeAccess = 0;
    Serial.println(" -- CHROME DINOSAUR GAME TRIGGERED !");
    Serial1.println(" -- CHROME DINOSAUR GAME TRIGGERED !");
  }
  if (gameOver)
  {
    if(score>HighScore){
      HighScore=score;
      EEPROM.put(EEPROMHighScore, score);
    }
    digitalWrite(sevenSegmentPin[2], HIGH);
    digitalWrite(sevenSegmentPin[3], HIGH);
    digitalWrite(sevenSegmentPin[4], HIGH);
    digitalWrite(sevenSegmentPin[6], HIGH);
    digitalWrite(LED_red, HIGH);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    LCD_displayMessageCentered("GAME OVER!", 0);
    LCD_displayMessageCentered("Score: " + String(score) + " HS: "+ String(HighScore), 1);
    delay(2500);
    digitalWrite(sevenSegmentPin[2], LOW);
    digitalWrite(sevenSegmentPin[3], LOW);
    digitalWrite(sevenSegmentPin[4], LOW);
    digitalWrite(sevenSegmentPin[6], LOW);
      ensureSingleModeAccess = 1;
      delay(300);
    return;
  }
  int yVal = analogRead(joystick_VRY);
  if ((yVal < 200 || yVal > 800 || digitalRead(joystick_button) == LOW) && !isJumping)
  {
    tone(buzzer, NOTE_E5, 50);
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, HIGH);
    digitalWrite(sevenSegmentPin[6], HIGH);
    digitalWrite(LED_blue, LOW);
    isJumping = true;
    jumpStartTime = millis();
  }
  if (isJumping && (millis() - jumpStartTime > 650))
  {
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(sevenSegmentPin[6], LOW);
    digitalWrite(LED_blue, LOW);
    isJumping = false;
  }
  int dinoRow = isJumping ? 0 : 1;
  if (dinoRow != lastDinoRow)
  {
    lcd.setCursor(2, lastDinoRow);
    lcd.print(" ");
    lcd.setCursor(2, dinoRow);
    lcd.write(0);
    lastDinoRow = dinoRow;
  }
  int gameSpeed = max(50, 300 - (score * 10));
  if (millis() - lastObstacleMoveTime > gameSpeed)
  {
    lastObstacleMoveTime = millis();
    if (obstaclePos != 2 || isJumping)
    {
      lcd.setCursor(obstaclePos, 1);
      lcd.print(" ");
    }
    obstaclePos--;
    if (obstaclePos < 0)
    {
      obstaclePos = 15;
      score++;
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, HIGH);
      tone(buzzer, NOTE_E5, 50);
      digitalWrite(sevenSegmentPin[1], HIGH);
      digitalWrite(sevenSegmentPin[2], HIGH);
      delay(120);
      tone(buzzer, NOTE_C6, 50);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[1], LOW);
      digitalWrite(sevenSegmentPin[2], LOW);
    }
    lcd.setCursor(obstaclePos, 1);
    lcd.write(1);
    lcd.setCursor(2, dinoRow);
    lcd.write(0);
    lcd.setCursor(4, 0);
    lcd.print("Score: ");
    lcd.setCursor(11, 0);
    lcd.print(score);
    if (obstaclePos == 2 && !isJumping)
    {
      gameOver = true;
      tone(buzzer, NOTE_G3, 300);
      delay(300);
      tone(buzzer, NOTE_C3, 500);
    }
  }
}
// Stopwatch And Timer mode
void handleStopwatchAndTimer() {
  sevenSegment_clearAllSegments();
  digitalWrite(LED_red, LOW);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_blue, LOW);
  static SubModeState subState = SUBMODE_SELECT;
  static Mode lastMode = NONE;
  static unsigned long modeStartTime = 0;
  static unsigned long lastJoystickAction = 0;
  static unsigned long lastDisplayUpdate = 0;
  static int lastPrintedSec = -1;
  static int timerHours = 0;
  static int timerMinutes = 0;
  static int timerSeconds = 0;
  static int activeUnit = 2;
  if ((lastMode != currentMode) || !counterStopwatchAndTimer) {
    subState = SUBMODE_SELECT;
    timerHours = 0;
    timerMinutes = 0;
    timerSeconds = 0;
    activeUnit = 2;
    lastPrintedSec = -1;
    sevenSegment_clearAllSegments();
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    lcd.clear();
    LCD_displayMessageCentered("Select a Mode :", 0);
    LCD_displayMessageCentered("Stopwatch\x7F\x7ETimer", 1);
    lastMode = currentMode;
    counterStopwatchAndTimer=!counterStopwatchAndTimer;
  }
  char key = keypad.getKey();
  unsigned long currentMillis = millis();
  switch (subState) {
    case SUBMODE_SELECT: {
      if (currentMillis - lastJoystickAction >= 250) {
        int vrx = analogRead(joystick_VRX);
        if (vrx < 250) {
          lastJoystickAction = currentMillis;
          buzzer_activeBeep(80);
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Elapsed:"); 
          LCD_displayMessageCentered("\x7EJS to Terminate", 1); 
          modeStartTime = currentMillis;
          lastPrintedSec = -1; 
          subState = STOPWATCH_RUNNING;
        } else if (vrx > 800) {
          lastJoystickAction = currentMillis;
          buzzer_activeBeep(80);
          lcd.clear();
          lastDisplayUpdate = 0; 
          subState = TIMER_CONFIG;
        }
      }
      break;
    }
    case STOPWATCH_RUNNING: {
      if (digitalRead(joystick_button) == LOW) {
        buzzer_activeBeep(100);
        subState = SUBMODE_SELECT; 
        lastMode = NONE;
        delay(300); 
        return;
      }
      unsigned long elapsedSecs = (currentMillis - modeStartTime) / 1000;
      int displaySecs = elapsedSecs % 60;
      if (displaySecs != lastPrintedSec) {
        lastPrintedSec = displaySecs;
        int displayHours = elapsedSecs / 3600;
        int displayMins = (elapsedSecs / 60) % 60;       
        lcd.setCursor(8, 0);
        char timeBuf[9];
        sprintf(timeBuf, "%02d:%02d:%02d", displayHours, displayMins, displaySecs);
        lcd.print(timeBuf);
      }
      break;
    }
    case TIMER_CONFIG: {
      bool actionTaken = false;
      if (currentMillis - lastJoystickAction >= 250) {
        int vry = analogRead(joystick_VRY);
        int vrx = analogRead(joystick_VRX);
        if (vrx < 250) { 
          activeUnit = (activeUnit + 1) % 3;
          actionTaken = true;
        } else if (vrx > 800) {
          activeUnit = (activeUnit - 1 + 3) % 3;
          actionTaken = true;
        }
        if (vry < 250) { 
          if (activeUnit == 0) timerHours = (timerHours + 1) % 24;
          if (activeUnit == 1) timerMinutes = (timerMinutes + 1) % 60;
          if (activeUnit == 2) timerSeconds = (timerSeconds + 1) % 60;
          actionTaken = true;
        } else if (vry > 800) { 
          if (activeUnit == 0) timerHours = (timerHours - 1 + 24) % 24;
          if (activeUnit == 1) timerMinutes = (timerMinutes - 1 + 60) % 60;
          if (activeUnit == 2) timerSeconds = (timerSeconds - 1 + 60) % 60;
          actionTaken = true;
        }
        if (actionTaken) {
          lastJoystickAction = currentMillis;
          tone(buzzer, 1500, 20);
        }
      }
      if (actionTaken || lastDisplayUpdate == 0) {
        lastDisplayUpdate = currentMillis;
        char configBuffer[16];
        sprintf(configBuffer, "%02dh:%02dm:%02ds", timerHours, timerMinutes, timerSeconds);
        LCD_displayMessageCentered(configBuffer, 0);
        if (activeUnit == 0)       LCD_displayMessageCentered("   [Hours]    ", 1);
        else if (activeUnit == 1)  LCD_displayMessageCentered("  [Minutes]   ", 1);
        else if (activeUnit == 2)  LCD_displayMessageCentered("  [Seconds]   ", 1);
      }
      if (digitalRead(joystick_button) == LOW) {
        buzzer_activeBeep(150);
        lcd.clear();   
        lcd.setCursor(0, 0);
        lcd.print("Elapsed:");
        LCD_displayMessageCentered("\x7EJS to Terminate", 1);       
        modeStartTime = currentMillis;
        lastPrintedSec = -1; 
        subState = TIMER_RUNNING;
      }
      break;
    }
    case TIMER_RUNNING: {
      if (digitalRead(joystick_button) == LOW) {
        buzzer_activeBeep(100);
        subState = SUBMODE_SELECT; 
        lastMode = NONE;
        delay(300);
        return;
      }
      long totalDurationSeconds = ((long)timerHours * 3600) + (timerMinutes * 60) + timerSeconds;
      long elapsedSeconds = (currentMillis - modeStartTime) / 1000;
      long remainingSeconds = totalDurationSeconds - elapsedSeconds;
      if (remainingSeconds <= 0) {
        lcd.clear();
        lastDisplayUpdate = 0; 
        subState = TIMER_FINISHED;
      } else {
        int remS = remainingSeconds % 60;
        if (remS != lastPrintedSec) {
          lastPrintedSec = remS;
          int remH = remainingSeconds / 3600;
          int remM = (remainingSeconds % 3600) / 60;
          lcd.setCursor(8, 0);
          char timeBuf[9];
          sprintf(timeBuf, "%02d:%02d:%02d", remH, remM, remS);
          lcd.print(timeBuf);
        }
      }
      break;
    }
    case TIMER_FINISHED: {
      if (currentMillis - lastDisplayUpdate >= 500) {
        lastDisplayUpdate = currentMillis;
        static bool flashToggle = false;
        flashToggle = !flashToggle;
        if (flashToggle) {
          digitalWrite(sevenSegmentPin[6], HIGH);
          digitalWrite(LED_red , HIGH);
          tone(buzzer, 2000, 400);
          LCD_displayMessageCentered("  TIME'S UP!  ", 0);
          LCD_displayMessageCentered("\x7EJS to Terminate", 1);
        } else {
          LCD_displayMessageCentered("                ", 0);
          LCD_displayMessageCentered("                ", 1);
          digitalWrite(sevenSegmentPin[6], LOW);
          digitalWrite(LED_red , LOW);
        }
      }
      if (digitalRead(joystick_button) == LOW) {
        buzzer_activeBeep(100);
        subState = SUBMODE_SELECT;
        lastMode = NONE;
      }
      break;
    }
  }
}
// Calculator mode
void handleCalculator(char key) {
  static decltype(currentMode) lastMode;
  static char num1[8] = "";      
  static char num2[8] = "";      
  static char activeOp = ' ';
  static bool hasOperator = false;
  static bool showingResult = false;
  static char resultStr[12] = "";
  if(firstRun || lastMode != currentMode){
  num1[0] = '\0'; num2[0] = '\0';
  activeOp = ' '; hasOperator = false; showingResult = false; resultStr[0] = '\0';
  sevenSegment_clearAllSegments();
  digitalWrite(LED_red, LOW); digitalWrite(LED_green, LOW); digitalWrite(LED_blue, LOW);
  lcd.clear();
  LCD_displayMessageCentered("Calculator Mode:", 0);
  LCD_displayMessageCentered("\x7E Enter a value", 1);
  lastMode = currentMode;
  firstRun = false;
  }
  if (!key) return; 
  tone(buzzer, 1500, 20); 
  if (key >= '0' && key <= '9') {
    if (showingResult) {
      num1[0] = key; num1[1] = '\0';
      num2[0] = '\0'; activeOp = ' ';
      hasOperator = false; showingResult = false;
    } else if (!hasOperator) {
      int len = strlen(num1);
      if (len < 6) { num1[len] = key; num1[len + 1] = '\0'; }
    } else {
      int len = strlen(num2);
      if (len < 6) { num2[len] = key; num2[len + 1] = '\0'; }
    }
  } 
  else if (key == 'A' || key == 'B' || key == 'C' || key == '*') {
    char displayOp = (key == 'A') ? '+' : (key == 'B') ? '-' : (key == 'C') ? '*' : '/';
    if (showingResult) {
      if (strcmp(resultStr, "Error") != 0) {
        strncpy(num1, resultStr, sizeof(num1) - 1);
        num1[sizeof(num1) - 1] = '\0';
        num2[0] = '\0'; activeOp = displayOp;
        hasOperator = true; showingResult = false;
      }
    } else if (hasOperator && strlen(num1) > 0 && strlen(num2) > 0) {
      float n1 = atof(num1); float n2 = atof(num2);
      float intermediate = (activeOp == '+') ? n1 + n2 : (activeOp == '-') ? n1 + n2 : (activeOp == '*') ? n1 * n2 : (n2 != 0) ? n1 / n2 : 0;
      if (activeOp == '/' && n2 == 0.0) {
        strcpy(resultStr, "Error"); showingResult = true;
      } else {
        if (intermediate == (long)intermediate) ltoa((long)intermediate, num1, 10);
        else dtostrf(intermediate, 1, 2, num1);
        activeOp = displayOp;
        num2[0] = '\0';
      }
    } else if (strlen(num1) > 0) {
      activeOp = displayOp;
      hasOperator = true;
    }
  } 
  else if (key == '#') {
    if (hasOperator && strlen(num1) > 0 && strlen(num2) > 0) { 
      float n1 = atof(num1); float n2 = atof(num2);
      float calcResult = 0; bool valid = true;
      if (activeOp == '+') calcResult = n1 + n2;
      else if (activeOp == '-') calcResult = n1 - n2;
      else if (activeOp == '*') calcResult = n1 * n2;
      else if (activeOp == '/') { if (n2 == 0.0) valid = false; else calcResult = n1 / n2; }
      if (valid) {
        if (calcResult == (long)calcResult) ltoa((long)calcResult, resultStr, 10);
        else dtostrf(calcResult, 1, 2, resultStr);
      } else {
        strcpy(resultStr, "Error");
      }
      showingResult = true;
    }
  } 
  else if (key == 'D') {
    num1[0] = '\0'; num2[0] = '\0'; activeOp = ' ';
    hasOperator = false; showingResult = false; resultStr[0] = '\0';
  }
  lcd.setCursor(0, 1);
  int printedChars = 0;
  if (showingResult) {
    int totalLen = strlen(num1) + 1 + strlen(num2) + 1 + strlen(resultStr);
    if (totalLen <= 16) {
      lcd.print(num1); lcd.print(activeOp); lcd.print(num2); lcd.print('='); lcd.print(resultStr);
      printedChars = totalLen;
    } else {
      lcd.print('='); lcd.print(resultStr);
      printedChars = 1 + strlen(resultStr);
    }
  } else {
    if (strlen(num1) == 0) { lcd.print("0"); printedChars = 1; } 
    else { lcd.print(num1); printedChars = strlen(num1); }
    if (hasOperator) {
      lcd.print(activeOp); printedChars += 1;
      if (strlen(num2) > 0) { lcd.print(num2); printedChars += strlen(num2); }
    }
  }
  for (int i = printedChars; i < 16; i++) {
    lcd.print(' ');
  }
}
// Simon Says mode
void playSimonColor(int color, int duration) {
  int ledPin = -1;
  int frequency = 0;
  if (color == 0) {
    ledPin = LED_red;
    LCD_displayMessageCentered("\x7E", 1);
    frequency = 400;
  } else if (color == 1) {
    ledPin = LED_green;
    LCD_displayMessageCentered("^", 1);
    frequency = 500;
  } else if (color == 2) {
    ledPin = LED_blue;
    LCD_displayMessageCentered("\x7F", 1);
    frequency = 600;
  }
  if (ledPin != -1) {
    digitalWrite(ledPin, HIGH);
    tone(buzzer, frequency, duration);
    delay(duration);
    digitalWrite(ledPin, LOW);
    noTone(buzzer);
  }
}
void handleSimonSays() {
  int sequence[50];
  int currentRound = 0;
  bool gameOver = false;
  lcd.clear();
  LCD_displayMessageCentered("Simon Says", 0);
  LCD_displayMessageCentered("Game !", 1);
  delay(1500);
  randomSeed(micros());
  while (!gameOver) {
    lcd.clear();
    LCD_displayMessageCentered("Score: ", 0);
    lcd.print(currentRound);
    sequence[currentRound] = random(0, 3);
    currentRound++;
    LCD_displayMessageCentered(" ", 1);
    int stepDelay = 600 - (currentRound * 35);
    if (stepDelay < 150) {
      stepDelay = 150;
    }
    for (int i = 0; i < currentRound; i++) {
      delay(stepDelay / 2);
      playSimonColor(sequence[i], stepDelay);
    }
    for (int i = 0; i < currentRound; i++) {
      int expectedColor = sequence[i];
      int playerInput = -1;
      while (playerInput == -1) {
        char customKey = keypad.getKey();
        if (customKey == 'D') {
          handleEmergency();
          return;
        }
        int xVal = analogRead(joystick_VRX);
        int yVal = analogRead(joystick_VRY);
        if (xVal < 200) {
          playerInput = 0;
        } else if (yVal > 800 || yVal < 200) {
          playerInput = 1;
        } else if (xVal > 800) {
          playerInput = 2;
        }
        delay(10);
      }
      playSimonColor(playerInput, 250);
      if (playerInput != expectedColor) {
        gameOver = true;
        break;
      }
      while (analogRead(joystick_VRX) < 200 || analogRead(joystick_VRX) > 800 || analogRead(joystick_VRY) > 800) {
        char checkKey = keypad.getKey();
        if (checkKey == 'D') {
          handleEmergency();
          return;
        }
        delay(10);
      }
      delay(50);
    }
    if (!gameOver) {
      delay(800);
    }
  }
  lcd.clear();
  LCD_displayMessageCentered("Game Over!", 0);
  lcd.setCursor(0, 1);
  if(currentRound>HighScoreSimon){
    HighScoreSimon=currentRound-1;
    EEPROM.put(EEPROMHighScoreSimon, currentRound - 1);
  }
  lcd.print("Score: " + String(currentRound - 1) + " HS: " + String(HighScoreSimon));
  tone(buzzer, 150, 1000);
  delay(2500);
  noTone(buzzer);
  lcd.clear();
  return;
}
// Sleepy Radar mode
void handleSleepyRadar()
{
  bool OD = false;
  bool LED_ON = false;
  lcd.clear();
  LCD_displayMessageCentered("Sleepy Radar", 0);
  LCD_displayMessageCentered("Initiated!", 1);
  delay(1500);
  int threshold = 15;
  int servoPos = 90;
  int sweepDirection = 1;
  unsigned long lastServoTime = 0;
  lcd.noBacklight();
  while (currentMode == SLEEPY_RADAR) {
    int xVal = analogRead(joystick_VRX);
    if (xVal < 200) {
      threshold = max(5, threshold + 1);
      lcd.backlight();
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Threshold:" + String(threshold) + "(+1)");
      lcd.print("cm");
      delay(100);
      lcd.noBacklight();
    } else if (xVal > 800) {
      threshold = min(100, threshold - 1);
      lcd.backlight();
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Threshold: " + String(threshold) + "(-1)");
      lcd.print("cm");
      delay(100);
      lcd.noBacklight();
    }
    if ((millis() - lastServoTime > 30) && !OD) {
      lastServoTime = millis();
      servoPos += sweepDirection;
      myServo.write(servoPos);
      if ((servoPos >= 160 || servoPos <= 20) && !OD) {
        sweepDirection = -sweepDirection;
      }
    }
    long distance = ultrasonicSensor_measureDistance();
    if (distance > 0 && distance <= threshold) {
      lcd.backlight();
      lcd.clear();
      digitalWrite(LED_green , LOW);
      LED_ON=false;
      lcd.setCursor(0, 0);
      lcd.print("Object Detected!");
      LCD_displayMessageCentered("Dist: " + String(distance) + "cm", 1);
      tone(buzzer, 1000, 200);
      digitalWrite(LED_red, HIGH);
      for (int i = 1; i < 5; i++)
        {
          digitalWrite(sevenSegmentPin[i], HIGH);
        }
        digitalWrite(sevenSegmentPin[6], HIGH);
      delay(800);
      digitalWrite(LED_red, LOW);
      for (int i = 1; i < 5; i++)
        {
          digitalWrite(sevenSegmentPin[i], LOW);
        }
        digitalWrite(sevenSegmentPin[6], LOW);
      lcd.noBacklight();
      delay(200);
      OD = true;
    }
    else{
      lcd.noBacklight();
      lcd.setCursor(0, 0);
      lcd.print("  Scanning ...  ");
      lcd.setCursor(0, 0);
      if(digitalRead(joystick_button) == LOW && LED_ON == false){
        digitalWrite(LED_green , HIGH);
        LED_ON=true;
        delay(200);
      } else if(digitalRead(joystick_button) == LOW && LED_ON == true){
        digitalWrite(LED_green , LOW);
        LED_ON=false;
        delay(200);
      }
      lcd.setCursor(0, 1);
      lcd.print("No obj in sight.");
      OD = false;
    }
    char key = keypad.getKey();
    if (key == 'D') {
      lcd.backlight();
      handleEmergency();
      return;
    }
    delay(10);
  }
  lcd.backlight();
}
// Melodies mode
void handleMelodies()
{
  lcd.clear();
  LCD_displayMessageCentered("Play Notes in", 0);
  LCD_displayMessageCentered("Piano Mode !", 1);
  delay(1500);
  lcd.clear();
  LCD_displayMessageCentered("Start Playing!", 0);
  LCD_displayMessageCentered("\x7EUse Keypad\x7F", 1);
  char currentNote = '\0';
  int octaveShift = 0; 
  bool joystickReset = true;
  while (currentMode == MELODIES) {
    int yVal = analogRead(joystick_VRY);
    if (joystickReset) {
      if (yVal < 200 && octaveShift!=5) {
        tone(buzzer, 1500, 20);
        octaveShift++;
        joystickReset = false;
      } else if (yVal > 800 && octaveShift!=-5) {
        tone(buzzer, 1500, 20);
        octaveShift--;
        joystickReset = false;
      }
    } else if (yVal >= 400 && yVal <= 600) {
      joystickReset = true;
    }
    if (digitalRead(joystick_button) == LOW) {
      tone(buzzer, 1500, 20);
      octaveShift = 0;
    }
    char key = keypad.getKey();
    KeyState state = keypad.getState();
    if (key != NO_KEY) {
      if (key == 'D') {
        noTone(buzzer);
        handleEmergency();
        return;
      }
      currentNote = key;
    }
    if (currentNote != '\0') {
      if (state == PRESSED || state == HOLD) {
        float baseFrequency = 0;
        switch (currentNote) {
          case '1': baseFrequency = 261.63; break; // C4
          case '2': baseFrequency = 293.66; break; // D4
          case '3': baseFrequency = 329.63; break; // E4
          case 'A': baseFrequency = 349.23; break; // F4
          case '4': baseFrequency = 392.00; break; // G4
          case '5': baseFrequency = 440.00; break; // A4
          case '6': baseFrequency = 493.88; break; // B4
          case 'B': baseFrequency = 523.25; break; // C5
          case '7': baseFrequency = 587.33; break; // D5
          case '8': baseFrequency = 659.25; break; // E5
          case '9': baseFrequency = 698.46; break; // F5
          case 'C': baseFrequency = 783.99; break; // G5
          case '*': baseFrequency = 880.00; break; // A5
          case '0': baseFrequency = 987.77; break; // B5
          case '#': baseFrequency = 1046.50; break; // C6
        }
        if (baseFrequency > 0) {
          int finalFrequency = baseFrequency * pow(2, octaveShift);
          finalFrequency = constrain(finalFrequency, 31, 4978); 
          tone(buzzer, finalFrequency);
          lcd.setCursor(0, 0);
          lcd.print("Note: ");
          lcd.print(currentNote);
          lcd.print(" Oct: ");
          if(octaveShift >= 0) lcd.print("+");
          lcd.print(octaveShift);
        }
      } 
      else if (state == RELEASED) {
        noTone(buzzer);
        currentNote = '\0';
      }
    }
    delay(10);
  }
}
// Reaction mode
void handleReaction()
{
  lcd.clear();
  LCD_displayMessageCentered("Reaction Test", 0);
  LCD_displayMessageCentered("Get Ready...", 1);
  randomSeed(micros());
  unsigned long randomDelay = random(2500, 6001);
  unsigned long startWait = millis();
  while (millis() - startWait < randomDelay) {
    keypad.getKey(); 
    digitalRead(joystick_button);
    char checkKey = keypad.getKey();
    if (checkKey == 'D') {
      handleEmergency();
      return;
    }
    delay(1);
  }
  lcd.clear();
  LCD_displayMessageCentered("PRESS NOW!", 0);
  LCD_displayMessageCentered("\x7EJS   \x7Es1", 1);
  tone(buzzer, 1000, 150); 
  unsigned long startTime = millis();
  unsigned long reactionTime = 0;
  bool pressed = false;
  while (currentMode == REACTION && !pressed) {
    char key = keypad.getKey();
    if (key == 'D') {
      noTone(buzzer);
      handleEmergency();
      return;
    }
    if (key == '1' || digitalRead(joystick_button) == LOW) {
      reactionTime = millis() - startTime;
      pressed = true;
    }
  }
  unsigned long totalSeconds = reactionTime / 1000;
  unsigned long minutes = totalSeconds / 60;
  unsigned long seconds = totalSeconds % 60;
  unsigned long milliseconds = reactionTime % 1000;
  // Display results
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("ELP:");
  lcd.print(minutes);
  lcd.print("m ");
  lcd.print(seconds);
  lcd.print("s ");
  lcd.print(milliseconds);
  lcd.print("ms");
  unsigned long currentHighScoreTotalMs = (HighScoreReactionMinutes * 60000) + (HighScoreReactionSeconds * 1000) + HighScoreReactionMilliseconds;
  if(reactionTime < currentHighScoreTotalMs || currentHighScoreTotalMs == 0){
    HighScoreReactionMinutes=minutes;
    EEPROM.put(EEPROMHighScoreReactionMinutes, HighScoreReactionMinutes);
    HighScoreReactionSeconds=seconds;
    EEPROM.put(EEPROMHighScoreReactionSeconds, HighScoreReactionSeconds);
    HighScoreReactionMilliseconds=milliseconds;
    EEPROM.put(EEPROMHighScoreReactionMilliseconds, HighScoreReactionMilliseconds);
  }
  lcd.setCursor(0, 1);
  lcd.print("\x7EHS:");
  lcd.print(HighScoreReactionMinutes);
  lcd.print("m ");
  lcd.print(HighScoreReactionSeconds);
  lcd.print("s ");
  lcd.print(HighScoreReactionMilliseconds);
  lcd.print("ms");
  delay(1000); 
  while (currentMode == REACTION) {
    char exitKey = keypad.getKey();
    if (exitKey == 'D') {
      handleEmergency();
      return;
    } else if (exitKey != NO_KEY || digitalRead(joystick_button) == LOW) {
      break;
    }
  }
  lcd.clear();
}
// Off mode
void handleOFF_buzzer_passiveBeep()
{
  const int LEVEL_CLEAR_TEMPO = 150;
  int creditsMelody[] = {
    NOTE_C5, NOTE_B4, NOTE_A4, NOTE_G4, NOTE_F4, NOTE_E4, NOTE_D4, NOTE_B3, NOTE_C4};
  int creditsDurations[] = {
      8, 4, 8, 4, 8, 8, 8, 8, 1};
  int notes = sizeof(creditsMelody) / sizeof(creditsMelody[0]);
  for (int i = 0; i < notes; i++)
  {
    long duration = (LEVEL_CLEAR_TEMPO * 4) / creditsDurations[i];
    if (creditsMelody[i] != NOTE_REST)
    {
      tone(buzzer, creditsMelody[i], duration);
    }
    long pauseBetweenNotes = duration * 1.30;
    delay(pauseBetweenNotes);
    noTone(buzzer);
  }
}
void handleOff()
{
  if(!offTriggered){
    Serial.println(" -- EMERGENCY MODE TRIGGERED !");
  Serial1.println(" -- EMERGENCY MODE TRIGGERED !");
    lcd.clear();
    LCD_displayMessageCentered("You can turn the", 0);
    LCD_displayMessageCentered("Robot OFF Safely.", 1);
    firstRun = true;
    menuPage = 1;
    counterStopwatchAndTimer=false;
    lastMode = NONE;
    timerHours = 0;
    timerMinutes = 0;
    timerSeconds = 0;
    activeUnit = 2;
    lastPrintedSec = -1;
    motorsStop();
    myServo.write(90);
    sevenSegment_clearAllSegments();
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    servoPos = 90;
    isMazePaused = false;
    ensureSingleModeAccess = 1;
    handleOFF_buzzer_passiveBeep();
    delay(7000);
    lcd.clear();
    lcd.noBacklight();
    offTriggered=true;
  }
}
// Manual Test mode
void handleManualTest(char customKey)
{
  if (ensureSingleModeAccess == 1)
  {
    ensureSingleModeAccess = 0;
    Serial.println(" -- MANUAL TESTING MODE TRIGGERED !");
    Serial1.println(" -- MANUAL TESTING MODE TRIGGERED !");
  }
  if (manTstJustEntered)
  {
    sevenSegment_clearAllSegments();
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    lcd.setCursor(0, 1);
    lcd.print("     Ready!");
    motorsStop();
    myServo.write(90);
    digitalWrite(buzzer, LOW);
    sevenSegIsOn = false;
    manTstJustEntered = false;
  }
  unsigned long currentTime = millis();
  if (!customKey && currentTime - lastDistanceUpdateTime >= DISTANCE_UPDATE_INTERVAL)
  {
    lastDistanceUpdateTime = currentTime;
    long distanceCm = ultrasonicSensor_measureDistance();
    if (distanceCm > 99)
    {
      distanceCm = 99;
    }
    char distBuffer[17];
    snprintf(distBuffer, sizeof(distBuffer), "Distance : %02ld cm", distanceCm);
    lcd.setCursor(0, 0);
    lcd.print(distBuffer);
  }
  if (customKey)
  {
    if (customKey == '8')
    {
      motorsForward();
      updateMovementState(" MOTORS FORWARD");
    }
    else if (customKey == '4')
    {
      motorsRight();
      updateMovementState("MOTORS RIGHT");
    }
    else if (customKey == '6')
    {
      motorsLeft();
      updateMovementState("MOTORS LEFT");
    }
    else if (customKey == '2')
    {
      motorsBackward();
      updateMovementState("MOTORS BACKWARD");
    }
    else if (customKey == '5')
    {
      motorsStop();
      updateMovementState("ROBOT IDLE");
    }
    else if (customKey == 'A')
    {
      digitalWrite(LED_red, HIGH);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      updateMovementState("RED LED: ON");
    }
    else if (customKey == 'B')
    {
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_blue, LOW);
      updateMovementState(" GREEN LED: ON");
    }
    else if (customKey == 'C')
    {
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, HIGH);
      updateMovementState("BLUE LED: ON");
    }
    if (customKey == '9')
    {
      sevenSegIsOn = !sevenSegIsOn;
      if (sevenSegIsOn)
      {
        for (int i = 0; i < 7; i++)
        {
          digitalWrite(sevenSegmentPin[i], HIGH);
        }
        lcd.setCursor(0, 1);
        lcd.print("   7-SEG: ON       ");
      }
      else
      {
        sevenSegment_clearAllSegments();
        lcd.setCursor(0, 1);
        lcd.print("   7-SEG: OFF      ");
      }
    }
  }
  if (customKey == '0' || analogRead(joystick_VRY) > 950 || analogRead(joystick_VRY) < 150)
  {
    myServo.write(90);
    updateMovementState("SERVO CENTERED");
  }
  else if (customKey == '*' || analogRead(joystick_VRX) > 950)
  {
    myServo.write(0);
    updateMovementState("SERVO RIGHT");
  }
  else if (customKey == '#' || analogRead(joystick_VRX) < 150)
  {
    myServo.write(180);
    updateMovementState("SERVO LEFT");
  }
  else if (customKey == '3')
  {
    myServo.write(135);
    updateMovementState("SERVO left");
  }
  else if (customKey == '1')
  {
    myServo.write(45);
    updateMovementState("SERVO right");
  }
  if (customKey == '7' || digitalRead(joystick_button) == LOW)
  {
    buzzerIsOn = !buzzerIsOn;
    if (buzzerIsOn)
    {
      LCD_displayMessageCentered("Buzzer Testing", 1);
      for (int m = 0; m < 2000; m++)
      {
        tone(buzzer, m);
        delay(2);
      }
    }
    LCD_displayMessageCentered("Buzzer Test Done", 1);
    buzzerIsOn = !buzzerIsOn;
    noTone(buzzer);
  }
}
// Automatic Test mode
void handleAutomaticTest()
{
  if (ensureSingleModeAccess == 1)
  {
    ensureSingleModeAccess = 0;
    Serial.println(" -- AUTOMATIC TESTING MODE TRIGGERED !");
    Serial1.println(" -- AUTOMATIC TESTING MODE TRIGGERED !");
  }
  static unsigned long lastLedBlinkTime = 0;
  static bool ledState = LOW;
  static unsigned long lastBuzzerBeapTime = 0;
  static int rgbLedCycle = 0;
  if (millis() - lastBuzzerBeapTime >= 500)
  {
    lastBuzzerBeapTime = millis();
    sevenSegment_displayDigit(8);
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    if (rgbLedCycle == 0)
    {
      digitalWrite(LED_red, HIGH);
    }
    else if (rgbLedCycle == 1)
    {
      digitalWrite(LED_green, HIGH);
    }
    else if (rgbLedCycle == 2)
    {
      digitalWrite(LED_blue, HIGH);
    }
    buzzer_activeBeep(50);
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    rgbLedCycle = (rgbLedCycle + 1) % 3;
  }
  lcd.setCursor(0, 0);
  lcd.print("Distance : ");
  long distance = ultrasonicSensor_measureDistance();
  if (distance < 10)
  {
    lcd.print("0");
  }
  if (distance < 100)
    lcd.print(distance);
  else
    lcd.print("99");
  lcd.print(" cm");
  switch (currenthandleAutomaticTest_testState)
  {
  case TEST_STATE_INIT:
    motorsStop();
    myServo.write(90);
    updateMovementState("Testing...");
    currenthandleAutomaticTest_testState = TEST_STATE_FORWARD;
    testStateStartTime = millis();
    break;
  case TEST_STATE_FORWARD:
    motorsForward();
    if (millis() - testStateStartTime == 0)
      updateMovementState("Forward");
    if (millis() - testStateStartTime >= TEST_ACTION_DURATION)
    {
      motorsStop();
      currenthandleAutomaticTest_testState = TEST_STATE_BACKWARD;
      testStateStartTime = millis();
    }
    break;
  case TEST_STATE_BACKWARD:
    motorsBackward();
    if (millis() - testStateStartTime == 0)
      updateMovementState("Backward");
    if (millis() - testStateStartTime >= TEST_ACTION_DURATION)
    {
      motorsStop();
      currenthandleAutomaticTest_testState = TEST_STATE_TURN_LEFT;
      testStateStartTime = millis();
    }
    break;
  case TEST_STATE_TURN_LEFT:
    motorsLeft();
    if (millis() - testStateStartTime == 0)
      updateMovementState("Left");
    if (millis() - testStateStartTime >= TEST_ACTION_DURATION)
    {
      motorsStop();
      currenthandleAutomaticTest_testState = TEST_STATE_TURN_RIGHT;
      testStateStartTime = millis();
    }
    break;
  case TEST_STATE_TURN_RIGHT:
    motorsRight();
    if (millis() - testStateStartTime == 0)
      updateMovementState("Right");
    if (millis() - testStateStartTime >= TEST_ACTION_DURATION)
    {
      motorsStop();
      currenthandleAutomaticTest_testState = TEST_STATE_SERVO_LEFT;
      testStateStartTime = millis();
    }
    break;
  case TEST_STATE_SERVO_LEFT:
    myServo.write(0);
    if (millis() - testStateStartTime == 0)
      updateMovementState("Servo Left");
    if (millis() - testStateStartTime >= TEST_ACTION_DURATION)
    {
      currenthandleAutomaticTest_testState = TEST_STATE_SERVO_CENTER;
      testStateStartTime = millis();
    }
    break;
  case TEST_STATE_SERVO_CENTER:
    myServo.write(90);
    if (millis() - testStateStartTime == 0)
      updateMovementState("Servo Centered");
    if (millis() - testStateStartTime >= TEST_ACTION_DURATION)
    {
      currenthandleAutomaticTest_testState = TEST_STATE_SERVO_RIGHT;
      testStateStartTime = millis();
    }
    break;
  case TEST_STATE_SERVO_RIGHT:
    myServo.write(180);
    if (millis() - testStateStartTime == 0)
      updateMovementState("Servo Right");
    if (millis() - testStateStartTime >= TEST_ACTION_DURATION)
    {
      currenthandleAutomaticTest_testState = TEST_STATE_CYCLE_COMPLETE;
      testStateStartTime = millis();
    }
    break;
  case TEST_STATE_CYCLE_COMPLETE:
    motorsStop();
    myServo.write(90);
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    updateMovementState("Test Finished.");
    tone(buzzer, NOTE_C3);
    currenthandleAutomaticTest_testState = TEST_STATE_INIT;
    testStateStartTime = millis();
    delay(1000);
    noTone(buzzer);
    delay(1500);
    handleReset(0);
    break;
  }
}
// Joystick Test mode
void handleJoystickTest()
{
  if (ensureSingleModeAccess == 1)
  {
    ensureSingleModeAccess = 0;
    Serial.println(" -- JOYSTICK TESTING MODE TRIGGERED !");
    Serial1.println(" -- JOYSTICK TESTING MODE TRIGGERED !");
  }
  int vrxValue = analogRead(joystick_VRX);
  int vryValue = analogRead(joystick_VRY);
  int clickState = digitalRead(joystick_button);
  Serial.print("VRX: ");
  Serial.print(vrxValue);
  Serial.print(" | VRY: ");
  Serial.print(vryValue);
  Serial.print(" | CLICK: ");
  Serial.println(clickState);
  lcd.setCursor(0, 0);
  lcd.print("X-Axis:");
  lcd.print(vrxValue);
  if (vrxValue < 1000)
    lcd.print("   ");
  if (vrxValue < 100)
    lcd.print("  ");
  if (vrxValue < 10)
    lcd.print(" ");
  lcd.setCursor(0, 1);
  lcd.print("Y-Axis:");
  lcd.print(vryValue);
  if (vryValue < 1000)
    lcd.print("   ");
  if (vryValue < 100)
    lcd.print("  ");
  if (vryValue < 10)
    lcd.print(" ");
  lcd.setCursor(11, 0);
  lcd.print("CLICK");
  if (clickState == LOW)
  {
    lcd.setCursor(11, 1);
    lcd.print("~YES");
  }
  else
  {
    lcd.setCursor(12, 1);
    lcd.print("NO ");
  }
  unsigned long currentTime = millis();
  digitalWrite(LED_red, LOW);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_blue, LOW);
}
// LCD
void LCD_displayMessageCentered(String msg, int row)
{
  int len = msg.length();
  int startCol = max(0, (16 - len) / 2);
  lcd.setCursor(0, row);
  for (int i = 0; i < 16; ++i)
    lcd.print(" ");
  lcd.setCursor(startCol, row);
  lcd.print(msg);
}
void LCD_welcomeMessage()
{
  handleReset(0);
  LCD_displayMessageCentered("Resetting all", 0);
  LCD_displayMessageCentered("Components...", 1);
  Serial.println(" -- Resetting all components...");
  Serial1.println(" -- Resetting all components...");
  delay(1500);
  lcd.clear();
  LCD_displayMessageCentered("Smash Joystick", 0);
  LCD_displayMessageCentered("to Initialize!", 1);
  Serial.println(" -- Click Joystick to start !");
  Serial1.println(" -- Click Joystick to start !");
  bool readJostickClick = false;
  if (digitalRead(joystick_button) == HIGH)
  {
    readJostickClick = true;
  }
  while (readJostickClick)
  {
    if (digitalRead(joystick_button) == LOW)
    {
      Serial.println(" -- Hello! I'm DAMI-3000!");
      Serial1.println(" -- Hello! I'm DAMI-3000!");
      lcd.clear();
      LCD_displayMessageCentered("Hello! my name", 0);
      LCD_displayMessageCentered("is DAMI-3000 !", 1);
      LCD_welcomeMessage_buzzer_passiveBeep();
      delay(100);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[6], LOW);
      delay(50);
      delay(1000);
      Serial.println(" -- Please choose a mode!");
      Serial1.println(" -- Please choose a mode!");
      lcd.clear();
      LCD_displayMessageCentered("Which mode would", 0);
      LCD_displayMessageCentered("you choose ?", 1);
      LCD_welcomeMenuMessage_buzzer_passiveBeep();
      delay(700);
      break;
    }
  }
}
void LCD_welcomeMessage_buzzer_passiveBeep()
{
  const int PACMAN_TEMPO = 200;
  int pacmanMelody[] = {
      NOTE_C4, NOTE_E4, NOTE_G4, NOTE_C5,
      NOTE_G4, NOTE_E4, NOTE_C4,
      NOTE_REST, NOTE_C5, NOTE_C5, NOTE_REST};
  int pacmanDurations[] = {
      8, 8, 8, 8,
      8, 8, 8,
      8, 8, 8};
  int notes = sizeof(pacmanMelody) / sizeof(pacmanMelody[0]);
  for (int i = 0; i < notes - 1; i++)
  {
    int duration = PACMAN_TEMPO / (pacmanDurations[i] / 4);
    tone(buzzer, pacmanMelody[i], duration);
    if (i == 0)
    {
      //myServo.write(70);
      digitalWrite(LED_red, HIGH);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 1)
    {
      //myServo.write(110);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 2)
    {
      myServo.write(110);
      digitalWrite(LED_blue, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 3)
    {
      //myServo.write(110);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 4)
    {
      //myServo.write(70);
      digitalWrite(LED_red, HIGH);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 5)
    {
      myServo.write(70);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 6)
    {
      //myServo.write(70);
      digitalWrite(LED_blue, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 7)
    {
      //myServo.write(70);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, HIGH);
      digitalWrite(sevenSegmentPin[0], LOW);
      digitalWrite(sevenSegmentPin[1], LOW);
      digitalWrite(sevenSegmentPin[2], LOW);
      digitalWrite(sevenSegmentPin[3], LOW);
      digitalWrite(sevenSegmentPin[4], LOW);
      digitalWrite(sevenSegmentPin[5], LOW);
      digitalWrite(sevenSegmentPin[6], LOW);
      digitalWrite(sevenSegmentPin[7], LOW);
      delay(50);
    }
    else if (i == 8)
    {
      myServo.write(90);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[0], HIGH);
      digitalWrite(sevenSegmentPin[1], HIGH);
      digitalWrite(sevenSegmentPin[2], HIGH);
      digitalWrite(sevenSegmentPin[3], HIGH);
      digitalWrite(sevenSegmentPin[4], HIGH);
      digitalWrite(sevenSegmentPin[5], HIGH);
      digitalWrite(sevenSegmentPin[6], HIGH);
      digitalWrite(sevenSegmentPin[7], HIGH);
      delay(35);
      digitalWrite(sevenSegmentPin[0], LOW);
      digitalWrite(sevenSegmentPin[1], LOW);
      digitalWrite(sevenSegmentPin[2], LOW);
      digitalWrite(sevenSegmentPin[3], LOW);
      digitalWrite(sevenSegmentPin[4], LOW);
      digitalWrite(sevenSegmentPin[5], LOW);
      digitalWrite(sevenSegmentPin[6], LOW);
      digitalWrite(sevenSegmentPin[7], LOW);
      digitalWrite(LED_green, LOW);
      delay(15);
    }
    else if (i == 9)
    {
      //myServo.write(90);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[0], HIGH);
      digitalWrite(sevenSegmentPin[1], HIGH);
      digitalWrite(sevenSegmentPin[2], HIGH);
      digitalWrite(sevenSegmentPin[3], HIGH);
      digitalWrite(sevenSegmentPin[4], HIGH);
      digitalWrite(sevenSegmentPin[5], HIGH);
      digitalWrite(sevenSegmentPin[6], HIGH);
      digitalWrite(sevenSegmentPin[7], HIGH);
      delay(35);
      digitalWrite(sevenSegmentPin[0], LOW);
      digitalWrite(sevenSegmentPin[1], LOW);
      digitalWrite(sevenSegmentPin[2], LOW);
      digitalWrite(sevenSegmentPin[3], LOW);
      digitalWrite(sevenSegmentPin[4], LOW);
      digitalWrite(sevenSegmentPin[5], LOW);
      digitalWrite(sevenSegmentPin[6], LOW);
      digitalWrite(sevenSegmentPin[7], LOW);
      digitalWrite(LED_green, LOW);
      delay(15);
    }
    int pauseTime = duration;
    delay(pauseTime);
    noTone(buzzer);
  }
}
void LCD_welcomeMenuMessage_buzzer_passiveBeep()
{
  const int PACMAN_TEMPO = 200;
  int pacmanMelody[] = {
      NOTE_C5, NOTE_G4, NOTE_E4, NOTE_C4,
      NOTE_G4, NOTE_E4, NOTE_C4,
      NOTE_REST, NOTE_C2, NOTE_C2, NOTE_REST};
  int pacmanDurations[] = {
      8, 8, 8, 8,
      8, 8, 8,
      8, 8, 8};
  int notes = sizeof(pacmanMelody) / sizeof(pacmanMelody[0]);
  for (int i = 0; i < notes - 1; i++)
  {
    int duration = PACMAN_TEMPO / (pacmanDurations[i] / 4);
    tone(buzzer, pacmanMelody[i], duration);
    if (i == 0)
    {
      //myServo.write(70);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, HIGH);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 1)
    {
      //myServo.write(110);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[5], HIGH);
      delay(50);
    }
    else if (i == 2)
    {
      myServo.write(70);
      digitalWrite(LED_blue, LOW);
      digitalWrite(LED_red, HIGH);
      digitalWrite(LED_green, LOW);
      digitalWrite(sevenSegmentPin[4], HIGH);
      delay(50);
    }
    else if (i == 3)
    {
      //myServo.write(110);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 4)
    {
      //myServo.write(70);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, HIGH);
      digitalWrite(sevenSegmentPin[2], HIGH);
      delay(50);
    }
    else if (i == 5)
    {
      myServo.write(110);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[1], HIGH);
      delay(50);
    }
    else if (i == 6)
    {
      //myServo.write(70);
      digitalWrite(LED_blue, LOW);
      digitalWrite(LED_red, HIGH);
      digitalWrite(LED_green, LOW);
      digitalWrite(sevenSegmentPin[i], HIGH);
      delay(50);
    }
    else if (i == 7)
    {
      //myServo.write(70);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(sevenSegmentPin[0], LOW);
      digitalWrite(sevenSegmentPin[1], LOW);
      digitalWrite(sevenSegmentPin[2], LOW);
      digitalWrite(sevenSegmentPin[3], LOW);
      digitalWrite(sevenSegmentPin[4], LOW);
      digitalWrite(sevenSegmentPin[5], LOW);
      digitalWrite(sevenSegmentPin[6], LOW);
      digitalWrite(sevenSegmentPin[7], LOW);
      delay(50);
    }
    else if (i == 8)
    {
      myServo.write(90);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, HIGH);
      digitalWrite(sevenSegmentPin[0], HIGH);
      digitalWrite(sevenSegmentPin[1], HIGH);
      digitalWrite(sevenSegmentPin[2], HIGH);
      digitalWrite(sevenSegmentPin[3], HIGH);
      digitalWrite(sevenSegmentPin[4], HIGH);
      digitalWrite(sevenSegmentPin[5], HIGH);
      digitalWrite(sevenSegmentPin[6], HIGH);
      digitalWrite(sevenSegmentPin[7], HIGH);
      delay(35);
      digitalWrite(sevenSegmentPin[0], LOW);
      digitalWrite(sevenSegmentPin[1], LOW);
      digitalWrite(sevenSegmentPin[2], LOW);
      digitalWrite(sevenSegmentPin[3], LOW);
      digitalWrite(sevenSegmentPin[4], LOW);
      digitalWrite(sevenSegmentPin[5], LOW);
      digitalWrite(sevenSegmentPin[6], LOW);
      digitalWrite(sevenSegmentPin[7], LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(LED_red, LOW);
      delay(15);
    }
    else if (i == 9)
    {
      //myServo.write(90);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_blue, HIGH);
      digitalWrite(sevenSegmentPin[0], HIGH);
      digitalWrite(sevenSegmentPin[1], HIGH);
      digitalWrite(sevenSegmentPin[2], HIGH);
      digitalWrite(sevenSegmentPin[3], HIGH);
      digitalWrite(sevenSegmentPin[4], HIGH);
      digitalWrite(sevenSegmentPin[5], HIGH);
      digitalWrite(sevenSegmentPin[6], HIGH);
      digitalWrite(sevenSegmentPin[7], HIGH);
      delay(35);
      digitalWrite(sevenSegmentPin[0], LOW);
      digitalWrite(sevenSegmentPin[1], LOW);
      digitalWrite(sevenSegmentPin[2], LOW);
      digitalWrite(sevenSegmentPin[3], LOW);
      digitalWrite(sevenSegmentPin[4], LOW);
      digitalWrite(sevenSegmentPin[5], LOW);
      digitalWrite(sevenSegmentPin[6], LOW);
      digitalWrite(sevenSegmentPin[7], LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
      digitalWrite(LED_red, LOW);
      delay(15);
    }
    int pauseTime = duration;
    delay(pauseTime);
    noTone(buzzer);
  }
}
void LCD_menuMessage()
{
  motorsStop();
  currentMode = NONE;
  sevenSegment_clearAllSegments();
  lcd.clear();
  lcd.clear();
  Serial.println(" -- Choose a mode :");
  Serial1.println(" -- Choose a mode :");
  if (menuPage == 1)
  {
  LCD_displayMessageCentered(" BTH MZE MRS FLM", 0);
  LCD_displayMessageCentered(" DCE CRD MTT ATT", 1);
  }
  else if (menuPage == 2){
    LCD_displayMessageCentered(" JTT DNO TIM CLC", 0);
    LCD_displayMessageCentered(" SMN RDR MLD REA", 1);
    }
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_red, LOW);
  digitalWrite(LED_blue, LOW);
  }
void LCD_menuMessage_animation()
{
  if (currentMode != NONE)
    return;
  if (millis() - lastLcdAnimationUpdate >= LCD_ANIMATION_DELAY)
  {
    lastLcdAnimationUpdate = millis();
    int prevRowLeft = (lcdAnimationPhase - 1 + 2) % 2;
    int currentRowLeft = lcdAnimationPhase % 2;
    int prevRowRight = (prevRowLeft == 0) ? 1 : 0;
    int currentRowRight = (currentRowLeft == 0) ? 1 : 0;
    lcd.setCursor(0, prevRowLeft);
    lcd.print(" ");
    lcd.setCursor(4, prevRowRight);
    lcd.print(" ");
    lcd.setCursor(8, prevRowLeft);
    lcd.print(" ");
    lcd.setCursor(12, prevRowRight);
    lcd.print(" ");
    lcd.setCursor(0, currentRowLeft);
    lcd.print("~");
    lcd.setCursor(4, currentRowRight);
    lcd.print("~");
    lcd.setCursor(8, currentRowLeft);
    lcd.print("~");
    lcd.setCursor(12, currentRowRight);
    lcd.print("~");
    lcdAnimationPhase++;
  }
}
void LCD_menuMessage_buzzer_passiveBeep()
{
  const int PACMAN_TEMPO = 200;
  int pacmanMelody[] = {
      NOTE_C4, NOTE_C4, NOTE_G4, NOTE_REST};
  int pacmanDurations[] = {
      8, 8, 8, 8,
      8};
  int notes = sizeof(pacmanMelody) / sizeof(pacmanMelody[0]);
  for (int i = 0; i < notes - 1; i++)
  {
    int duration = PACMAN_TEMPO / (pacmanDurations[i] / 4);
    tone(buzzer, pacmanMelody[i], duration);
    if (i == 0)
    {
      //myServo.write(100);
      digitalWrite(sevenSegmentPin[0], HIGH);
      digitalWrite(LED_red, HIGH);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, LOW);
    }
    if (i == 1)
    {
      //myServo.write(80);
      digitalWrite(sevenSegmentPin[6], HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, HIGH);
      digitalWrite(LED_blue, LOW);
    }
    if (i == 2)
    {
      //myServo.write(90);
      digitalWrite(sevenSegmentPin[3], HIGH);
      digitalWrite(LED_red, LOW);
      digitalWrite(LED_green, LOW);
      digitalWrite(LED_blue, HIGH);
    }
    int pauseTime = duration;
    delay(pauseTime);
    noTone(buzzer);
    digitalWrite(LED_red, LOW);
    digitalWrite(LED_green, LOW);
    digitalWrite(LED_blue, LOW);
    sevenSegment_clearAllSegments();
  }
}
// 7 Segments Display
void sevenSegment_clearAllSegments()
{
  for (int i = 0; i < 7; i++)
    digitalWrite(sevenSegmentPin[i], LOW);
}
void sevenSegment_animation()
{
  if (currentMode != NONE)
    return;
  static unsigned long lastSegmentUpdate = 0;
  static unsigned long lastLedHoverUpdate = 0;
  const unsigned long LED_HOVER_INTERVAL = 5;
  static int segmentIndex = 0;
  if (millis() - lastSegmentUpdate >= 50)
  {
    lastSegmentUpdate = millis();
    sevenSegment_clearAllSegments();
    if (segmentIndex < 6)
    {
      digitalWrite(sevenSegmentPin[segmentIndex], HIGH);
    }
    segmentIndex = (segmentIndex + 1) % 6;
  }
  if (millis() - lastLedHoverUpdate >= LED_HOVER_INTERVAL)
  {
    lastLedHoverUpdate = millis();
    static int rgbLedCycle = 0;
    ledBrightness += fadeAmount;
    if (ledBrightness <= 0 || ledBrightness >= 255)
    {
      fadeAmount = -fadeAmount;
      ledBrightness = constrain(ledBrightness, 0, 255);
      if (ledBrightness == 0 && fadeAmount > 0)
      {
        rgbLedCycle = (rgbLedCycle + 1) % 3;
      }
    }
    analogWrite(LED_red, (rgbLedCycle == 0) ? ledBrightness : LOW);
    analogWrite(LED_green, (rgbLedCycle == 1) ? ledBrightness : LOW);
    analogWrite(LED_blue, (rgbLedCycle == 2) ? ledBrightness : LOW);
  }
}
void sevenSegment_displayDigit(int digit)
{
  if (digit < 0 || digit > 9)
    return;
  byte numbers[10] = {
      0b00111111, 0b00000110, 0b01011011, 0b01001111,
      0b01100110, 0b01101101, 0b01111101, 0b00000111,
      0b01111111, 0b01101111};
  byte segments = numbers[digit];
  for (int i = 0; i < 7; i++)
  {
    digitalWrite(sevenSegmentPin[i], (segments >> i) & 0x01);
  }
}
void sevenSegment_displayLetter(char letter)
{
  byte segments = 0;
  switch (letter)
  {
  case 'A':
    segments = 0b01110111;
    break;
  case 'B':
    segments = 0b01111100;
    break;
  case 'C':
    segments = 0b00111001;
    break;
  case 'D':
    segments = 0b01011110;
    break;
  case 'E':
    segments = 0b01111001;
    break;
  case 'F':
    segments = 0b01110001;
    break;
  case 'G':
    segments = 0b01111101;
    break;
  case 'H':
    segments = 0b01110110;
    break;
  case 'I':
    segments = 0b00000110;
    break;
  case 'J':
    segments = 0b00011110;
    break;
  case 'K':
    segments = 0b01110000;
    break;
  case 'L':
    segments = 0b00111000;
    break;
  case 'M':
    segments = 0b00110111;
    break;
  case 'N':
    segments = 0b01010100;
    break;
  case 'O':
    segments = 0b00111111;
    break;
  case 'P':
    segments = 0b01110011;
    break;
  case 'Q':
    segments = 0b01100111;
    break;
  case 'R':
    segments = 0b01010000;
    break;
  case 'S':
    segments = 0b01101101;
    break;
  case 'T':
    segments = 0b01111000;
    break;
  case 'U':
    segments = 0b00111110;
    break;
  case 'V':
    segments = 0b00111110;
    break;
  case 'W':
    segments = 0b00011100;
    break;
  case 'X':
    segments = 0b01110110;
    break;
  case 'Y':
    segments = 0b01101110;
    break;
  case 'Z':
    segments = 0b01011011;
    break;
  case '0':
    segments = 0b00111111;
    break;
  case '1':
    segments = 0b00000110;
    break;
  case '2':
    segments = 0b01011011;
    break;
  case '3':
    segments = 0b01001111;
    break;
  case '4':
    segments = 0b01100110;
    break;
  case '5':
    segments = 0b01101101;
    break;
  case '6':
    segments = 0b01111101;
    break;
  case '7':
    segments = 0b00000111;
    break;
  case '8':
    segments = 0b01111111;
    break;
  case '9':
    segments = 0b01101111;
    break;
  }
  for (int i = 0; i < 9; i++)
  {
    digitalWrite(sevenSegmentPin[i], (segments >> i) & 0x01);
  }
}
// Line counter
static const int handleCredits_codeLinesCounter_endLine = __LINE__;
static const int handleCredits_codeLinesCounter_calculator = handleCredits_codeLinesCounter_endLine - handleCredits_codeLinesCounter_firstLine;