//#include <Wire.h>
//#include <pico/stdlib.h>

// #include "Adafruit_LiquidCrystal.h"
// #include "pico/multicore.h"
// #include "hardware/gpio.h"
// #include "hardware/uart.h"
// #include "sketch2.ino.h"




// The liquidCrystalRasPiRealWorld variable determines which LCD library to use.
// 1 = Setting for Raspberry Pi Pico LCD, 0 = Setup for SimulIDE i2c LCD
#define liquidCrystalRasPiRealWorld 1
#define raspberryPiPicoMicroController 1


// // UART0 TX on GP0 (pin 1 on the Pico)
// #define UART_TX_PIN 0
// // UART0 RX on GP1 (pin 2 on the Pico)
// #define UART_RX_PIN 1
// #define UART_ID uart0
#define BAUD_RATE 57600
#define RESPONSE_TIMEOUT 3000  // for example, 1000 milliseconds
#define XDCR_PWR_ON HIGH
#define XDCR_PWR_OFF LOW
#define XDCR_PWR_PIN 22



#if liquidCrystalRasPiRealWorld == 1
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);
#else
#include <LiquidCrystal_AIP31068_I2C.h>
LiquidCrystal_AIP31068_I2C lcd(0x27, 20, 4);  // set the LCD address to 0x3E for a 20 chars and 4 line display
#endif

void initLcd() {
#if liquidCrystalRasPiRealWorld == 1
  lcd.init();
  lcd.backlight();
#else
  lcd.init();
  // lcd.begin(20, 4);
  // lcd.setBacklight(HIGH);
#endif
}

#if raspberryPiPicoMicroController == 1
#include "pico/multicore.h"
#include "hardware/gpio.h"
#include "hardware/uart.h"
#include <string.h>
using namespace std;
#define UART_ID uart0
// UART0 TX on GP0 (pin 1 on the Pico)
#define UART_TX_PIN 0
// UART0 RX on GP1 (pin 2 on the Pico)
#define UART_RX_PIN 1
#define UART1_ID uart1
// UART0 TX on GP0 (pin 1 on the Pico)
#define UART1_TX_PIN 8
// UART0 RX on GP1 (pin 2 on the Pico)
#define UART1_RX_PIN 9
// Pin definitions for the buttons
#define BACK_BUTTON_PIN 16    //13
#define LEFT_BUTTON_PIN 26    //10
#define DOWN_BUTTON_PIN 19    //8
#define UP_BUTTON_PIN 18      //9
#define RIGHT_BUTTON_PIN 27   //7
#define SELECT_BUTTON_PIN 17  //6
uart_inst_t* UART_OBJ = UART_ID;

#else
// Pin definitions for the buttons
#define BACK_BUTTON_PIN 13
#define LEFT_BUTTON_PIN 10
#define DOWN_BUTTON_PIN 8
#define UP_BUTTON_PIN 9
#define RIGHT_BUTTON_PIN 7
#define SELECT_BUTTON_PIN 6
HardwareSerial* UART_OBJ = &Serial1;  // Use the desired Serial port (Serial1, Serial2, etc.)
#endif


void uartInit(unsigned long baudRate) {
#if raspberryPiPicoMicroController == 1
  uart_init(UART_ID, baudRate);
  gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
  gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
  uart_init(UART1_ID, BAUD_RATE);  // Initialize UART1 with the same baud rate
  gpio_set_function(UART1_TX_PIN, GPIO_FUNC_UART);
  gpio_set_function(UART1_RX_PIN, GPIO_FUNC_UART);
#else
  Serial.begin(baudRate);
#endif
}


void wait(unsigned long milliseconds) {
#if raspberryPiPicoMicroController == 1
  // For Raspberry Pi Pico
  sleep_ms(milliseconds);
#else
  // For Arduino Mega 2560
  delay(milliseconds);
#endif
}



void uartPutString(const char* str) {
#if raspberryPiPicoMicroController == 1
  // For Raspberry Pi Pico
  uart_puts(UART_ID, str);
#else
  // For Arduino Mega 2560
  Serial.print(str);
#endif
}


void uartPutChar(char c) {
#if raspberryPiPicoMicroController == 1
  // For Raspberry Pi Pico
  uart_putc(UART_ID, c);
#else
  // For Arduino Mega 2560
  Serial.write(c);
#endif
}

void uart_flush_input() {
#if raspberryPiPicoMicroController == 1
  while (uart_getc(uart0) != -1) {}  // Clear RX FIFO
#else
  while (Serial.available()) {
    Serial.read();
  }
#endif
}


void uartGetChar(char& c) {
#if raspberryPiPicoMicroController == 1
  // For Raspberry Pi Pico
  c = uart_getc(UART_ID);
#else
  // For Arduino Mega 2560
  c = Serial.read();
#endif
}



// void uartReceive(char* uartReceivedText, size_t uartReceivedTextSize) {
// #if raspberryPiPicoMicroController == 1
//   // Raspberry Pi Pico
//   if (uart_read_response(uartReceivedText, uartReceivedTextSize, RESPONSE_TIMEOUT)) {
//     lcd.print(uartReceivedText);
//   } else {
//     lcd.print("Error: No data received");
//   }
// #else
//   // Arduino
//   Serial.readStringUntil('\n').toCharArray(uartReceivedText, uartReceivedTextSize);
//   lcd.print(uartReceivedText);
// #endif
// }



void uartReceive(char* uartReceivedText, size_t uartReceivedTextSize) {
  memset(uartReceivedText, 0, uartReceivedTextSize);  // Clear the entire buffer

#if raspberryPiPicoMicroController == 1
    // Raspberry Pi Pico
  if (uart_read_response(uartReceivedText, uartReceivedTextSize, RESPONSE_TIMEOUT)) {
    lcd.print(uartReceivedText);
  } else {
    lcd.print("Error: No data received");
  }
#else
    // Arduino
  Serial.readStringUntil('\n').toCharArray(uartReceivedText, uartReceivedTextSize);
  lcd.print(uartReceivedText);
  UART_OBJ->flush();  // Flush transmit buffer after receiving (Arduino)
#endif
}




void setSNXPosition(int caseNum) {
  char serialNum[7];
  memset(serialNum, 0, sizeof(serialNum));  // Clear serialNum buffer
  // strcpy (serialNum, "991174\r\n");
  char serialNumLastThree[3];
  lcd.clear();
  // uart_flush_input();
  uart_send_string_without_delay("sn?\r\n", 0);
  uartReceive(serialNum, sizeof(serialNum));
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(serialNum);
  lcd.setCursor(2, 0);
  lcd.print(caseNum);

  int i = 3;  // Start from the fourth character
  int j = 0;
  while (i < 6 && serialNum[i] >= '0' && serialNum[i] <= '9') {
    serialNumLastThree[j++] = serialNum[i++];
  }
  serialNumLastThree[j] = '\0';  // Null-terminate the extracted digits

  // Generate the "mynum=" string
  char mynumString[16];
  sprintf(mynumString, "mynum=99%d%s\r\n", caseNum, serialNumLastThree);
  uart_send_string_without_delay(mynumString, strlen(mynumString));
}

