/* ============================================================================

AFSK Radio Communication System - Slave Controller (Part 1 of 3)

Designed for Arduino Uno with GPS module and SoftModem communication
Responds to master polling requests and transmits GPS location data
============================================================================
*/

/* --------------------------- Subpart 1.1 – Required Libraries

--------------------------- */
#include <Wire.h>
#include <SSD1306Ascii.h>
#include <SSD1306AsciiWire.h>
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <EEPROM.h>
#include <SoftModem.h> // SoftModem for AFSK audio over MIC/SPK
#include <avr/wdt.h> // Watchdog timer library

/* --------------------------- Subpart 1.2 – Pin Definitions (Arduino Uno)

--------------------------- */
#define MODEM_TX_PIN 9 // SoftModem TX → Radio MIC (MUST use OC1A on Uno -
PWM pin)
#define MODEM_RX_PIN 8 // SoftModem RX ← Radio SPK
#define GPS_RX 4 // GPS TX → Arduino SoftwareSerial RX
#define GPS_TX 3 // GPS RX ← Arduino SoftwareSerial TX
#define PTT_BUTTON 2 // Manual PTT Button (INPUT_PULLUP)
#define PTT_RELAY_PIN 7 // Relay control for PTT
#define TX_LED 6 // TX indicator LED
#define RX_LED 5 // RX indicator LED
#define OLED_ADDRESS 0x3C // I2C OLED Address (SDA: Pin A4, SCL: Pin A5 on Uno)

/* --------------------------- Subpart 1.3 – Configuration Constants

--------------------------- */
#define MASTER_TIMEOUT_MS 5000 // Wait time for master command
#define EEPROM_GPS_ADDR 0 // EEPROM address for saving GPS coordinates
#define OLED_IDLE_TIMEOUT 10000 // 10s idle time for default display
#define PREAMBLE_COUNT 10 // SoftModem preamble sync (adaptive)
#define PTT_DELAY_MS 500 // Delay after PTT before sending
#define WATCHDOG_TIMEOUT WDTO_8S // 8 second watchdog timeout
#define MAX_PACKET_SIZE 48 // Maximum packet size
#define BUFFER_SAFETY_MARGIN 2 // Safety margin for buffer operations
#define SYSTEM_CONFIG_ADDR 200 // EEPROM address for system config
#define PACKET_RETRY_COUNT 3 // Retries for failed transmissions
#define SERIAL_BUFFER_SIZE 64 // Size of serial command buffer

// Display settings
#define STATUS_SECTION_HEIGHT 32 // Top half of 64-pixel display
#define RESPONSE_SECTION_HEIGHT 32 // Bottom half of display
#define MAX_RESPONSE_HISTORY 4 // Store up to 4 recent responses
#define CHARS_PER_LINE 21 // Maximum characters per line
#define MESSAGE_TIMEOUT 2000 // Each message displays for 2 seconds

// SSD1306 display commands

#define SSD1306_DISPLAYON 0xAF
#define SSD1306_DISPLAYOFF 0xAE

// Slave device ID - CHANGE THIS FOR EACH SLAVE

const char* SLAVE_ID = "001"; // Slave ID matching master's SLAVE_IDS array
/* --------------------------- Subpart 1.4 - System Configuration Structure
--------------------------- */
// Structure for saving system settings to EEPROM
typedef struct {
byte configVersion; // Version of config structure
bool debugMode; // Debug output enabled
unsigned int pttDelayMs; // PTT delay in milliseconds
byte preambleCount; // Number of preamble bytes to send
byte crc; // CRC checksum
} SystemConfig;

// GPS data structure for EEPROM storage

typedef struct {
float latitude; // GPS latitude
float longitude; // GPS longitude
unsigned long timestamp; // Last update timestamp
byte isValid; // Validity flag
byte crc; // CRC checksum
} GPSData;

/* --------------------------- Subpart 2.1 – Object Initialization

--------------------------- */
TinyGPSPlus gps;
SoftwareSerial gpsSerial(GPS_RX, GPS_TX); // RX, TX on Arduino
SoftModem modem;
SSD1306AsciiWire oled;

/* --------------------------- Subpart 2.2 – Buffers and States

--------------------------- */
// Data storage
GPSData gpsData; // Current GPS data for storage
char lastValidPacket[MAX_PACKET_SIZE] = ""; // Stores the last valid packet
char receivedCommand[MAX_PACKET_SIZE] = ""; // Current command from master

// System configuration
SystemConfig sysConfig;
int currentPreambleCount = PREAMBLE_COUNT; // Dynamically adjusted preamble
int currentPttDelay = PTT_DELAY_MS; // Dynamically adjusted PTT delay

// Display queue for responses

typedef struct {
char text[MAX_PACKET_SIZE]; // Fixed buffer instead of String
unsigned long timestamp; // When added to queue
bool isLongMessage; // If message needs multiple lines
byte priority; // 0=highest, 255=lowest
} ResponseMessage;

ResponseMessage responseHistory[MAX_RESPONSE_HISTORY]; // Message storage

int responseHistoryHead = 0; // Index for adding new messages
int responseHistoryTail = 0; // Index for displaying oldest message
int responseHistoryCount = 0; // Count of messages in queue

// Static buffer for string operations to reduce heap fragmentation

char strBuffer[MAX_PACKET_SIZE + 20]; // Extra size for formatting

