/*
  SparkFun Apple Accessory Arduino Library - Example

  ESP32_BluetoothSerial

  This example demonstrates how to create an Apple Accessory,
  based on ESP32 hardware, which will communicate with Apple
  devices using Bluetooth transport.

  Details of the Apple Accessory Protocol are only available under NDA.
  The SparkFun Apple Accessory Arduino Library is pre-compiled for ESP32
  to protect the Apple IP contained in the source code.

  This example shares the NMEA Location Information from a u-blox GNSS
  with the iPhone, allowing map apps to use the location. It requires a
  u-blox GNSS which supports the configuration interface: F9, M10, X20.

  Requires the SparkFun_u-blox_GNSS_v3 library >= v3.1.10

  NMEA GGA and RMC are shared with apps like Maps, over the Control Session
  as Location Information.
  NMEA GGA, RMC, GST, VTG, GSA and GSV are shared with apps like Field Maps,
  over a dedicated External Accessory protocol session.

  This example contains a copy of Espressif's BluetoothSerial code, with a
  modified ESP_BT_GAP_ACL_CONN_CMPL_STAT_EVT event.

  This example also needs a custom-compiled version of the Espressif libbt.a
  Bluetooth library. The custom version can be found in the "patch" folder.
  It needs to be copied into:
    C:\Users\<YOUR USER>\AppData\Local\Arduino15\packages\esp32\tools
    \esp32-arduino-libs\idf-release_v5.1-632e0c2a\esp32\lib
  We created it using the Espressif ESP32 Arduino Lib Builder:
    https://github.com/espressif/esp32-arduino-lib-builder
  It includes:
    CONFIG_BT_ENABLED=y
    CONFIG_BT_CLASSIC_ENABLED=y
    CONFIG_BT_A2DP_ENABLE=y
    CONFIG_BT_SPP_ENABLED=y
    CONFIG_BT_HFP_ENABLE=y
    CONFIG_BT_STACK_NO_LOG=y
    CONFIG_BT_BLE_DYNAMIC_ENV_MEMORY=y
    CONFIG_BT_BLUEDROID_ENABLED=y
    CONFIG_BT_L2CAP_ENABLED=y
    CONFIG_BT_SDP_COMMON_ENABLED=y
    CONFIG_BTDM_CTRL_MODE_BTDM=y
    CONFIG_BTDM_SCAN_DUPL_CACHE_SIZE=20
    CONFIG_BTDM_CTRL_MODE_BLE_ONLY=n
    CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=n
  (SDP is disabled in the standard libbt.a)
  It also corrects the UUID byte-order reversal in add_raw_sdp

  Written for and tested on: Arduino esp32 v3.0.7 (IDF 5.1)

*/

// ===================================================================================================================
// ESP32 Service Discovery Protocol

#include "esp_sdp_api.h"

// ===================================================================================================================
// GNSS

#include <SparkFun_u-blox_GNSS_v3.h> //Click here to get the library: http://librarymanager/All#SparkFun_u-blox_GNSS_v3
SFE_UBLOX_GNSS myGNSS;
char latestGPGGA[SFE_UBLOX_MAX_NMEA_BYTE_COUNT] = { 0 };
char latestGPRMC[SFE_UBLOX_MAX_NMEA_BYTE_COUNT] = { 0 };
const size_t additionalNmeaBufferSize = 4096;
char additionalNmeaBuffer[additionalNmeaBufferSize] = { 0 };

void newGPGGA(NMEA_GGA_data_t *nmeaData)
{
  if (nmeaData->length < SFE_UBLOX_MAX_NMEA_BYTE_COUNT)
  {
    strncpy(latestGPGGA, (const char *)nmeaData->nmea, nmeaData->length);
    //Serial.print(latestGPGGA); // .nmea is printable (NULL-terminated) and already has \r\n on the end
    latestGPGGA[nmeaData->length - 2] = 0; // Truncate after checksum. Remove CR LF
  }
}

void newGPRMC(NMEA_RMC_data_t *nmeaData)
{
  if (nmeaData->length < SFE_UBLOX_MAX_NMEA_BYTE_COUNT)
  {
    strncpy(latestGPRMC, (const char *)nmeaData->nmea, nmeaData->length);
    //Serial.print(latestGPRMC); // .nmea is printable (NULL-terminated) and already has \r\n on the end
    latestGPRMC[nmeaData->length - 2] = 0; // Truncate after checksum. Remove CR LF
  }
}