void setSNxXPosition(int caseNum) {
  char serialNum[7];
  memset(serialNum, 0, sizeof(serialNum));  // Clear serialNum buffer
  // strcpy (serialNum, "991174\r\n");
  lcd.clear();
  // uart_flush_input();
  uart_send_string_without_delay("sn?\r\n", 0);
  uartReceive(serialNum, sizeof(serialNum));

  // if (serialNum[0] == '\0') {
  //   // Null character received, clear buffer and try again
  //   uart_flush_input();  // Replace with your UART library's flush function
  //   uart_send_string_without_delay("sn?\r\n", 0);
  //   uartReceive(serialNum, sizeof(serialNum));
  // } else if {
  //   break;
  // }

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(serialNum);
  lcd.setCursor(3, 0);
  lcd.print(caseNum);

  // Generate the "mynum=" string
  char mynumString[16];
  sprintf(mynumString, "mynum=99%c%d%c%c\r\n", serialNum[2], caseNum, serialNum[4], serialNum[5]);
  uart_send_string_without_delay(mynumString, strlen(mynumString));
}

void setSNxxXPosition(int caseNum) {
  char serialNum[7];
  memset(serialNum, 0, sizeof(serialNum));  // Clear serialNum buffer
  // strcpy (serialNum, "991174\r\n");
  lcd.clear();
  // uart_flush_input();
  uart_send_string_without_delay("sn?\r\n", 0);
  uartReceive(serialNum, sizeof(serialNum));
  lcd.setCursor(0, 0);
  lcd.print(serialNum);
  lcd.setCursor(4, 0);
  lcd.print(caseNum);

  // Generate the "mynum=" string
  char mynumString[16];
  sprintf(mynumString, "mynum=99%c%c%d%c\r\n", serialNum[2], serialNum[3], caseNum, serialNum[5]);
  uart_send_string_without_delay(mynumString, strlen(mynumString));
}

void setSNxxxXPosition(int caseNum) {
  char serialNum[7];
  memset(serialNum, 0, sizeof(serialNum));  // Clear serialNum buffer
  // strcpy (serialNum, "991174\r\n");
  lcd.clear();
  // uart_flush_input();
  uart_send_string_without_delay("sn?\r\n", 0);
  uartReceive(serialNum, sizeof(serialNum));
  lcd.setCursor(0, 0);
  lcd.print(serialNum);
  lcd.setCursor(5, 0);
  lcd.print(caseNum);

  // Generate the "mynum=" string
  char mynumString[16];
  sprintf(mynumString, "mynum=99%c%c%c%d\r\n", serialNum[2], serialNum[3], serialNum[4], caseNum);
  uart_send_string_without_delay(mynumString, strlen(mynumString));
}



volatile bool buttonPressed = false;
volatile int cursorPos = 0;

bool backPressed = false;
bool selectPressed = false;
bool upPressed = false;
bool downPressed = false;


char response[96] = { 0 };  // Buffer for UART response

const char* optionAD14[] = { "3", "4", "5", "A", "B", "C", "D", "0", "1", "2" };
const char* optionSN09[] = { "9", "0", "1", "2", "3", "4", "5", "6", "7", "8" };

int numOptionAD14 = sizeof(optionAD14) / sizeof(optionAD14[0]);
int numOptionSN09 = sizeof(optionSN09) / sizeof(optionSN09[0]);

void scrollMenu(int direction) {
  cursorPos = (cursorPos + numOptionAD14 + direction) % numOptionAD14;
}

//int cursorPos = 0;  // Current cursor position


// Enums for the menu states
enum MainMenuState {
  MainMenu,


  XdcrSubMenu,  //

  XdcrComms,      ////
  XdcrCommsBaud,  //////

  XdcrAddress,      ////
  XdcrAddressBaud,  //////
  XdcrAddressAD14,  //////


  M9414SubMenu,  //

  M9414RegTest,                ////
  M9414RegTestCurrentGet,      //////
  M9414RegTestPressSettings,   //////
  M9414RegTestRevertSettings,  //////

  M9414PressureRateSetWindow,  ////

  M9414RxDx,  ////


  MDualSubMenu,  //

  MDualSN,      ////
  MDualSNX,     //////
  MDualSNxX,    //////
  MDualSNxxX,   //////
  MDualSNxxxX,  //////

  MDualPriSec,  ////

};

// Global variables to store the current menu state and cursor position

MainMenuState currentMainMenuState = MainMenu;


// lcd.setCursor(0, 0);
//   lcd.print("                    ");
//   lcd.setCursor(0, 1);
//   lcd.print(cursorPos == 0 ? ">1. A               " : " 1. A               ");
//   lcd.setCursor(0, 2);
//   lcd.print(cursorPos == 1 ? ">                   " : "                    ");
//   lcd.setCursor(0, 3);
//   lcd.print(cursorPos == 2 ? ">                   " : "                    ");



//int cursorPos = 0;



// Function to display the main menu
void displayMainMenu() {
  //lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Main Menu");

  // Display menu options with cursor indicator
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. XDCR" : " 1. XDCR");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">2. 9414" : " 2. 9414");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">3. 9414/9424" : " 3. 9414/9424");
}

/////////////////////////////////////////////////////////////////////////////

// Function to display the XDCR submenu
void displayXdcrSubMenu() {
  //lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("XDCR Submenu");
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. Set Baud Rate" : " 1. Set Baud Rate");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">2. Test Comms" : " 2. Test Comms");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">3. Set Address" : " 3. Set Address");
}

////////////////////////////////////////////////////

void displayXdcrComms() {
  //void displayXdcrSubMenu2() {
  lcd.setCursor(0, 0);
  lcd.print("XDCR comms");
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. Go >>>" : " 1. Go >>>");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">                   " : " ");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">                   " : " ");
}

//////////////////////////////////

void displayXdcrCommsBaud() {
  lcd.setCursor(0, 0);
  lcd.print("XDCR Comms Baud");
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. 9600" : " 1. 9600");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">2. 19200" : " 2. 19200");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">3. 57600" : " 3. 57600");
}

//////////////////////////////////////////////////

// //no longer used

// void displayXdcrAddress() {
//   lcd.setCursor(0, 0);
//   lcd.print("XDCR Set Address   ");
//   lcd.setCursor(0, 1);
//   lcd.print(cursorPos == 0 ? ">1. Set Baud Rate   " : " 1. Set Baud Rate   ");
//   lcd.setCursor(0, 2);
//   lcd.print(cursorPos == 1 ? ">2. Set XDCR address" : " 2. Set XDCR address");
//   lcd.setCursor(0, 3);
//   lcd.print(cursorPos == 2 ? ">                   " : "                    ");
// }

// //no longer used
// ///////////////////////////////////

// //no longer used

// void displayXdcrAddressBaud() {
//   lcd.setCursor(0, 0);
//   lcd.print("XDCR Comms Baud    ");
//   lcd.setCursor(0, 1);
//   lcd.print(cursorPos == 0 ? ">1. 9600            " : " 1. 9600            ");
//   lcd.setCursor(0, 2);
//   lcd.print(cursorPos == 1 ? ">2. 19200           " : " 2. 19200           ");
//   lcd.setCursor(0, 3);
//   lcd.print(cursorPos == 2 ? ">3. 57600           " : " 3. 57600           ");
// }

// //no longer used