// Timing variables
unsigned long lastDisplayUpdate = 0;
unsigned long lastGPSCheck = 0;
unsigned long lastActionTime = 0; // For OLED idle screen
unsigned long lastResponseRotation = 0; // For message rotation timing
unsigned long lastModemActivity = 0; // For modem watchdog
unsigned long lastSystemCheck = 0; // For system health checks
unsigned long lastMemoryCheck = 0; // For memory monitoring
unsigned long lastGPSSave = 0; // For GPS data saving

// Status tracking
bool debugMode = false; // Enhanced debugging output
unsigned long packetCounter = 0; // Count packets processed
unsigned long modemErrorCounter = 0; // Count modem communication errors
unsigned short modemResetCount = 0; // Track modem resets for escalation
bool transmissionInProgress = false; // Flag for transmission in progress
bool setupSuccess = true; // Track if setup completed successfully
bool isMasterPolling = false; // Flag when master is polling us
bool isWaitingForMaster = false; // Flag when waiting for master commands

// Signal quality tracking

typedef struct {
int signalStrength; // 0-100 scale
int noiseLevel; // 0-100 scale
int successRate; // 0-100 percentage of successful transfers
unsigned long lastUpdate; // When last updated
} SignalQuality;

SignalQuality modemSignalQuality = {50, 0, 100, 0}; // Start with neutral values

// Function declarations
void logMessage(const char* message, bool isError, byte priority = 128);
void addToResponseHistory(const char* response, byte priority = 128);
void updateStatusSection();
void updateResponseSection();
void printSystemStatus();
void printHelpMenu();
void resetWatchdog();
bool validatePacketChecksum(const char* packet);
void sendResponse(const char* packetData);
void processCommand(const char* command);
bool checkGPSHealth();
bool formatGPSResponse(char* buffer, int maxSize);
bool saveGPSDataToEEPROM();
bool loadGPSDataFromEEPROM();
byte calculateConfigCRC(SystemConfig* config);
byte calculateGPSCRC(GPSData* data);
void displayWrappedMessage(const char* message, int row);
void checkResponseDisplay();

/* --------------------------- Subpart 3.1 – SoftModem Setup & Watchdog

--------------------------- */
// Enhanced modem setup with verification
bool setupModem() {
// Ensure pins are in correct state before initialization
pinMode(MODEM_TX_PIN, OUTPUT);
digitalWrite(MODEM_TX_PIN, HIGH); // Idle state
pinMode(MODEM_RX_PIN, INPUT);

// Initialize SoftModem
modem.begin();

// Brief test to verify operation

 digitalWrite(TX_LED, HIGH);
delay(50);
digitalWrite(TX_LED, LOW);

// Send sync bytes for initial carrier testing

for (int i = 0; i < 3; i++) {
modem.write(0x7E); // HDLC flag byte (01111110)
delay(5);
}

lastModemActivity = millis();
return true;
}

// Configure watchdog timer

void setupWatchdog() {
wdt_disable(); // Disable during setup
delay(100);
wdt_enable(WATCHDOG_TIMEOUT); // Enable with 8-second timeout
}

// Reset watchdog timer

void resetWatchdog() {
wdt_reset();
}

/* --------------------------- Subpart 3.2 – Configuration

Management--------------------------- */
// Calculate CRC for configuration structure
byte calculateConfigCRC(SystemConfig* config) {
byte* bytes = (byte*)config;
byte crc = 0;
for (int i = 0; i < sizeof(SystemConfig) - 1; i++) {
crc ^= bytes[i];
}
return crc;
}

// Calculate CRC for GPS data structure

byte calculateGPSCRC(GPSData* data) {
byte* bytes = (byte*)data;
byte crc = 0;
for (int i = 0; i < sizeof(GPSData) - 1; i++) {
crc ^= bytes[i];
}
return crc;
}

// Save system configuration to EEPROM

bool saveSystemConfig() {
// Prepare configuration structure
sysConfig.configVersion = 1;
sysConfig.debugMode = debugMode;
sysConfig.pttDelayMs = currentPttDelay;
sysConfig.preambleCount = currentPreambleCount;
sysConfig.crc = calculateConfigCRC(&sysConfig);

// Save to EEPROM
for (int i = 0; i < sizeof(SystemConfig); i++) {
 EEPROM.write(SYSTEM_CONFIG_ADDR + i, ((byte*)&sysConfig)[i]);
}

// Verify the write

for (int i = 0; i < sizeof(SystemConfig); i++) {
if (EEPROM.read(SYSTEM_CONFIG_ADDR + i) != ((byte*)&sysConfig)[i]) {
logMessage("Config save verification failed", true, 50);
return false;
}
}

return true;
}

// Load system configuration from EEPROM

bool loadSystemConfig() {
// Read from EEPROM
for (int i = 0; i < sizeof(SystemConfig); i++) {
((byte*)&sysConfig)[i] = EEPROM.read(SYSTEM_CONFIG_ADDR + i);
}

// Verify CRC
byte expectedCRC = calculateConfigCRC(&sysConfig);
if (sysConfig.configVersion != 1 || sysConfig.crc != expectedCRC) {
logMessage("Invalid config CRC - using defaults", true, 50);
return false;
}

// Apply settings
debugMode = sysConfig.debugMode;
currentPttDelay = sysConfig.pttDelayMs;
currentPreambleCount = sysConfig.preambleCount;

// Sanity check on values

if (currentPttDelay < 300 || currentPttDelay > 5000) {
currentPttDelay = PTT_DELAY_MS;
}

if (currentPreambleCount < 5 || currentPreambleCount > 30) {

currentPreambleCount = PREAMBLE_COUNT;
}

return true;
}