// ===================================================================================================================
// Apple Accessory

#include <SparkFun_Apple_Accessory.h>

SparkFunAppleAccessoryDriver appleAccessory;

const char *accessoryName = "SparkPNT RTK Flex";
const char *modelIdentifier = "SparkPNT RTK Flex";
const char *manufacturer = "SparkFun Electronics";
const char *serialNumber = "123456";
const char *firmwareVersion = "1.0.0";
const char *hardwareVersion = "1.0.0";
const char *EAProtocol = "com.sparkfun.rtk";
const char *BTTransportName = "Bluetooth";
const char *LIComponentName = "com.sparkfun.li";
const char *productPlanUID = "0123456789ABCDEF"; // This comes from the MFi Portal, when you register the product with Apple

// ===================================================================================================================
// Bluetooth

#include "src/BluetoothSerial/BluetoothSerial.h" //Local copy for modifying ESP_BT_GAP_ACL_CONN_CMPL_STAT_EVT event
BluetoothSerial SerialBT;

const char *sdp_service_name = "iAP2";

const int rfcommChanneliAP2 = 2; // We can use RFCOMM channel 2 for iAP2

// With the byte-order correction in libbt.a btc_sdp.c add_raw_sdp() (ARRAY_TO_BE_STREAM_REVERSE replaces ARRAY_TO_BE_STREAM),
// we need to provide the UUID as little-endian:
// UUID : Little-Endian
static const uint8_t  UUID_IAP2[] = {0xFF, 0xCA, 0xCA, 0xDE, 0xAF, 0xDE, 0xCA, 0xDE, 0xDE, 0xFA, 0xCA, 0xDE, 0x00, 0x00, 0x00, 0x00};

uint8_t btMacAddress[6];

void transportConnected(bool *isConnected)
{
  *isConnected = SerialBT.connected();
}

void transportDisconnect(bool *disconnected)
{
  *disconnected = SerialBT.disconnect();
}

// ===================================================================================================================
// Callback for Service Discovery Protocol
// This allows the iAP2 record to be created _after_ SDP is initialized
// and the Serial Port (SPP) record has been created

volatile bool sdpCreateRecordEvent = false; // Flag to indicate when the iAP2 record has been created

static void esp_sdp_callback(esp_sdp_cb_event_t event, esp_sdp_cb_param_t *param)
{
    switch (event) {
    case ESP_SDP_INIT_EVT:
        Serial.printf("ESP_SDP_INIT_EVT: status: %d\r\n", param->init.status);
        if (param->init.status == ESP_SDP_SUCCESS) {
            // SDP has been initialized. _Now_ we can create the iAP2 record!
            esp_bluetooth_sdp_hdr_overlay_t record = {(esp_bluetooth_sdp_types_t)0};
            record.type = ESP_SDP_TYPE_RAW;
            record.uuid.len = sizeof(UUID_IAP2);
            memcpy(record.uuid.uuid.uuid128, UUID_IAP2, sizeof(UUID_IAP2));
            // The service_name isn't critical. But we can't not set one.
            // (If we don't set a name, the record doesn't get created.)
            record.service_name_length = strlen(sdp_service_name) + 1;
            record.service_name = (char *)sdp_service_name;
            record.rfcomm_channel_number = rfcommChanneliAP2; // RFCOMM channel
            record.l2cap_psm = -1;
            record.profile_version = -1;
            esp_sdp_create_record((esp_bluetooth_sdp_record_t *)&record);
        }
        break;
    case ESP_SDP_DEINIT_EVT:
        Serial.printf("ESP_SDP_DEINIT_EVT: status: %d\r\n", param->deinit.status);
        break;
    case ESP_SDP_SEARCH_COMP_EVT:
        Serial.printf("ESP_SDP_SEARCH_COMP_EVT: status: %d\r\n", param->search.status);
        break;
    case ESP_SDP_CREATE_RECORD_COMP_EVT:
        Serial.printf("ESP_SDP_CREATE_RECORD_COMP_EVT: status: %d\r\n", param->create_record.status);
        sdpCreateRecordEvent = true; // Flag that the iAP2 record has been created
        break;
    case ESP_SDP_REMOVE_RECORD_COMP_EVT:
        Serial.printf("ESP_SDP_REMOVE_RECORD_COMP_EVT: status: %d\r\n", param->remove_record.status);
        break;
    default:
        break;
    }
}

// ===================================================================================================================