/////////////////////////////////

// void displayXdcrAddressAD14() {
//   lcd.setCursor(0, 0);
//   lcd.print("XDCR Comms Baud    ");
//   lcd.setCursor(0, 1);
//   lcd.print(cursorPos == 0 ? "> A                 " : "  A                 ");
//   lcd.setCursor(0, 2);
//   lcd.print(cursorPos == 1 ? "> B                 " : "  B                 ");
//   lcd.setCursor(0, 3);
//   lcd.print(cursorPos == 2 ? "> C                 " : "  C                 ");
//   lcd.setCursor(0, 4);
//   lcd.print(cursorPos == 3 ? "> D                 " : "  D                 ");
//   lcd.setCursor(0, 5);
//   lcd.print(cursorPos == 4 ? "> 1                 " : "  1                 ");
//   lcd.setCursor(0, 6);
//   lcd.print(cursorPos == 5 ? "> 2                 " : "  2                 ");
//   lcd.setCursor(0, 7);
//   lcd.print(cursorPos == 6 ? "> 3                 " : "  3                 ");
//   lcd.setCursor(0, 8);
//   lcd.print(cursorPos == 7 ? "> 4                 " : "  4                 ");
// }





// void displayXdcrAddressAD14() {
//   //lcd.clear();
//   lcd.setCursor(0, 0);
//   lcd.print("XDCR Comms Baud    ");

//   for (int i = 0; i < 3; i++) {
//     int optionIndex = (cursorPos + i) % numOptions;
//     lcd.setCursor(0, i + 1);

//     if (i == 1) {
//       lcd.print("> ");
//     } else {
//       lcd.print("  ");
//     }

//     lcd.print(optionAD14[optionIndex]);
//   }
// }



void displayXdcrAddressAD14() {
  lcd.setCursor(0, 0);
  lcd.print("XDCR Comms Address  ");

  for (int i = 0; i < 3; i++) {
    int optionIndex = (cursorPos + i) % 10;  // Adjust the modulus to the number of options you want to display
    lcd.setCursor(0, i + 1);

    if (i == 1) {
      lcd.print("> ");
    } else {
      lcd.print("  ");
    }

    lcd.print(optionAD14[optionIndex]);
  }
}







/////////////////////////////////////////////////////////////////////////////

// Function to display the 9414/9424 submenu
void displayM9414SubMenu() {
  //lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("9414 Submenu");
  // Display menu options with cursor indicator
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. Regulator Test" : " 1. Regulator Test");

  lcd.setCursor(0, 2);
  if (cursorPos == 1) {
    scrollTextWithPrefix(">", "      TBD>>2. Reset Pressure Rate and Set Window             ", 20);
  } else {
    printLimitedChars(" TBD>>2. Reset Pressure Rate and Set Window", 20);
  }

  lcd.setCursor(0, 3);
  if (cursorPos == 2) {
    scrollTextWithPrefix(">", "      TBD>>3. Capture Receive/Delivered As Settings            ", 20);
  } else {
    printLimitedChars(" TBD>>3. Capture Receive/Delivered As settings", 20);
  }
}

/////////////////////////////////////////////////////

void displayM9414RegTest() {
  lcd.setCursor(0, 0);
  lcd.print("9414 Regulator test");
  // Display menu options with cursor indicator
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. Current Settings" : " 1. Current Settings");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">2. Leak Testing" : " 2. Leak Testing");
  lcd.setCursor(0, 3);
  if (cursorPos == 2) {
    scrollTextWithPrefix(">", "      3. Revert Back to Default Settings            ", 20);
  } else {
    printLimitedChars(" 3. Revert Back to Default Settings", 20);
  }
}

//also...duplicate.  see M9414PressureRateSetWindow
void displayM9414RegTestCurrentGet() {
  lcd.setCursor(0, 0);
  lcd.print("Current Settings");
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. Go >>>" : " 1. Go >>>");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">                   " : " ");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">                   " : " ");
}

void displayM9414RegTestPressSettings() {
  lcd.setCursor(0, 0);
  lcd.print("Set Leak-Test Rates");
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1.PR 1.0 SW 0.1 >>>" : " 1.PR 1.0 SW 0.1 >>>");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">" : " ");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? "> " : " ");
}

void displayM9414RegTestRevertSettings() {
  lcd.setCursor(0, 0);
  lcd.print("Set Default Settings");
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. PR 25 SW 0.004" : " 1. PR 25 SW 0.004");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">2. PR 25 SW 0.03" : " 2. PR 25 SW 0.03");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">3. PR 50 SW 0.03" : " 3. PR 50 SW 0.03");
}


//TBD   also...duplicate. see M9414RegTestCurrentGet
void displayM9414PressureRateSetWindow() {
  lcd.setCursor(0, 0);
  lcd.print("Current Settings");
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. Go >>>          " : " 1. Go >>>          ");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">                   " : "                    ");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">                   " : "                    ");
}

//TBD
void displayM9414RxDx() {
  lcd.setCursor(0, 0);
  lcd.print("Record 9414 Settings");
  // Display menu options with cursor indicator
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. Go >>>" : " 1. Go >>>");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">                   " : " ");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">                   " : " ");
}


////////////////////////////////////////////////////////////////////////////

// Function to display the 9414/9424 submenu
void displayMDualSubMenu() {
  //lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("9414/9424 Submenu");
  // Display menu options with cursor indicator
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. S/N Set" : " 1. S/N Set");

  lcd.setCursor(0, 2);
  if (cursorPos == 1) {
    scrollTextWithPrefix(">", "      2. Slave Opt Set 9414 2.04.xx S/W            ", 20);
  } else {
    printLimitedChars(" 2. Slave Opt Set 9414 2.04.xx S/W", 20);
  }

  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">" : " ");
}

////////////////////////////////////////////////////

void displayMDualSN() {
  lcd.setCursor(0, 0);
  lcd.print(cursorPos == 0 ? ">Set SN 99 X _ _ _ " : " Set SN 99 X _ _ _ ");
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 1 ? ">Set SN 99 _ X _ _ " : " Set SN 99 _ X _ _ ");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 2 ? ">Set SN 99 _ _ X _ " : " Set SN 99 _ _ X _ ");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 3 ? ">Set SN 99 _ _ _ X " : " Set SN 99 _ _ _ X ");
}

void displayMDualSNX() {
  for (int i = 0; i < 3; i++) {
    int optionIndex = (cursorPos + i) % 10;  // Adjust the modulus to the number of options you want to display
    lcd.setCursor(0, i + 1);

    if (i == 1) {
      lcd.print("> ");
    } else {
      lcd.print("  ");
    }

    lcd.print(optionSN09[optionIndex]);
  }
}

void displayMDualSNxX() {
  for (int i = 0; i < 3; i++) {
    int optionIndex = (cursorPos + i) % 10;  // Adjust the modulus to the number of options you want to display
    lcd.setCursor(0, i + 1);

    if (i == 1) {
      lcd.print(">  ");
    } else {
      lcd.print("   ");
    }

    lcd.print(optionSN09[optionIndex]);
  }
}