// Save GPS data to EEPROM

bool saveGPSDataToEEPROM() {
// Only save if GPS data is valid
if (!gpsData.isValid) {
return false;
}

// Update timestamp and CRC

gpsData.timestamp = millis();
gpsData.crc = calculateGPSCRC(&gpsData);

// Save to EEPROM
for (int i = 0; i < sizeof(GPSData); i++) {
EEPROM.write(EEPROM_GPS_ADDR + i, ((byte*)&gpsData)[i]);
 }

// Verify the write

for (int i = 0; i < sizeof(GPSData); i++) {
if (EEPROM.read(EEPROM_GPS_ADDR + i) != ((byte*)&gpsData)[i]) {
logMessage("GPS data save verification failed", true, 50);
return false;
}
}

if (debugMode) {
snprintf(strBuffer, sizeof(strBuffer), "Saved GPS: %f, %f",
gpsData.latitude, gpsData.longitude);
logMessage(strBuffer, false, 150);
}

return true;
}

// Load GPS data from EEPROM

bool loadGPSDataFromEEPROM() {
// Read from EEPROM
for (int i = 0; i < sizeof(GPSData); i++) {
((byte*)&gpsData)[i] = EEPROM.read(EEPROM_GPS_ADDR + i);
}

// Verify CRC
byte expectedCRC = calculateGPSCRC(&gpsData);
if (gpsData.crc != expectedCRC) {
logMessage("Invalid GPS data CRC", true, 50);

// Initialize with invalid data

gpsData.latitude = 0;
gpsData.longitude = 0;
gpsData.timestamp = 0;
gpsData.isValid = 0;

return false;
}

if (gpsData.isValid) {
snprintf(strBuffer, sizeof(strBuffer), "Loaded GPS: %f, %f",
gpsData.latitude, gpsData.longitude);
logMessage(strBuffer, false, 150);
}

return gpsData.isValid;
}
/* ============================================================================
AFSK Radio Communication System - Slave Controller (Part 2 of 3)
Packet handling, communication functions, and GPS processing
============================================================================
*/

/* --------------------------- Subpart 3.3 – Packet Handling with Checksums

--------------------------- */
// Improved CRC-8 checksum calculation
byte calculateChecksum(const char* data, int len) {
const byte polynomial = 0x07;
 byte crc = 0;

for (int i = 0; i < len; i++) {

crc ^= data[i];
for (int j = 0; j < 8; j++) {
if (crc & 0x80) {
crc = (crc << 1) ^ polynomial;
} else {
crc <<= 1;
}
}
}

return crc;
}

// Convert byte to hex string

void byteToHex(byte val, char* hexStr) {
sprintf(hexStr, "%02X", val);
}

// Validate packet checksum

bool validatePacketChecksum(const char* packet) {
int len = strlen(packet);

// Check format <data*checksum>

if (packet[0] != '<' || packet[len-1] != '>' || len < 5)
return false;

// Find star position

const char* starPos = strchr(packet, '*');
if (!starPos || starPos <= packet+1 || starPos >= packet+len-3)
return false;

// Calculate data length (exclude < and *)

int dataLen = starPos - (packet + 1);

// Extract data and checksum

char chkHex[3] = {starPos[1], starPos[2], 0};

// Calculate expected checksum

byte expectedChk = calculateChecksum(packet + 1, dataLen);
byte receivedChk = strtol(chkHex, NULL, 16);

return expectedChk == receivedChk;

}

// Send a response to the master

void sendResponse(const char* responseData) {
if (transmissionInProgress) {
logMessage("TX Error: Already transmitting", true, 50);
return;
}

transmissionInProgress = true;
digitalWrite(TX_LED, HIGH);

// Activate PTT
digitalWrite(PTT_RELAY_PIN, HIGH);
 delay(currentPttDelay);

// Build payload with boundary check

char payload[MAX_PACKET_SIZE];
strncpy(payload, responseData, sizeof(payload) - 1);
payload[sizeof(payload) - 1] = '\0';

// Calculate checksum
byte chk = calculateChecksum(payload, strlen(payload));
char chkHex[3];
byteToHex(chk, chkHex);

// Build packet with checksum

char fullPacket[MAX_PACKET_SIZE + 5]; // +5 for <, *, checksum, >
snprintf(fullPacket, sizeof(fullPacket), "<%s*%s>", payload, chkHex);

// Send HDLC flag bytes for sync - USES DYNAMIC VALUE

for (int i = 0; i < currentPreambleCount; i++) {
modem.write(0x7E); // HDLC flag byte (01111110)
}
delay(5);

// Send the actual packet

modem.print(fullPacket);

// Ensure all data is transmitted

delay(20);

// Release PTT
digitalWrite(PTT_RELAY_PIN, LOW);

// Update activity timestamp

lastModemActivity = millis();

// Log the transmission

if (debugMode) {
snprintf(strBuffer, sizeof(strBuffer), "Sending: %s", fullPacket);
logMessage(strBuffer, false, 100);
}

transmissionInProgress = false;
digitalWrite(TX_LED, LOW);

// Store the response for display

addToResponseHistory(fullPacket, 50);
}