void setup()
{
  Serial.begin(115200);
  delay(250);

  Wire.begin(); // Needed for authentication coprocessor

  // ==============================================================================================================
  // Setup Bluetooth

  SerialBT.enableSSP(false, false); //Enable secure pairing

  //Bluetooth device name, master mode, disable BLE, rxQueueSize, txQueueSize
  SerialBT.begin(accessoryName, false, true, 2048, 512);

  Serial.println("The BT device was started.");

  SerialBT.getBtAddress(btMacAddress); // Read the ESP32 BT MAC Address
  Serial.print("BT MAC: ");
  for (uint8_t i = 0; i < 6; i++)
    Serial.printf("%02X ",btMacAddress[i]);
  Serial.println();

  // Comment the next two lines if you do not want or need to delete the previous pairings.
  // (But, deleting them is essential if you have changed the secure pairing mode)
  Serial.println("Deleting all previous bonded devices.");
  SerialBT.deleteAllBondedDevices(); // Must be called after begin

  // The SDP callback will create the iAP2 record
  esp_sdp_register_callback(esp_sdp_callback);
  esp_sdp_init();

  // ==============================================================================================================
  // Setup Apple Accessory and Authentication Coprocessor

  appleAccessory.usePSRAM(false); // Tell the driver whether to use PSRAM - before begin

  // Check the authentication coprocessor is connected - and awake
  if(!appleAccessory.begin(Wire))
  {
    Serial.println("Could not initialize the authentication coprocessor. Freezing...");
    while(1);
  }

  //appleAccessory.enableDebug(&Serial); // Uncomment to enable debug prints to Serial

  // Pass Identity Information, Protocols and Names into the accessory driver
  appleAccessory.setAccessoryName(accessoryName);
  appleAccessory.setModelIdentifier(modelIdentifier);
  appleAccessory.setManufacturer(manufacturer);
  appleAccessory.setSerialNumber(serialNumber);
  appleAccessory.setFirmwareVersion(firmwareVersion);
  appleAccessory.setHardwareVersion(hardwareVersion);
  appleAccessory.setExternalAccessoryProtocol(EAProtocol);
  appleAccessory.setBluetoothTransportName(BTTransportName);
  appleAccessory.setBluetoothMacAddress(btMacAddress);
  appleAccessory.setLocationInfoComponentName(LIComponentName);
  appleAccessory.setProductPlanUID(productPlanUID);

  // Pass the pointers for the latest NMEA GGA and RMC data into the Accessory driver
  // GGA and RMC are passed to apps over the iAP2 Control Session
  appleAccessory.setNMEApointers(latestGPGGA, latestGPRMC);

  // Pass the pointer for additional NMEA GST, VTG, GSA and GSV data
  // GGA, RMC, GST, VTG, GSA and GSV are passed to apps over an iAP2 EA Session 
  appleAccessory.setEASessionPointer(additionalNmeaBuffer);

  // Pass the transport connected and disconnect methods into the accessory driver
  appleAccessory.setTransportConnectedMethod(&transportConnected);
  appleAccessory.setTransportDisconnectMethod(&transportDisconnect);

  // ==============================================================================================================
  // Begin the GNSS. Set up callbacks for the NMEA data

  //myGNSS.enableDebugging(); // Uncomment this line to enable debug messages on Serial

  // Storage for NMEA GST, VTG, GSA and GSV. setFileBufferSize must be called _before_ .begin
  myGNSS.setFileBufferSize(additionalNmeaBufferSize);

  while (!myGNSS.begin())
  {
    Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring."));
  }

  // Disable or enable various NMEA sentences over the I2C interface
  myGNSS.setI2COutput(COM_TYPE_NMEA | COM_TYPE_UBX); // Turn on both UBX and NMEA sentences on I2C. (Turn off RTCM and SPARTN)
  myGNSS.newCfgValset(VAL_LAYER_RAM_BBR); // Use cfgValset to disable / enable individual NMEA messages
  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GLL_I2C, 0); // Enable required NMEA messages
  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GSA_I2C, 1);
  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GSV_I2C, 1);
  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_RMC_I2C, 1);
  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_VTG_I2C, 1);
  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GGA_I2C, 1);
  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_I2C, 0);
  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_NMEA_ID_GST_I2C, 1);
  myGNSS.addCfgValset(UBLOX_CFG_NMEA_PROTVER, 23); // Set the NMEA version to 2.3
  myGNSS.addCfgValset(UBLOX_CFG_NMEA_OUT_FROZENCOG, 1); // Always output the RMC Course Over Ground
  if (myGNSS.sendCfgValset()) // Send the configuration VALSET
    Serial.println(F("NMEA messages were configured successfully"));
  else
    Serial.println(F("NMEA message configuration failed!"));

  // Set the Main Talker ID to "GP". The NMEA GGA messages will be GPGGA instead of GNGGA
  myGNSS.setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);

  myGNSS.setHighPrecisionMode(true); // Enable High Precision Mode - include extra decimal places in the GGA messages

  // Set up the callback for GPGGA
  myGNSS.setNMEAGPGGAcallbackPtr(&newGPGGA);

  // Set up the callback for GPRMC
  myGNSS.setNMEAGPRMCcallbackPtr(&newGPRMC);

  // Log NMEA GST, VTG, GSA and GSV in the GNSS library file buffer
  myGNSS.setNMEALoggingMask(SFE_UBLOX_FILTER_NMEA_GST | SFE_UBLOX_FILTER_NMEA_VTG
                            | SFE_UBLOX_FILTER_NMEA_GSA | SFE_UBLOX_FILTER_NMEA_GSV);
}