void displayMDualSNxxX() {
  for (int i = 0; i < 3; i++) {
    int optionIndex = (cursorPos + i) % 10;  // Adjust the modulus to the number of options you want to display
    lcd.setCursor(0, i + 1);

    if (i == 1) {
      lcd.print(">   ");
    } else {
      lcd.print("    ");
    }

    lcd.print(optionSN09[optionIndex]);
  }
}

void displayMDualSNxxxX() {
  for (int i = 0; i < 3; i++) {
    int optionIndex = (cursorPos + i) % 10;  // Adjust the modulus to the number of options you want to display
    lcd.setCursor(0, i + 1);

    if (i == 1) {
      lcd.print(">    ");
    } else {
      lcd.print("     ");
    }

    lcd.print(optionSN09[optionIndex]);
  }
}



//////////////////////////////////////

///////////////////////////////////////////////////
void displayMDualPriSec() {
  lcd.setCursor(0, 0);
  scrollTextWithPrefix("", "      Pri/Sec Set (Only Applies to 2.04.XX S/W and Later            ", 20);
  // Display menu options with cursor indicator
  lcd.setCursor(0, 1);
  lcd.print(cursorPos == 0 ? ">1. Primary/Master" : " 1. Primary/Master");
  lcd.setCursor(0, 2);
  lcd.print(cursorPos == 1 ? ">2. Secondary/Slave" : " 2. Secondary/Slave");
  lcd.setCursor(0, 3);
  lcd.print(cursorPos == 2 ? ">" : " ");
}

//////////////////////////////////////////////////////////////////////////////


void scrollTextWithPrefix(const char* prefix, const char* text, int maxChars) {
  int prefixLength = strlen(prefix);
  int textLength = strlen(text);
  int totalLength = prefixLength + textLength;

  // Check if scrolling is necessary
  if (totalLength > maxChars) {
    static int shift = 0;

    // Display the fixed prefix
    for (int i = 0; i < prefixLength; i++) {
      lcd.write(prefix[i]);
    }

    // Shift and display the scrolling text
    for (int i = 0; i < maxChars - prefixLength; i++) {
      lcd.write(text[(i + shift) % textLength]);
    }

    // Increment the shift value for the next iteration
    shift = (shift + 1) % textLength;

    // Introduce a delay to control the scrolling speed
    delay(150);  // Adjust the delay to control the speed
  } else {
    // Total length fits within the 20-character limit, just display it
    lcd.print(prefix);
    lcd.print(text);
  }
}



void printLimitedChars(const char* text, int maxChars) {
  char buffer[maxChars + 1];  // Static buffer for truncation
  strncpy(buffer, text, maxChars);
  buffer[maxChars] = '\0';
  lcd.print(buffer);  // Directly print to LCD within the function
}



// Function to send a string over UART with a delay between characters
void uart_send_string_with_delay(const char* str, unsigned int delay_ms) {
  for (; *str != '\0'; ++str) {
    uartPutChar(*str);
    wait(delay_ms);
  }
}



bool uart_read_response(char* buffer, size_t buffer_size, unsigned long response_timeout) {
  buffer[0] = '\0';
  size_t index = 0;
  unsigned long startTime = millis();

  while ((millis() - startTime >= response_timeout) && (index < buffer_size - 1)) {
    char c;
    uartGetChar(c);
    buffer[index++] = c;
    if (c == '\n' || c == '\r') {
      buffer[index - 1] = '\0';  // Null-terminate the string
      return true;
    }
  }

  buffer[index] = '\0';  // Null-terminate the string
  return index > 0;
}






//////////////////////////////////working with raspberry pi pico without any issues
// bool uart_read_response(uart_inst_t* uart, char* buffer, size_t buffer_size, unsigned long response_timeout) {
//   size_t index = 0;
//   unsigned long startTime = millis();

//   while ((millis() - startTime < response_timeout) && (index < buffer_size - 1)) {
// #if raspberryPiPicoMicroController == 1
//     if (uart_is_readable(uart)) {
//       char ch = uart_getc(uart);
//       buffer[index++] = ch;
//       if (ch == '\n' || ch == '\r') {  //} || index >= buffer_size - 1) {
//         buffer[index - 1] = '\0';      // Null-terminate the string
//         return true;                   // End of line or buffer full
//       }
//       //buffer[index++] = ch;
//     }
// #else
//     if (Serial.available() > 0) {
//       char c = Serial.read();
//       buffer[index++] = c;
//       if (c == '\n' || c == '\r') {
//         buffer[index - 1] = '\0'; // Null-terminate the string
//         return true; // End of line or buffer full
//       }
//     }
// #endif
//   }

//   buffer[index] = '\0';  // Null-terminate the string
//   return index > 0;      // Return true if any data was received
// }





void uart_send_string_without_delay(const char* str, unsigned int delay_ms) {
  uartPutString(str);
  // The delay here will occur after the entire string is sent,
  // not between individual characters.
  wait(delay_ms);
}






// Function to wait for a response with a timeout and debug counter
bool uart_wait_for_response(unsigned long timeout) {
  unsigned long startTime = millis();
  int counter = 0;

  while ((millis() - startTime) < timeout && counter < 100) {
#if raspberryPiPicoMicroController == 1
    if (uart_is_readable(UART_ID)) {
      // Read and ignore the incoming data to clear the buffer
      while (uart_is_readable(UART_ID)) {
        uart_getc(UART_ID);
      }
      return true;  // Data received
    }
#else
    if (Serial.available() > 0) {
      // Read and ignore the incoming data to clear the buffer
      while (Serial.available() > 0) {
        char c = Serial.read();
      }
      return true;  // Data received
    }
#endif

    wait(10);  // Prevent CPU hogging
    counter++;
  }



  return false;  // No data received
}

// bool uart_read_response(uart_inst_t* uart, char* buffer, size_t buffer_size) {
//   size_t index = 0;
//   unsigned long startTime = millis();

//   while (millis() - startTime < RESPONSE_TIMEOUT) {
//     if (uart_is_readable(UART_ID)) {
//       char ch = uart_getc(UART_ID);
//       if (ch == '\n' || ch == '\r' || index >= buffer_size - 1) {
//         buffer[index] = '\0'; // Null-terminate the string
//         return true; // End of line or buffer full
//       }
//       buffer[index++] = ch;
//     }
//   }

//   buffer[index] = '\0'; // Null-terminate the string
//   return index > 0; // Return true if any data was received
// }







// bool uart_read_response(uart_inst_t* uart, char* buffer, size_t buffer_size, unsigned long response_timeout) {
//     size_t index = 0;
//     unsigned long startTime = millis();

//     while (millis() - startTime < response_timeout && index < buffer_size - 2) {
//         if (uart_is_readable(uart)) {
//             char ch = uart_getc(uart);
//             if (ch == '\n' || ch == '\r') {
//                 break;
//             }
//             buffer[index++] = ch;
//         }
//     }
//     // Add a newline character before the null terminator, if there's space
//     if (index < buffer_size - 2) {
//         buffer[index++] = '\n';
//     }
//     // Ensure the buffer is null-terminated
//     buffer[index] = '\0';
//     return index > 0; // Return true if any data was received
// }






// void scrollMenu(int direction) {
//   // Direction should be 1 for down and -1 for up
//   displayStartPos += direction;

//   if (displayStartPos < 0) {
//     displayStartPos = 0;
//   }