/* --------------------------- Subpart 3.4 – Receive Packet using SoftModem with

Timeout --------------------------- */
// Receive packet with timeout
bool receiveSoftPacket(char* packet, int maxSize, unsigned long timeout) {
unsigned long start = millis();
bool startFound = false;
int packetLen = 0;

// Clear packet buffer

memset(packet, 0, maxSize);

digitalWrite(RX_LED, HIGH);
 // Pre-clear any lingering data in buffer
while (modem.available()) modem.read();

while (millis() - start < timeout) {

resetWatchdog(); // Keep watchdog happy during long waits

if (modem.available()) {
char c = modem.read();
lastModemActivity = millis(); // Update activity timestamp

if (c == '<') {
packet[0] = '<';
packetLen = 1;
startFound = true;
} else if (startFound && packetLen < maxSize - 1) {
packet[packetLen++] = c;
packet[packetLen] = '\0'; // Always null-terminate

if (c == '>') {
digitalWrite(RX_LED, LOW);
return true; // Successfully received complete packet
}
}

// Buffer safety check

if (packetLen >= maxSize - 1) {
modemErrorCounter++;
packet[maxSize-4] = '.';
packet[maxSize-3] = '.';
packet[maxSize-2] = '>';
packet[maxSize-1] = '\0';
startFound = false;
digitalWrite(RX_LED, LOW);
return false;
}
} else {
delayMicroseconds(100); // Short delay to prevent tight loop
}
}

digitalWrite(RX_LED, LOW);

// If we started to receive something but it's incomplete

if (startFound && packetLen > 1) {
modemErrorCounter++;
if (debugMode) {
Serial.print(F("[MODEM] Incomplete packet: "));
Serial.println(packet);
}
}

return false; // Timeout without complete packet

}

/* --------------------------- Subpart 4.1 – Command Processing

--------------------------- */
// Process received command from the master
void processCommand(const char* command) {
 // Validate basic packet format
int commandLen = strlen(command);
if (command[0] != '<' || command[commandLen-1] != '>' || commandLen < 3) {
logMessage("Invalid command format", true, 100);
return;
}

// Extract payload (without checksum)

char payload[MAX_PACKET_SIZE];
memset(payload, 0, sizeof(payload));

// Find checksum separator if present

const char* starPos = strchr(command, '*');
if (starPos != NULL) {
// Copy up to the star
int payloadLen = starPos - (command + 1);
if (payloadLen > 0 && payloadLen < sizeof(payload)) {
strncpy(payload, command + 1, payloadLen);
payload[payloadLen] = '\0';
}
} else {
// No checksum, copy entire content
strncpy(payload, command + 1, commandLen - 2);
payload[commandLen - 2] = '\0';
}

// Check if this is addressed to us

if (strcmp(payload, SLAVE_ID) == 0) {
logMessage("Master is polling us!", false, 50);

// Prepare GPS data response

char responseData[MAX_PACKET_SIZE];

if (formatGPSResponse(responseData, sizeof(responseData))) {
// Send GPS data
sendResponse(responseData);
} else {
// Send NOFIX response
snprintf(responseData, sizeof(responseData), "%s NOFIX", SLAVE_ID);
sendResponse(responseData);
}
} else if (debugMode) {
snprintf(strBuffer, sizeof(strBuffer), "Command not for us: %s", payload);
logMessage(strBuffer, false, 200);
}
}