// ===================================================================================================================

void loop()
{
  // ==============================================================================================================
  // Update the Accessory driver

  appleAccessory.update();

  // ==============================================================================================================
  // Update the GNSS

  myGNSS.checkUblox(); // Check for the arrival of new data and process it.
  myGNSS.checkCallbacks(); // Check if any callbacks are waiting to be processed.

  // ==============================================================================================================
  // Copy latest GST, VTG, GSA and GSV into additionalNmeaBuffer

  // If the Apple Accessory is not sending data to the EA Session, copy the data into additionalNmeaBuffer.
  // Bad things would happen if we were to manipulate additionalNmeaBuffer while appleAccessory is using it.
  if (appleAccessory.latestEASessionDataIsBlocking() == false)
  {
    size_t spaceAvailable = additionalNmeaBufferSize - strlen(additionalNmeaBuffer);
    if (spaceAvailable >= 1)
      spaceAvailable -= 1; // Leave room for the NULL
    uint16_t fileBufferAvailable = myGNSS.fileBufferAvailable(); // Check how much data is available
    while (spaceAvailable < fileBufferAvailable) // If the buffer is full, delete the oldest message(s)
    {
      const char *lfPtr = strstr(additionalNmeaBuffer, "\n"); // Find the first LF
      if (lfPtr == nullptr)
        break; // Something has gone badly wrong...
      lfPtr++; // Point at the byte after the LF
      size_t oldLen = lfPtr - additionalNmeaBuffer; // This much data is old
      size_t newLen = strlen(additionalNmeaBuffer) - oldLen; // This much is new (not old)
      for (size_t i = 0; i <= newLen; i++) // Move the new data over the old. Include the NULL
        additionalNmeaBuffer[i] = additionalNmeaBuffer[oldLen + i];
      spaceAvailable += oldLen;
    }
    size_t dataLen = strlen(additionalNmeaBuffer);
    myGNSS.extractFileBufferData((uint8_t *)&additionalNmeaBuffer[dataLen], fileBufferAvailable); // Add the new NMEA data
    dataLen += fileBufferAvailable;
    additionalNmeaBuffer[dataLen] = 0; // NULL terminate
  }

  // ==============================================================================================================
  // Check if the iAP2 SDP record has been created
  // If it has, restart the SPP Server

  if (sdpCreateRecordEvent)
  {
    sdpCreateRecordEvent = false;
    esp_spp_stop_srv();
    esp_spp_start_srv(ESP_SPP_SEC_NONE, ESP_SPP_ROLE_SLAVE, rfcommChanneliAP2, "ESP32SPP2");
  }

  // ==============================================================================================================
  // Check for a new device connection

  if (SerialBT.aclConnected() == true)
  {
    Serial.println("Apple Device found. Waiting for connection...");

    unsigned long connectStart = millis();
    while ((millis() - connectStart) < 5000)
    {
      if (SerialBT.connected())
      {
        Serial.println("Device connected. Sending handshake...");
        appleAccessory.startHandshake(&SerialBT);
        break;
      }
      delay(10);
    }
  }

  // ==============================================================================================================
  // If the user hits 'r' then restart
  if (Serial.available())
  {
    byte incoming = Serial.read();

    if (incoming == 'r')
      ESP.restart();
  }
}

