/* Use this Code with ESP32 Wemos D1 Mini or any board with a Wroom 32 module
   Can be compiled with board library esp32 expressif V3.0.5
   This works up to 460800bps or higher
   Optimized by me Oct 24
*/

#include <BluetoothSerial.h>   // Library for classic Bluetooth
#include <BLEDevice.h>         // Library for BLE Bluetooth
#include <BLE2902.h>
#include <esp_task_wdt.h>

// ************************ Set up the following details to suit your configuration

// Set up uart pins
#define TX1_PIN 16                
#define RX1_PIN 4                 // Don't use 0 for RX, some devices can't drive it

// LED pin
#define LED_PIN  2                // Adjust if the LED is on a different pin

// #define BT_BLE_NAME      "LG290_RTK"  // Name of the ESP32 BT for Android or BLE for iOS
// #define BT_BLE_NAME      "Ublox_RTK"  // Name of the ESP32 BT for Android or BLE for iOS
#define BT_BLE_NAME      "TEST_RTK"  // Name of the ESP32 BT for Android or BLE for iOS

#define UART_BAUDRATE 460800
#define CONSOLE_RATE  115200

// *************************** End of configuration

bool sentOnce = false, blinkOnce = false;

// BLE UUIDs for the Nordic UART Service (NUS)
#define SERVICE_UUID           "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

// Watchdog timeout in seconds
#define WDT_TIMEOUT 60

BLEServer* pServer = NULL;
BLECharacteristic* pTxCharacteristic = NULL;
BLECharacteristic* pRxCharacteristic = NULL;
bool bleDeviceConnected = false;
bool oldBleDeviceConnected = false;
bool btDeviceConnected = false;

// Bluetooth for Android (Server)
BluetoothSerial SerialBTServer;

// Callback class for BLE for connected/disconnected states
class MyServerCallbacks : public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      bleDeviceConnected = true;
      Serial.println("BLE device connected.");
    };
    void onDisconnect(BLEServer* pServer) {
      bleDeviceConnected = false;
      Serial.println("BLE device disconnected.");
    }
};
// Callback class for BLE sending data to UART
class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic* pCharacteristic) override {
        String value = pCharacteristic->getValue();
        if (value.length() > 0) {
            Serial1.write((uint8_t*)value.c_str(), value.length());
        }
    }
};

void setup() {
  Serial.begin(CONSOLE_RATE);

  esp_task_wdt_config_t config = {
    .timeout_ms = WDT_TIMEOUT * 1000,
    .trigger_panic = true,      // Trigger panic if watchdog timer is not reset
  };
  esp_task_wdt_reconfigure(&config);
  esp_task_wdt_add(NULL);       // Add the current task to the watchdog

  pinMode(LED_PIN, OUTPUT);     // Initialize the LED
  digitalWrite(LED_PIN, LOW);

  // Initialize UART1 with baud rate
  Serial1.begin(UART_BAUDRATE, SERIAL_8N1, RX1_PIN, TX1_PIN);

  // Start classic Bluetooth as a server for Android devices and register callbacks for status and data
  if (SerialBTServer.begin(BT_BLE_NAME)) {
    SerialBTServer.register_callback(btServerCallback);     // Register status
    SerialBTServer.onData(onBluetoothData);                 // Register the onData callback
    Serial.println("Classic Bluetooth server started. Waiting for connection to Android device...");
  }
  else {
    Serial.println("Error starting classic Bluetooth (Server for Android).");
  }

  // BLE initialization for IOS devices
  BLEDevice::init(BT_BLE_NAME); 
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());
  BLEService* pService = pServer->createService(SERVICE_UUID);
  pTxCharacteristic = pService->createCharacteristic(
                        CHARACTERISTIC_UUID_TX,
                        BLECharacteristic::PROPERTY_READ |
                        BLECharacteristic::PROPERTY_NOTIFY
                      );
  pTxCharacteristic->addDescriptor(new BLE2902());
  pRxCharacteristic = pService->createCharacteristic(
                        CHARACTERISTIC_UUID_RX,
                        BLECharacteristic::PROPERTY_WRITE
                      );
  pRxCharacteristic->setCallbacks(new MyCallbacks());
  pService->start();
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);  
  pAdvertising->setMinPreferred(0x12);
  pAdvertising->start();
  Serial.println("BLE started and advertising sent.");

  // Start the UART task on 2nd core
  xTaskCreatePinnedToCore(uartTask, "UART Task", 8192, NULL, 1, NULL, 1);
}

void loop() {                         // Lower priority tasks
  // Check the BLE states for disconnection
  if (bleDeviceConnected && !oldBleDeviceConnected) {
    oldBleDeviceConnected = bleDeviceConnected;
  }
  if (!bleDeviceConnected && oldBleDeviceConnected) {
    oldBleDeviceConnected = bleDeviceConnected;
    pServer->startAdvertising();
    Serial.println("Advertising restarted.");
  }

  // Service the LED so you get flicker for data while connected and forwarding
  if (bleDeviceConnected || btDeviceConnected) {
    if (blinkOnce == true){
      digitalWrite(LED_PIN, HIGH);
      vTaskDelay(2 / portTICK_PERIOD_MS);
      digitalWrite(LED_PIN, LOW);
      blinkOnce = false;
      vTaskDelay(20 / portTICK_PERIOD_MS);
    } else {
      digitalWrite(LED_PIN, HIGH);
    }
  } else {
    digitalWrite(LED_PIN, LOW);
  }
  esp_task_wdt_reset();  // Watchdog reset
}

// Function for the UART task
void uartTask(void *pvParameters) {
  while (true) {
    // Receive data from UART and forward to BLE (iOS) or Bluetooth device (Android)
    while (Serial1.available()) {
      size_t len = Serial1.available();
      uint8_t buffer[len];
      Serial1.readBytes(buffer, len);
      
      if (bleDeviceConnected) {
        pTxCharacteristic->setValue(buffer, len);
        pTxCharacteristic->notify(); // Send data to connected iOS device
        sentOnce = false;
        blinkOnce = true;
      } else if (btDeviceConnected) {
        SerialBTServer.write(buffer, len);  // Send data to connected Android device
        sentOnce = false;
        blinkOnce = true;
      } else {
        if (sentOnce == false) {            // Only send message once per disconnect
          Serial.println("No BLE or BT device connected. Data not being sent.");
          sentOnce = true;
        }
      }
    }
  }
}

// Callback function triggered when data is received via classic Bluetooth, send to uart
void onBluetoothData(const uint8_t* buffer, size_t size) {
  // Process the received data and write it to UART
  for (size_t i = 0; i < size; i++) {
    Serial1.write(buffer[i]);  // Write the data to UART
  }
  blinkOnce = true;
}

// Callback function for classic Bluetooth status
void btServerCallback(esp_spp_cb_event_t event, esp_spp_cb_param_t *param) {
  if (event == ESP_SPP_SRV_OPEN_EVT) {
    btDeviceConnected = true;
    Serial.println("Classic Bluetooth device connected (Android).");
  } else if (event == ESP_SPP_CLOSE_EVT) {
    btDeviceConnected = false;
    Serial.println("Classic Bluetooth device disconnected (Android).");
  }
}