/* --------------------------- Subpart 4.2 – Serial Command Handler

--------------------------- */
// Check for commands from Serial monitor
void checkSerialInput() {
static char serialBuffer[SERIAL_BUFFER_SIZE];
static int bufferPos = 0;

while (Serial.available()) {
char c = Serial.read();

// Add to buffer if not newline or carriage return

if (c != '\n' && c != '\r') {
 if (bufferPos < sizeof(serialBuffer) - 1) {
serialBuffer[bufferPos++] = c;
serialBuffer[bufferPos] = '\0'; // Keep null-terminated
}
} else if (bufferPos > 0) {
// Process the completed line
serialBuffer[bufferPos] = '\0'; // Ensure null-termination
bufferPos = 0; // Reset for next line

// Process special commands

if (strcmp(serialBuffer, "DEBUG:ON") == 0) {
debugMode = true;
Serial.println(F("[SYS] Debug mode enabled"));
saveSystemConfig();
return;
}
else if (strcmp(serialBuffer, "DEBUG:OFF") == 0) {
debugMode = false;
Serial.println(F("[SYS] Debug mode disabled"));
saveSystemConfig();
return;
}
else if (strcmp(serialBuffer, "STATUS") == 0) {
printSystemStatus();
return;
}
else if (strcmp(serialBuffer, "HELP") == 0) {
printHelpMenu();
return;
}
else if (strcmp(serialBuffer, "RESET") == 0) {
Serial.println(F("[SYS] Resetting device in 3 seconds..."));
delay(3000);
// Reset the Arduino
asm volatile ("jmp 0");
return;
}
else if (strcmp(serialBuffer, "SENDGPS") == 0) {
// Manual test to format and print GPS data
char gpsBuffer[MAX_PACKET_SIZE];
if (formatGPSResponse(gpsBuffer, sizeof(gpsBuffer))) {
Serial.print(F("[GPS] "));
Serial.println(gpsBuffer);
} else {
Serial.println(F("[GPS] NOFIX"));
}
return;
}

// Manual command sending (for testing)

if (serialBuffer[0] == '<' && strchr(serialBuffer, '>') != NULL) {
processCommand(serialBuffer);
lastActionTime = millis();
} else if (strlen(serialBuffer) > 0) {
Serial.println(F("[ERROR] Invalid format. Type HELP for commands"));
}
}
}
}
/* --------------------------- Subpart 5.1 – GPS Handling Functions
--------------------------- */
// Check GPS module health and process data
bool checkGPSHealth() {
static unsigned long lastGoodGPS = 0;
const unsigned long MAX_GPS_PROCESS_TIME = 25; // ms

// Process GPS data with time limit to prevent blocking

unsigned long startTime = millis();
int bytesProcessed = 0;
while (gpsSerial.available() && (millis() - startTime < MAX_GPS_PROCESS_TIME)) {
if (gps.encode(gpsSerial.read())) {
// New sentence was processed
bytesProcessed++;

// Check if we have a valid position

if (gps.location.isValid()) {
lastGoodGPS = millis();

// Update our GPS data structure if position changed

if (gpsData.latitude != gps.location.lat() ||
gpsData.longitude != gps.location.lng()) {

gpsData.latitude = gps.location.lat();
gpsData.longitude = gps.location.lng();
gpsData.isValid = 1;

// Save GPS data to EEPROM (but limit writes to once per minute)
if (millis() - lastGPSSave > 60000) {
saveGPSDataToEEPROM();
lastGPSSave = millis();
}

// Update the display

updateStatusSection();
}
}
}
}

if (debugMode && bytesProcessed > 0) {

snprintf(strBuffer, sizeof(strBuffer), "Processed %d GPS bytes",
bytesProcessed);
logMessage(strBuffer, false, 200);
}

// Check if we're still receiving data

if (gps.charsProcessed() < 10 && millis() > 5000) {
if (debugMode) {
logMessage("No data from GPS module", true, 70);
}
return false;
}

// Check if we have a valid fix

if (gps.location.isValid()) {
return true;
} else if (millis() - lastGoodGPS > 120000 && lastGoodGPS > 0) {
 // We had a fix before but lost it for over 2 minutes
logMessage("Lost GPS fix for over 2 minutes", false, 100);
}

// We either have no fix, or lost it recently

return false;
}

// Format GPS data for response to master

bool formatGPSResponse(char* buffer, int maxSize) {
// First check if we have a fresh GPS fix
if (gps.location.isValid()) {
// We have a fresh fix, use it
snprintf(buffer, maxSize, "%s %f %f %04d-%02d-%02d %02d:%02d:%02d",
SLAVE_ID,
gps.location.lat(), gps.location.lng(),
gps.date.year(), gps.date.month(), gps.date.day(),
gps.time.hour(), gps.time.minute(), gps.time.second());
return true;
}
// No fresh fix, check if we have stored GPS data
else if (gpsData.isValid) {
// Use last known position with current time if available
if (gps.date.isValid() && gps.time.isValid()) {
snprintf(buffer, maxSize, "%s %f %f %04d-%02d-%02d %02d:%02d:%02d",
SLAVE_ID,
gpsData.latitude, gpsData.longitude,
gps.date.year(), gps.date.month(), gps.date.day(),
gps.time.hour(), gps.time.minute(), gps.time.second());
return true;
}
// Use last known position with stored timestamp
else {
// Convert milliseconds timestamp to readable date/time (approximate)
unsigned long seconds = gpsData.timestamp / 1000;
unsigned long minutes = seconds / 60;
unsigned long hours = minutes / 60;

snprintf(buffer, maxSize, "%s %f %f NO-DATE %02lu:%02lu:%02lu",

SLAVE_ID,
gpsData.latitude, gpsData.longitude,
hours % 24, minutes % 60, seconds % 60);
return true;
}
}

// No valid GPS data available

return false;
}

/* ============================================================================
AFSK Radio Communication System - Slave Controller (Part 3 of 3)
Display functions, system status, and main program logic
============================================================================
*/