//   if (displayStartPos + 3 > numOptions) {
//     displayStartPos = numOptions - 3;
//   }
// }





// Function to handle button presses
void handleButtonPresses() {
  if (!buttonPressed) {
    // Read the state of the buttons
#if raspberryPiPicoMicroController == 1
    int backButtonState = gpio_get(BACK_BUTTON_PIN);
    int selectButtonState = gpio_get(SELECT_BUTTON_PIN);
    int upButtonState = gpio_get(UP_BUTTON_PIN);
    int downButtonState = gpio_get(DOWN_BUTTON_PIN);
#else
    int backButtonState = digitalRead(BACK_BUTTON_PIN);
    int selectButtonState = digitalRead(SELECT_BUTTON_PIN);
    int upButtonState = digitalRead(UP_BUTTON_PIN);
    int downButtonState = digitalRead(DOWN_BUTTON_PIN);
#endif

    // Handle the select button press
    if (digitalRead(SELECT_BUTTON_PIN) == LOW) {
      if (!selectPressed) {
        switch (currentMainMenuState) {
          case MainMenu:
            // Depending on the cursor position, select the appropriate submenu
            switch (cursorPos) {
              case 0:
                lcd.clear();
                currentMainMenuState = XdcrSubMenu;
                break;
              case 1:
                lcd.clear();
                currentMainMenuState = M9414SubMenu;
                break;
              case 2:
                lcd.clear();
                currentMainMenuState = MDualSubMenu;
                break;
            }
            break;

          case XdcrSubMenu:
            // Handle the XdcrSubMenu options
            switch (cursorPos) {
              case 0:
                lcd.clear();
                currentMainMenuState = XdcrCommsBaud;
                break;
              case 1:
                lcd.clear();
                currentMainMenuState = XdcrComms;
                digitalWrite(XDCR_PWR_PIN, XDCR_PWR_ON);
                delay(1000);
                //?????
                // lcd.setCursor(0, 3);
                // lcd.print("                    ");
                break;
              case 2:
                lcd.clear();
                currentMainMenuState = XdcrAddressAD14;
                digitalWrite(XDCR_PWR_PIN, XDCR_PWR_ON);
                delay(1000);
                break;
            }
            break;

          case XdcrCommsBaud:
            switch (cursorPos) {
              case 0:
                lcd.clear();
                uartInit(9600);
                uartPutString("9600\r\n");
                break;
              case 1:
                lcd.clear();
                uartInit(19200);
                uartPutString("19200\r\n");
                break;
              case 2:
                lcd.clear();
                uartInit(57600);
                uartPutString("57600\r\n");
                break;
            }
            break;

          // case XdcrComms:
          //   switch (cursorPos) {
          //     case 0:
          //       uart_send_string_without_delay("#*ID?\r\n", 10);
          //       bool responseReceived = uart_wait_for_response(RESPONSE_TIMEOUT);

          //       lcd.setCursor(19, 1);
          //       if (responseReceived) {
          //         lcd.print("#");
          //         uartPutString("#\n"); // Confirm reception by sending '#' and a newline
          //       } else {
          //         lcd.print("X");
          //         uartPutString("X\n"); // Indicate no reception by sending 'X' and a newline
          //       }
          //       break;
          //   }
          //   break;
          case XdcrComms:
            switch (cursorPos) {
              case 0:
                {


                  //memset(response, '\0', sizeof(response));
                  // uart_flush_input();
                  // uart_send_string_without_delay("#*ID?\r\n", 0);
                  uart_send_string_without_delay("5 ID   Mensor,     6110,   590455, V1.10\r\n", 0);
                  response[0] = '\0';
                  bool responseReceived = uart_read_response(response, sizeof(response), RESPONSE_TIMEOUT);  //UART_ID,

                  lcd.setCursor(19, 1);
                  lcd.print("#");
                  if (responseReceived) {

                    // Check if response is too large for one line and needs scrolling
                    if (strlen(response) > 0) {
                      char line1[19];  // Buffer for the first line (20 characters + null terminator)
                      char line2[19];  // Buffer for the second line

                      // Copy up to the first 19 characters to line1
                      strncpy(line1, response, 19);
                      // If the response is longer than 20 characters, copy the next 20 characters to line2
                      if (strlen(response) > 19) {
                        strncpy(line2, response + 19, 19);  // Start copying from the 21st character
                      } else {
                        line2[0] = '\0';  // If no more characters, set line2 to an empty string
                      }
                      // Display on the LCD
                      lcd.setCursor(1, 2);  // Set cursor to line 3
                      lcd.print(line1);
                      lcd.setCursor(1, 3);  // Set cursor to line 4
                      lcd.print(line2);
                      line1[0] = '\0';
                      line2[0] = '\0';
                      response[0] = '\0';
                      responseReceived = 0;
                    }
                  } else {
                    lcd.print("X");
                    // uartPutString("X\n");  // Indicate no reception by sending 'X' and a newline
                  }
                  break;
                }
              case 1:
              case 2:
                memset(response, '\0', sizeof(response));
                lcd.clear();
                break;
            }
            break;



          case XdcrAddressAD14:
            switch (cursorPos) {
              case 2:
                lcd.clear();
                uart_send_string_without_delay("#*A A\r\n", 10);
                delay(2000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 3:
                lcd.clear();
                uart_send_string_without_delay("#*A B\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 4:
                lcd.clear();
                uart_send_string_without_delay("#*A C\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 5:
                lcd.clear();
                uart_send_string_without_delay("#*A D\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 6:
                lcd.clear();
                uart_send_string_without_delay("#*A 0\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 7:
                lcd.clear();
                uart_send_string_without_delay("#*A 1\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 8:
                lcd.clear();
                uart_send_string_without_delay("#*A 2\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 9:
                lcd.clear();
                uart_send_string_without_delay("#*A 3\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 0:
                lcd.clear();
                uart_send_string_without_delay("#*A 4\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
              case 1:
                lcd.clear();
                uart_send_string_without_delay("#*A 5\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("#*SAVE\r\n", 10);
                delay(3000);
                break;
            }
            break;





          case M9414SubMenu:
            // Add logic to navigate within M9414SubMenu and its submenus
            switch (cursorPos) {
              case 0:
                lcd.clear();
                currentMainMenuState = M9414RegTest;
                uartInit(57600);
                // uartPutString("57600\r\n");
                break;
              case 1:
                lcd.clear();
                currentMainMenuState = M9414PressureRateSetWindow;
                uartInit(57600);
                // uartPutString("57600\r\n");
                break;
              case 2:
                lcd.clear();
                currentMainMenuState = M9414RxDx;
                uartInit(57600);
                // uartPutString("57600\r\n");
                break;
            }
            break;
          case M9414RegTest:

            // Add logic to navigate within M9414SubMenu and its submenus

            switch (cursorPos) {
              case 0:
                lcd.clear();
                currentMainMenuState = M9414RegTestCurrentGet;
                break;
              case 1:
                lcd.clear();
                currentMainMenuState = M9414RegTestPressSettings;
                break;
              case 2:
                lcd.clear();
                currentMainMenuState = M9414RegTestRevertSettings;
                break;
            }
            break;





            // case M9414RegTestCurrentGet:
            //   switch (cursorPos) {
            //     case 0:

            //       response[0] = '\0';
            //       // uart_send_string_without_delay("gains?\r\n", 10);
            //       uart_send_string_without_delay("m=1.23456 m=1.23456 m=1.23456 m=1.23456 m=1.23456 m=1.23456 mult=1.23456 \r\n", 0); ////mult text pickout and then display characters after the mult

            //       bool responseReceived = uart_read_response(UART_ID, response, sizeof(response), RESPONSE_TIMEOUT);

            //       lcd.setCursor(19, 1);
            //       //uartPutString("\n");
            //       uartPutString(response);
            //       if (responseReceived) {
            //         {
            //           lcd.print("#");
            //           // uartPutString("#\n"); // Confirm reception by sending '#' and a newline

            //           // Check if response is too large for one line and needs scrolling
            //           if (strlen(response) > 0) {
            //             char line1[19]; // Buffer for the first line (20 characters + null terminator)
            //             //char line2[19]; // Buffer for the second line


            //             // Copy up to the first 19 characters to line1
            //             strncpy(line1, response + 60, 19); ////?36? = or whatever the mult following characthers happen to be if able to read the uart string and select text then display
            //             line1[19] = '\0'; // Ensure null termination

            //             // Display on the LCD
            //             lcd.setCursor(1, 2); // Set cursor to line 3
            //             lcd.print("Mult= ");
            //             lcd.print(line1);
            //           }
            //         }

            //         {
            //           response[0] = '\0';
            //           // uart_send_string_without_delay("setwindow?\r\n", 10);
            //           uart_send_string_without_delay("setwindow = 0.004 \r\n", 0); ////mult text pickout and then display characters after the mult

            //           bool responseReceived = uart_read_response(UART_ID, response, sizeof(response), RESPONSE_TIMEOUT);
            //           if (strlen(response) > 0) {
            //             //char line1[19]; // Buffer for the first line (20 characters + null terminator)
            //             char line2[19]; // Buffer for the second line


            //             // Copy up to the first 19 characters to line1
            //             strncpy(line2, response, 19); ////?36? = or whatever the mult following characthers happen to be if able to read the uart string and select text then display
            //             line2[19] = '\0'; // Ensure null termination
            //             lcd.setCursor(1, 3); // Set cursor to line 4
            //             //lcd.print("SetWin= ");
            //             lcd.print(line2);
            //           }
            //         }

            //       } else {
            //         lcd.print("X");
            //         uartPutString("X\n"); // Indicate no reception by sending 'X' and a newline
            //       }


            //       //response[0] = '\0';
            //       break;

            //     case 1:
            //       lcd.clear();
            //       break;
            //     case 2:
            //       lcd.clear();
            //       break;
            //   }
            //   break;




          case M9414RegTestCurrentGet:
            switch (cursorPos) {
              case 0:
                {
                  bool responseReceived;
                  char commandString[15];
                  char responseBufferMult[100];
                  char responseBufferMultString[10];
                  char responseBufferSW[10];
                  // string responseBufferSWString[10];
                  // First UART operation
                  memset(responseBufferSW, '\0', sizeof(responseBufferSW));
                  memset(responseBufferMult, '\0', sizeof(responseBufferMult));
                  sprintf(commandString, "gains?\r\n");
                  uart_send_string_without_delay(commandString, 0);
                  // uart_send_string_without_delay("m=1.23456 m=1.23456 m=1.23456 m=1.23456 m=1.23456 m=1.23456 mult:=1.23456 \r\n", 0);


                  responseBufferMult[0] = '\0';
                  if (uart_read_response(responseBufferMult, sizeof(responseBufferMult), RESPONSE_TIMEOUT)) {

                    char* multPos = strstr(responseBufferMult, "mult:");

                    if (multPos != NULL) {
                      int startIndex = (multPos - responseBufferMult) + 6;
                      strncpy(responseBufferMultString, responseBufferMult + startIndex, 8);
                      responseBufferMultString[8] = '\0';  // Null-terminate the string
                      lcd.setCursor(1, 2);
                      lcd.print("Mult= ");
                      lcd.print(responseBufferMultString);
                    } else {
                      lcd.setCursor(1, 2);  // Set cursor position
                      lcd.print("Error: No data received");
                    }
                  }


                  // String responseBufferMult = Serial.readStringUntil('\n');
                  // if (responseBufferMult.indexOf("mult:") != -1) {
                  //   int startIndex = responseBufferMult.indexOf("mult:") + 5;
                  //   String extractedDataMult = responseBufferMult.substring(startIndex, startIndex + 8);
                  //   lcd.print(extractedDataMult);
                  // } else {
                  //   lcd.print("--.-----");
                  // }

                  memset(commandString, '\0', sizeof(commandString));
                  sprintf(commandString, "setwindow?\r\n");
                  uart_send_string_without_delay(commandString, 0);
                  //  uart_send_string_without_delay("0.004 \r\n", 0);


                  // //raspberry pi pico code
                  // if (uart_read_response(responseBufferSW, sizeof(responseBufferSW), RESPONSE_TIMEOUT)) {
                  //   lcd.setCursor(1, 3);  // Set cursor position on LCD
                  //   lcd.print("SetWin= ");
                  //   lcd.print(responseBufferSW);
                  // } else {
                  //   lcd.setCursor(1, 3); // Set cursor position
                  //   lcd.print("Error: No data received");
                  // }
                  // //arduino code
                  // String responseBufferSW = Serial.readStringUntil('\n');
                  // lcd.setCursor(1, 3);  // Set cursor position on LCD
                  // lcd.print("SetWin= ");
                  // lcd.print(responseBufferSW);

                  lcd.setCursor(1, 3);  // Set cursor position on LCD
                  lcd.print("SetWin= ");
                  uartReceive(responseBufferSW, 10);
                  lcd.print(responseBufferSW);



                  responseReceived = responseBufferSW != 0;



                  if (responseReceived) {
                    lcd.setCursor(19, 1);
                    lcd.print("#");
                  } else {
                    lcd.setCursor(19, 1);
                    lcd.print("X");
                  }
                  responseBufferMult[0] = '\0';
                  responseBufferSW[0] = '\0';
                }

                break;

              case 1:
              case 2:

                lcd.clear();
                break;
            }
            break;







          case M9414RegTestPressSettings:
            switch (cursorPos) {
              case 0:
                lcd.clear();
                uart_send_string_without_delay("mult= 1\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("setwindow= 0.1\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("unitno= 1\r\n", 10);
                break;
            }
            break;


          case M9414RegTestRevertSettings:
            switch (cursorPos) {
              case 0:
                lcd.clear();
                uart_send_string_without_delay("mult= 25\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("setwindow= 0.004\r\n", 10);
                delay(3000);
                break;
              case 1:
                lcd.clear();
                uart_send_string_without_delay("mult= 25\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("setwindow= 0.03\r\n", 10);
                delay(3000);
                break;
              case 2:
                lcd.clear();
                uart_send_string_without_delay("mult= 50\r\n", 10);
                delay(1000);
                uart_send_string_without_delay("setwindow= 0.03\r\n", 10);
                delay(3000);
                break;
            }
            break;



          case M9414RxDx:

            // Add logic to navigate within M9414SubMenu and its submenus
            bool responseReceived0;
            bool responseReceived1;

            switch (cursorPos) {
              case 0:
                uart_send_string_without_delay("SN?\r\n", 0);
                responseReceived0 = uart_read_response(response, sizeof(response), RESPONSE_TIMEOUT);

                lcd.setCursor(19, 1);
                if (responseReceived0) {
                  lcd.print("#");
                  // uartPutString("#\n");  // Confirm reception by sending '#' and a newline

                  // Check if response is too large for one line and needs scrolling
                  if (strlen(response) > 0) {
                    char line1[19];  // Buffer for the first line (20 characters + null terminator)
                    char line2[19];  // Buffer for the second line

                    // Copy up to the first 19 characters to line1
                    strncpy(line1, response, 19);
                    line1[19] = '\0';  // Ensure null termination

                    // If the response is longer than 20 characters, copy the next 20 characters to line2
                    if (strlen(response) > 19) {
                      strncpy(line2, response + 19, 19);  // Start copying from the 21st character
                      line2[19] = '\0';
                    } else {
                      line2[0] = '\0';  // If no more characters, set line2 to an empty string
                    }
                    // Display on the LCD
                    lcd.setCursor(1, 2);  // Set cursor to line 3
                    lcd.print(line1);
                    lcd.setCursor(1, 3);  // Set cursor to line 4
                    lcd.print(line2);
                    // uartPutString("Received response: #");
                    // uartPutString(response);
                    // uartPutString("#\r\n");
                  }
                }
                wait(1000);
                uart_send_string_without_delay("GAINS?\r\n", 0);
                responseReceived1 = uart_read_response(response, sizeof(response), RESPONSE_TIMEOUT);

                lcd.setCursor(19, 1);
                if (responseReceived1) {
                  lcd.print("#");
                  // uartPutString("#\n");  // Confirm reception by sending '#' and a newline

                  // Check if response is too large for one line and needs scrolling
                  if (strlen(response) > 0) {
                    char line1[19];  // Buffer for the first line (20 characters + null terminator)
                    char line2[19];  // Buffer for the second line

                    // Copy up to the first 19 characters to line1
                    strncpy(line1, response + 19, 19);
                    line1[19] = '\0';  // Ensure null termination

                    // If the response is longer than 20 characters, copy the next 20 characters to line2
                    if (strlen(response) > 38) {
                      strncpy(line2, response + 38, 19);  // Start copying from the 21st character
                      line2[19] = '\0';
                    } else {
                      line2[0] = '\0';  // If no more characters, set line2 to an empty string
                    }
                    // line2[19] = '\0';

                    // Display on the LCD
                    lcd.setCursor(1, 2);  // Set cursor to line 3
                    lcd.print(line1);
                    lcd.setCursor(1, 3);  // Set cursor to line 4
                    lcd.print(line2);
                    // uartPutString("Received response: #");
                    // uartPutString(response);
                    // uartPutString("#\r\n");
                  }
                } else {
                  lcd.print("X");
                  // uartPutString("X\n");  // Indicate no reception by sending 'X' and a newline
                }
                break;

              case 1:
              case 2:
                lcd.clear();
                break;
            }
            break;

          case MDualSubMenu:
            // Add logic to navigate within MDualSubMenu and its submenus
            switch (cursorPos) {

              case 0:
                lcd.clear();
                currentMainMenuState = MDualSN;
                break;
              case 1:
                lcd.clear();
                currentMainMenuState = MDualPriSec;
                uartInit(57600);
                uartPutString("57600\n");
                break;
            }
            break;



          case MDualSN:
            char dispSerialNum[7];
            switch (cursorPos) {
              case 0:
                lcd.clear();
                currentMainMenuState = MDualSNX;
                uart_send_string_without_delay("sn?\r\n", 0);
                uartReceive(dispSerialNum, 7);
                lcd.setCursor(0, 0);
                lcd.print(dispSerialNum);
                break;
              case 1:
                lcd.clear();
                currentMainMenuState = MDualSNxX;
                uart_send_string_without_delay("sn?\r\n", 0);
                uartReceive(dispSerialNum, 7);
                lcd.setCursor(0, 0);
                lcd.print(dispSerialNum);
                break;
              case 2:
                lcd.clear();
                currentMainMenuState = MDualSNxxX;
                uart_send_string_without_delay("sn?\r\n", 0);
                uartReceive(dispSerialNum, 7);
                lcd.setCursor(0, 0);
                lcd.print(dispSerialNum);
                break;
              case 3:
                lcd.clear();
                currentMainMenuState = MDualSNxxxX;
                uart_send_string_without_delay("sn?\r\n", 0);
                uartReceive(dispSerialNum, 7);
                lcd.setCursor(0, 0);
                lcd.print(dispSerialNum);
                break;
            }
            break;



          case MDualSNX:
            switch (cursorPos) {
              case 0:
                setSNXPosition(0);
                break;
              case 1:
                setSNXPosition(1);
                break;
              case 2:
                setSNXPosition(2);
                break;
              case 3:
                setSNXPosition(3);
                break;
              case 4:
                setSNXPosition(4);
                break;
              case 5:
                setSNXPosition(5);
                break;
              case 6:
                setSNXPosition(6);
                break;
              case 7:
                setSNXPosition(7);
                break;
              case 8:
                setSNXPosition(8);
                break;
              case 9:
                setSNXPosition(9);
                break;
            }
            break;

          case MDualSNxX:
            switch (cursorPos) {
              case 0:
                setSNxXPosition(0);
                break;
              case 1:
                setSNxXPosition(1);
                break;
              case 2:
                setSNxXPosition(2);
                break;
              case 3:
                setSNxXPosition(3);
                break;
              case 4:
                setSNxXPosition(4);
                break;
              case 5:
                setSNxXPosition(5);
                break;
              case 6:
                setSNxXPosition(6);
                break;
              case 7:
                setSNxXPosition(7);
                break;
              case 8:
                setSNxXPosition(8);
                break;
              case 9:
                setSNxXPosition(9);
                break;
            }
            break;

          case MDualSNxxX:
            switch (cursorPos) {
              case 0:
                setSNxxXPosition(0);
                break;
              case 1:
                setSNxxXPosition(1);
                break;
              case 2:
                setSNxxXPosition(2);
                break;
              case 3:
                setSNxxXPosition(3);
                break;
              case 4:
                setSNxxXPosition(4);
                break;
              case 5:
                setSNxxXPosition(5);
                break;
              case 6:
                setSNxxXPosition(6);
                break;
              case 7:
                setSNxxXPosition(7);
                break;
              case 8:
                setSNxxXPosition(8);
                break;
              case 9:
                setSNxxXPosition(9);
                break;
            }
            break;

          case MDualSNxxxX:
            switch (cursorPos) {
              case 0:
                setSNxxxXPosition(0);
                break;
              case 1:
                setSNxxxXPosition(1);
                break;
              case 2:
                setSNxxxXPosition(2);
                break;
              case 3:
                setSNxxxXPosition(3);
                break;
              case 4:
                setSNxxxXPosition(4);
                break;
              case 5:
                setSNxxxXPosition(5);
                break;
              case 6:
                setSNxxxXPosition(6);
                break;
              case 7:
                setSNxxxXPosition(7);
                break;
              case 8:
                setSNxxxXPosition(8);
                break;
              case 9:
                setSNxxxXPosition(9);
                break;
            }
            break;





          case MDualPriSec:

            switch (cursorPos) {
              case 0:
                lcd.clear();
                uart_send_string_without_delay("slaveopt= 1\r\n", 10);
                break;
              case 1:
                lcd.clear();
                uart_send_string_without_delay("slaveopt= 0\r\n", 10);
                break;
            }
            break;
        }
        selectPressed = true;
      }
    } else {
      selectPressed = false;
    }





    // Display the menu based on the current state
    if (currentMainMenuState == MainMenu) {
      displayMainMenu();
    } else if (currentMainMenuState == XdcrSubMenu) {
      displayXdcrSubMenu();
    } else if (currentMainMenuState == XdcrComms) {
      displayXdcrComms();
    } else if (currentMainMenuState == XdcrCommsBaud) {
      displayXdcrCommsBaud();
      // } else if (currentMainMenuState == XdcrAddress) {
      //   displayXdcrAddress();
      // } else if (currentMainMenuState == XdcrAddressBaud) {
      //   displayXdcrCommsBaud();
    } else if (currentMainMenuState == XdcrAddressAD14) {
      displayXdcrAddressAD14();
    } else if (currentMainMenuState == M9414SubMenu) {
      displayM9414SubMenu();
    } else if (currentMainMenuState == M9414RegTest) {
      displayM9414RegTest();
    } else if (currentMainMenuState == M9414RegTestCurrentGet) {
      displayM9414RegTestCurrentGet();
    } else if (currentMainMenuState == M9414RegTestPressSettings) {
      displayM9414RegTestPressSettings();
    } else if (currentMainMenuState == M9414RegTestRevertSettings) {
      displayM9414RegTestRevertSettings();
    } else if (currentMainMenuState == M9414PressureRateSetWindow) {
      displayM9414RegTestCurrentGet();
    } else if (currentMainMenuState == M9414RxDx) {
      displayM9414RxDx();
    } else if (currentMainMenuState == MDualSubMenu) {
      displayMDualSubMenu();
    } else if (currentMainMenuState == MDualSN) {
      displayMDualSN();
    } else if (currentMainMenuState == MDualSNX) {
      displayMDualSNX();
    } else if (currentMainMenuState == MDualSNxX) {
      displayMDualSNxX();
    } else if (currentMainMenuState == MDualSNxxX) {
      displayMDualSNxxX();
    } else if (currentMainMenuState == MDualSNxxxX) {
      displayMDualSNxxxX();
    } else if (currentMainMenuState == MDualPriSec) {
      displayMDualPriSec();
    }





    // Handle the back button press
    if (digitalRead(BACK_BUTTON_PIN) == LOW) {
      if (!backPressed) {
        // If we are in a submenu, go back to the main menu
        if (currentMainMenuState != MainMenu) {
          // Serial.print("Hello World!");
          lcd.clear();
          currentMainMenuState = MainMenu;
          backPressed = true;
          delay(1000);
          digitalWrite(XDCR_PWR_PIN, XDCR_PWR_OFF);
        }
      } else {
        backPressed = false;
      }
    }


    // if (digitalRead(UP_BUTTON_PIN) == LOW) {
    //   if (!upPressed) {
    //     cursorPos = (cursorPos - 1 + 3) % 3;  // Wrap around the options
    //     upPressed = true;
    //   }
    // } else {
    //   upPressed = false;
    // }


    // if (digitalRead(DOWN_BUTTON_PIN) == LOW) {
    //   if (!downPressed) {
    //     cursorPos = (cursorPos + 1) % 3;  // Wrap around the options
    //     downPressed = true;
    //   }
    // } else {
    //   downPressed = false;
    // }


    if (digitalRead(UP_BUTTON_PIN) == LOW) {
      if (!upPressed) {
        if (currentMainMenuState == XdcrAddressAD14) {
          cursorPos = (cursorPos - 1 + numOptionAD14) % numOptionAD14;  // Wrap around the options
        } else if (currentMainMenuState == MDualSNX || currentMainMenuState == MDualSNxX || currentMainMenuState == MDualSNxxX || currentMainMenuState == MDualSNxxxX) {
          cursorPos = (cursorPos - 1 + numOptionSN09) % numOptionSN09;
        } else if (currentMainMenuState == MDualSN) {
          cursorPos = (cursorPos - 1 + 4) % 4;
        } else {
          cursorPos = (cursorPos - 1 + 3) % 3;
        }
        upPressed = true;
      }
    } else {
      upPressed = false;
    }

    if (digitalRead(DOWN_BUTTON_PIN) == LOW) {
      if (!downPressed) {
        if (currentMainMenuState == XdcrAddressAD14) {
          cursorPos = (cursorPos + 1) % numOptionAD14;
        } else if (currentMainMenuState == MDualSN || currentMainMenuState == MDualSNX || currentMainMenuState == MDualSNxX || currentMainMenuState == MDualSNxxX || currentMainMenuState == MDualSNxxxX) {
          cursorPos = (cursorPos + 1) % numOptionSN09;
        } else if (currentMainMenuState == MDualSN) {
          cursorPos = (cursorPos + 1) % 4;
        } else {
          cursorPos = (cursorPos + 1) % 3;
        }
        downPressed = true;
      }
    } else {
      downPressed = false;
    }


    // Ensure cursorPos stays within the valid range
    if (cursorPos < 0) {
      cursorPos = 0;
    } else if (cursorPos >= numOptionAD14) {
      cursorPos = numOptionAD14 - 1;
    } else if (cursorPos >= numOptionSN09) {
      cursorPos = numOptionSN09 - 1;
    }




    buttonPressed = true;
  } else {
    buttonPressed = false;
  }
}



void setup() {
  initLcd();
  pinMode(BACK_BUTTON_PIN, INPUT_PULLUP);
  pinMode(SELECT_BUTTON_PIN, INPUT_PULLUP);
  pinMode(UP_BUTTON_PIN, INPUT_PULLUP);
  pinMode(DOWN_BUTTON_PIN, INPUT_PULLUP);
  pinMode(LEFT_BUTTON_PIN, INPUT_PULLUP);
  pinMode(RIGHT_BUTTON_PIN, INPUT_PULLUP);

  displayMainMenu();

  pinMode(XDCR_PWR_PIN, OUTPUT);
  digitalWrite(XDCR_PWR_PIN, XDCR_PWR_OFF);

  uartInit(BAUD_RATE);
  // gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
  // gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
}
void loop() {


  handleButtonPresses();






  // // Send a test character
  // //uartPutChar('A');


  // const char *stringToSend = "Hello, World!";
  // for (const char *p = stringToSend; *p != '\0'; ++p) {
  //   uartPutChar(*p);
  //   // Optionally, add a small delay here if needed
  // }

  // // Wait for the next character to be available
  // while (!uart_is_readable(UART_ID)) {
  //   tight_loop_contents();
  // }

  // // Read a character
  // char ch = uart_getc(UART_ID);

  // // Print out the received character to the USB serial
  // printf("Received: %c\n", ch);

  // // Delay a little bit
  // sleep_ms(100);
}