/* --------------------------- Subpart 5.2 – Display Management Functions

--------------------------- */
// Add a message to the response history queue with priority
void addToResponseHistory(const char* response, byte priority) {
if (response == NULL || strlen(response) == 0) return;

// First handle the case where there's space in the queue

if (responseHistoryCount < MAX_RESPONSE_HISTORY) {
// Copy the text with truncation if needed
strncpy(responseHistory[responseHistoryHead].text, response, MAX_PACKET_SIZE -
1);
responseHistory[responseHistoryHead].text[MAX_PACKET_SIZE - 1] = '\0';

responseHistory[responseHistoryHead].timestamp = millis();
responseHistory[responseHistoryHead].isLongMessage = (strlen(response) >
CHARS_PER_LINE);
responseHistory[responseHistoryHead].priority = priority;

// Update head pointer and count

responseHistoryHead = (responseHistoryHead + 1) % MAX_RESPONSE_HISTORY;
responseHistoryCount++;
}
// If queue is full, we need to decide what to replace
else {
// Find lowest priority message (higher number = lower priority)
byte lowestPriority = 0;
int lowestPriorityIdx = -1;

for (int i = 0; i < MAX_RESPONSE_HISTORY; i++) {

int idx = (responseHistoryTail + i) % MAX_RESPONSE_HISTORY;
// Lower number = higher priority
if (responseHistory[idx].priority > lowestPriority) {
lowestPriority = responseHistory[idx].priority;
lowestPriorityIdx = idx;
}
}

// Only replace if new message has higher priority (lower number)

if (lowestPriorityIdx >= 0 && priority < lowestPriority) {
strncpy(responseHistory[lowestPriorityIdx].text, response, MAX_PACKET_SIZE -
1);
responseHistory[lowestPriorityIdx].text[MAX_PACKET_SIZE - 1] = '\0';
responseHistory[lowestPriorityIdx].timestamp = millis();
responseHistory[lowestPriorityIdx].isLongMessage = (strlen(response) >
CHARS_PER_LINE);
responseHistory[lowestPriorityIdx].priority = priority;
}
}

// Force immediate display update

updateResponseSection();

// Reset rotation timer

lastResponseRotation = millis();
lastActionTime = millis(); // Reset idle timer too
}

// Log a message to both Serial and response history

void logMessage(const char* message, bool isError, byte priority) {
const char* prefix = isError ? "ERROR: " : "INFO: ";

// Log to serial with timestamp

 Serial.print(F("["));
Serial.print(millis() / 1000);
Serial.print(F("s] "));
Serial.print(prefix);
Serial.println(message);

// Format for display queue

snprintf(strBuffer, sizeof(strBuffer), "%s%s", prefix, message);

// Add to response history for OLED display

addToResponseHistory(strBuffer, priority);

// Reset idle timer

lastActionTime = millis();
}

// Clear old messages (expired after timeout)

void clearExpiredMessages() {
unsigned long currentTime = millis();
int messagesToRemove = 0;

// Check from tail and count expired messages

for (int i = 0; i < responseHistoryCount; i++) {
int idx = (responseHistoryTail + i) % MAX_RESPONSE_HISTORY;
if (currentTime - responseHistory[idx].timestamp > MESSAGE_TIMEOUT) {
messagesToRemove++;
} else {
break; // Remaining messages haven't expired yet
}
}

// Remove expired messages

if (messagesToRemove > 0) {
responseHistoryTail = (responseHistoryTail + messagesToRemove) %
MAX_RESPONSE_HISTORY;
responseHistoryCount -= messagesToRemove;

// Update display to show new state

updateResponseSection();
}
}

// Display a message with wrapping if needed

void displayWrappedMessage(const char* message, int row) {
if (message == NULL || strlen(message) == 0) return;

if (strlen(message) <= CHARS_PER_LINE) {

// Single line message
oled.setCursor(0, row);
oled.print(message);
} else {
// First line
oled.setCursor(0, row);

// Safely display first line with null termination

char firstLine[CHARS_PER_LINE + 1];
strncpy(firstLine, message, CHARS_PER_LINE);
firstLine[CHARS_PER_LINE] = '\0';
oled.print(firstLine);
 // Second line (if within display bounds)
if (row + 1 < (STATUS_SECTION_HEIGHT + RESPONSE_SECTION_HEIGHT) / 8) {
oled.setCursor(0, row + 1);

// Calculate remaining text length safely

int messageLen = strlen(message);
int remainingLength = messageLen - CHARS_PER_LINE;

// Add ellipsis if too long for second line

if (remainingLength > CHARS_PER_LINE) {
char secondLine[CHARS_PER_LINE + 1];
strncpy(secondLine, message + CHARS_PER_LINE, CHARS_PER_LINE - 3);
secondLine[CHARS_PER_LINE - 3] = '.';
secondLine[CHARS_PER_LINE - 2] = '.';
secondLine[CHARS_PER_LINE - 1] = '.';
secondLine[CHARS_PER_LINE] = '\0';
oled.print(secondLine);
} else if (remainingLength > 0) {
oled.print(message + CHARS_PER_LINE);
}
}
}
}

// Update the status section (top half of display)

void updateStatusSection() {
// Save current cursor position
byte oldRow = oled.row();
byte oldCol = oled.col();

// Clear the status section only (not the entire display)

for (int i = 0; i < STATUS_SECTION_HEIGHT / 8; i++) {
oled.setCursor(0, i);
oled.clearToEOL();
}

// Move to top of status section

oled.setCursor(0, 0);

// Draw separator line

for (int i = 0; i < CHARS_PER_LINE; i++) {
oled.print(F("-"));
}

// Print slave ID and signal quality indicator

oled.setCursor(0, 1);
oled.print(F("ID:"));
oled.print(SLAVE_ID);
oled.print(F(" SQ:"));
// Print signal quality bars (0-5)
int bars = map(modemSignalQuality.successRate, 0, 100, 0, 5);
for (int i = 0; i < 5; i++) {
oled.print(i < bars ? F("#") : F("."));
}

// Show GPS info if valid

if (gps.location.isValid()) {
// Position
 oled.setCursor(0, 2);
oled.print(F("LAT: "));
oled.print(gps.location.lat(), 6);

oled.setCursor(0, 3);
oled.print(F("LON: "));
oled.print(gps.location.lng(), 6);

// Date and Time - with null checks

if (gps.date.isValid() && gps.time.isValid()) {
char dateTime[22]; // Buffer for date and time
sprintf(dateTime, "%04d-%02d-%02d %02d:%02d:%02d",
gps.date.year(), gps.date.month(), gps.date.day(),
gps.time.hour(), gps.time.minute(), gps.time.second());

oled.setCursor(0, STATUS_SECTION_HEIGHT / 8 - 1);

oled.print(dateTime);
}
}
// No fresh GPS fix but have stored data
else if (gpsData.isValid) {
oled.setCursor(0, 2);
oled.print(F("LAT(mem): "));
oled.print(gpsData.latitude, 6);

oled.setCursor(0, 3);
oled.print(F("LON(mem): "));
oled.print(gpsData.longitude, 6);
}
// No GPS data at all
else {
oled.setCursor(0, 2);
oled.print(F("GPS: NO FIX"));

// Show satellites if available

if (gps.satellites.isValid()) {
oled.setCursor(0, 3);
oled.print(F("Satellites: "));
oled.print(gps.satellites.value());
}
}

// Draw separator line at bottom of status section

oled.setCursor(0, STATUS_SECTION_HEIGHT / 8 - 1);
for (int i = 0; i < CHARS_PER_LINE; i++) {
oled.print(F("-"));
}

// Restore cursor if it was in bottom section

if (oldRow >= STATUS_SECTION_HEIGHT / 8) {
oled.setCursor(oldCol, oldRow);
}

lastDisplayUpdate = millis();
}

// Update the response section (bottom half of display)

void updateResponseSection() {
// Start row for response section
 int startRow = STATUS_SECTION_HEIGHT / 8;

// Clear the response section

for (int i = 0; i < RESPONSE_SECTION_HEIGHT / 8; i++) {
oled.setCursor(0, startRow + i);
oled.clearToEOL();
}

// If no messages, just return

if (responseHistoryCount == 0) {
return;
}

// Calculate how many messages we can show

int currentRow = startRow;
int displayedCount = 0;

// Start from the oldest message (tail)

int currentIndex = responseHistoryTail;

// Display messages until we run out of space or messages

while (displayedCount < responseHistoryCount &&
currentRow < (STATUS_SECTION_HEIGHT + RESPONSE_SECTION_HEIGHT) / 8) {

// Get the current message

const char* message = responseHistory[currentIndex].text;
bool isLong = responseHistory[currentIndex].isLongMessage;

// Display message (handles wrapping if needed)

displayWrappedMessage(message, currentRow);

// Update row counter - long messages take 2 rows

if (isLong && currentRow + 2 <= (STATUS_SECTION_HEIGHT +
RESPONSE_SECTION_HEIGHT) / 8) {
currentRow += 2;
} else {
currentRow += 1;
}

// Move to next message

currentIndex = (currentIndex + 1) % MAX_RESPONSE_HISTORY;
displayedCount++;

// Stop if we've wrapped around

if (currentIndex == responseHistoryHead) {
break;
}
}
}

// Check if we need to update the display

void checkResponseDisplay() {
unsigned long currentTime = millis();

// Clear expired messages (older than MESSAGE_TIMEOUT)

if (currentTime - lastResponseRotation > MESSAGE_TIMEOUT) {
clearExpiredMessages();
lastResponseRotation = currentTime;
}
}

/* --------------------------- Subpart 5.3 – Status and Help Interface

--------------------------- */
// Print system status to Serial monitor
void printSystemStatus() {
Serial.println(F("\n===== SYSTEM STATUS ====="));
Serial.print(F("Slave ID: "));
Serial.println(SLAVE_ID);
Serial.print(F("Time Running: "));
Serial.print(millis() / 1000);
Serial.println(F(" seconds"));
Serial.print(F("Debug Mode: "));
Serial.println(debugMode ? F("ON") : F("OFF"));
Serial.print(F("Current PTT Delay: "));
Serial.print(currentPttDelay);
Serial.println(F("ms"));
Serial.print(F("Current Preamble Count: "));
Serial.println(currentPreambleCount);
Serial.print(F("Packets Processed: "));
Serial.println(packetCounter);
Serial.print(F("Modem Errors: "));
Serial.println(modemErrorCounter);
Serial.print(F("GPS Fix: "));
Serial.println(gps.location.isValid() ? F("YES") : F("NO"));
Serial.print(F("GPS Data Stored: "));
Serial.println(gpsData.isValid ? F("YES") : F("NO"));

// Signal quality information

Serial.print(F("Signal Quality: "));
Serial.print(modemSignalQuality.successRate);
Serial.println(F("%"));

if (gps.location.isValid()) {
Serial.print(F("GPS Position: "));
Serial.print(gps.location.lat(), 6);
Serial.print(F(", "));
Serial.println(gps.location.lng(), 6);
Serial.print(F("GPS Date/Time: "));
if (gps.date.isValid() && gps.time.isValid()) {
char dateTime[22];
sprintf(dateTime, "%04d-%02d-%02d %02d:%02d:%02d",
gps.date.year(), gps.date.month(), gps.date.day(),
gps.time.hour(), gps.time.minute(), gps.time.second());
Serial.println(dateTime);
} else {
Serial.println(F("Invalid"));
}
} else if (gpsData.isValid) {
Serial.println(F("Using stored GPS data:"));
Serial.print(F("GPS Position: "));
Serial.print(gpsData.latitude, 6);
Serial.print(F(", "));
Serial.println(gpsData.longitude, 6);
}

// Memory health check - fixed section

#ifdef _arm_
// ARM boards like Due don't use this memory method
 Serial.print(F("\nFree Memory: Not available on this board"));
#else
// AVR boards like Uno, Mega, etc.
extern int __heap_start, *__brkval;
int freeMemory;
if ((int)__brkval == 0)
freeMemory = ((int)&freeMemory) - ((int)&__heap_start);
else
freeMemory = ((int)&freeMemory) - ((int)__brkval);
Serial.print(F("\nFree Memory: "));
Serial.print(freeMemory);
Serial.println(F(" bytes"));
#endif

Serial.println(F("=======================\n"));
}

// Print help menu to Serial monitor

void printHelpMenu() {
Serial.println(F("\n===== HELP MENU ====="));
Serial.print(F("Slave Device: "));
Serial.println(SLAVE_ID);
Serial.println(F("Available Commands:"));
Serial.println(F(" DEBUG:ON - Enable debug output"));
Serial.println(F(" DEBUG:OFF - Disable debug output"));
Serial.println(F(" STATUS - Display system status"));
Serial.println(F(" HELP - Show this menu"));
Serial.println(F(" RESET - Reset the device"));
Serial.println(F(" SENDGPS - Manually format and print GPS data"));

Serial.println(F("\nTest Commands:"));
Serial.print(F(" <"));
Serial.print(SLAVE_ID);
Serial.println(F("> - Simulate master poll"));
Serial.println(F(" <CFG:PTTDELAY=500> - Set PTT delay (NOT IMPLEMENTED)"));

Serial.println(F("====================\n"));
}

/* --------------------------- Subpart 6.1 – Main Program Logic

--------------------------- */
// Check for commands from master
void checkForMasterCommands() {
static unsigned long lastCheck = 0;

// Check every 100ms

if (millis() - lastCheck > 100) {
lastCheck = millis();

if (modem.available()) {
char command[MAX_PACKET_SIZE];
if (receiveSoftPacket(command, sizeof(command), 1000)) {
// Process the command
processCommand(command);
// Log it
addToResponseHistory(command, 50);
}
}
}
}

/* --------------------------- Subpart 8.1 – Arduino Main Functions

--------------------------- */
// Enhanced setup function
void setup() {
// Initialize serial interfaces
Serial.begin(9600);
gpsSerial.begin(9600);

// Initialize I2C and OLED

Wire.begin();
oled.begin(&Adafruit128x64, OLED_ADDRESS);
oled.setFont(System5x7);
oled.clear();
oled.print(F("Slave "));
oled.println(SLAVE_ID);
oled.println(F("Booting..."));
delay(500);

// Initialize pin modes

pinMode(MODEM_TX_PIN, OUTPUT); digitalWrite(MODEM_TX_PIN, HIGH);
pinMode(MODEM_RX_PIN, INPUT);
pinMode(PTT_BUTTON, INPUT_PULLUP);
pinMode(PTT_RELAY_PIN, OUTPUT); digitalWrite(PTT_RELAY_PIN, LOW);
pinMode(TX_LED, OUTPUT); digitalWrite(TX_LED, LOW);
pinMode(RX_LED, OUTPUT); digitalWrite(RX_LED, LOW);

// Load system configuration from EEPROM

if (!loadSystemConfig()) {
// If loading fails, use defaults
currentPttDelay = PTT_DELAY_MS;
currentPreambleCount = PREAMBLE_COUNT;
debugMode = false;

// Save default config

saveSystemConfig();
}

// Load saved GPS data from EEPROM

loadGPSDataFromEEPROM();

// Initialize SoftModem
if (!setupModem()) {
Serial.println(F("WARNING: SoftModem initialization failed"));
setupSuccess = false;
}

// Setup watchdog timer

setupWatchdog();

// Initialize response queue

for (int i = 0; i < MAX_RESPONSE_HISTORY; i++) {
responseHistory[i].text[0] = '\0';
responseHistory[i].timestamp = 0;
responseHistory[i].isLongMessage = false;
responseHistory[i].priority = 255;
}
 // Initialize the display system
updateStatusSection(); // Top half
updateResponseSection(); // Bottom half

// Final setup status

if (setupSuccess) {
snprintf(strBuffer, sizeof(strBuffer), "Slave %s Ready", SLAVE_ID);
logMessage(strBuffer, false, 50);
} else {
logMessage("WARNING: System errors detected", true, 50);
// Blink error pattern on LEDs
for (int i = 0; i < 3; i++) {
digitalWrite(TX_LED, HIGH);
digitalWrite(RX_LED, HIGH);
delay(200);
digitalWrite(TX_LED, LOW);
digitalWrite(RX_LED, LOW);
delay(200);
}
}
}

// Main loop function

void loop() {
// Reset watchdog timer
resetWatchdog();

// Check for commands from Serial

checkSerialInput();

// Check for commands from master

checkForMasterCommands();

// Process GPS data

checkGPSHealth();

// Update display if needed

checkResponseDisplay();

// Check idle display timeout

if (millis() - lastActionTime > OLED_IDLE_TIMEOUT) {
updateStatusSection(); // Refresh status section if idle
lastActionTime = millis();
}

// Small delay to prevent tight looping

delay(10);
}

// End of code