// central_bleuart_bridge.ino Rev 1.3
// 4nd Dec 2017
// V1.3 Added Blue Led disable 
/*
   Based on Adafruit example central_bleuart_multi.ino
   modifications (c)2014-2017 Forward Computing and Control Pty. Ltd.
   NSW Australia, www.forward.com.au
   This code is not warranted to be fit for any purpose. You may only use it at your own risk.
   This code may be freely used for both private and commercial use
   Provide this copyright is maintained.
*/

/*********************************************************************
  This is an example for our nRF52 based Bluefruit LE modules

  Pick one up today in the adafruit shop!

  Adafruit invests time and resources providing this open source code,
  please support Adafruit and open-source hardware by purchasing
  products from Adafruit!

  MIT license, check LICENSE for more information
  All text above, and the splash screen below must be included in
  any redistribution
*********************************************************************/

/* This sketch bridges Serial to one periphal BLE device that implements Nordic UART Service with RX Notify
    whose MAC address matches VALID_ADDRESS
    It auto-recoonects if disconnected
*/

/* LED PATTERNS
   ------------
   LED_RED   - Blinks pattern once for each connection.
   LED_BLUE  - Constant when connected else blinks, unless disabled via #define TURN_BLUE_LED_OFF
*/

// Uncomment next define to get debug output
//#define DEBUG

// Uncomment next define to Connected to ... and Disconnected from ... messages
//#define CONNECT_MSGS

// Uncomment next define to disable Blue Led
//#define TURN_BLUE_LED_OFF

// Uncomment the next define to ECHO the Serial input back on the Serial output when it is sent to the BLE periphal device
//#define ECHO

#include <bluefruit.h>

// These next three lines of code come from the central_bleuart_scanner.ino output
const char *BLE_NAME = "GENUINO 101-FC8F";
const char *BLE_ADDRESS = "98:4F:EE:0C:FC:8F";
const uint8_t BLE_ADDRESS_TYPE = 0;

enum ConnectError {DID_NOT_CONNECT, NONE, NoUART, NoNotify};
volatile ConnectError connectionError = DID_NOT_CONNECT;

// Struct containing peripheral info
const size_t NAME_SIZE = 33;
typedef struct {
  uint8_t addr[BLE_GAP_ADDR_LEN];  //addr[6]
  uint16_t conn_handle;
  // Each prph need its own bleuart client service
  BLEClientUart bleuart;
  char name[NAME_SIZE];
} prph_info_t;

const size_t BLE_NAME_LEN = 33; // including null
// Struct containing list of devices to ignore in scan
typedef struct {
  uint8_t bleName[BLE_NAME_LEN];
  bool foundName; // resolved a non blank name
  bool connected; // set to true if we have connected to this device
  uint8_t addr_type;
  uint8_t addr[BLE_GAP_ADDR_LEN];  //addr[6]
} scan_add_ignore_t;

scan_add_ignore_t targetDevice; // initialized in setup
bool volatile hasConnected = false;

// return false if conversion  fails.
bool bleMACStrToArray(const char* macStr, uint8_t addr[BLE_GAP_ADDR_LEN]) {
  int i = BLE_GAP_ADDR_LEN - 1;
  uint8_t num = 0; // start with 0
  int8_t multiplier = 16;
  while ((*macStr) != '\0') {
    char c = *macStr;
    if (c == ' ') {
      macStr++;
      continue; // skip blanks
    }

    if (i < 0) { // filled output array but still more non-blank chars
      return false; // invalid
    }

    // while not end of string
    if (c == ':') {
      // store num and move on to next position
      addr[i--] = num;
      num = 0;
      multiplier = 16;
    } else {
      if (multiplier < 0) {
        return false; // have more than 2 hex digits
      }
      if ((c >= '0') && (c <= '9')) {
        num = num + multiplier * (c - '0');
      } else if ((c >= 'A') && (c <= 'F')) {
        num = num + multiplier * (10 + (c - 'A'));
      } else if ((c >= 'a') && (c <= 'f')) {
        num = num + multiplier * (10 + (c - 'a'));
      }
      multiplier -= 15;
    }
    macStr++;
  }
  if (i == 0) {
    // store last num
    addr[i--] = num;;
  }
  if (i >= 0) {
    return false; // not enough
  }
  return true;
}


/* Peripheral info array (one per peripheral device)

   There are 'BLE_CENTRAL_MAX_CONN' central connections, but the
   the connection handle can be numerically larger (for example if
   the peripheral role is also used, such as connecting to a mobile
   device). As such, we need to convert connection handles <-> the array
   index where appropriate to prevent out of array accesses.

   Note: One can simply declares the array with BLE_MAX_CONN and use connection
   handle as index directly with the expense of SRAM.
*/
prph_info_t prphs[BLE_CENTRAL_MAX_CONN];

// Software Timer for blinking the RED LED
SoftwareTimer blinkTimer;
uint8_t connection_num = 0; // for blink pattern

bool checkEqualAddress(ble_gap_addr_t peerAddr, scan_add_ignore_t ignoreAddr) {
  for (int i = 0; i < BLE_GAP_ADDR_LEN; i++) {
    if (peerAddr.addr[i] != ignoreAddr.addr[i]) {
      return false;
    }
  }
  // also check type
  if (peerAddr.addr_type != ignoreAddr.addr_type) {
    return false;
  }
  return true; // valid
}

void printAddress(uint8_t *addr) {
  for (int i = BLE_GAP_ADDR_LEN - 1; i >= 0; i--) {
    if (addr[i] < 16) {
      Serial.print('0');
    }
    Serial.print(addr[i], HEX);
    if (i > 0) {
      Serial.print(':');
    }
  }
}

// clean up name
// replace \r with space, and \n with space
char* cleanUpName(uint8_t *name) {
  uint8_t *str = name;
  while (*str != '\0') {
    if ((*str == '\r') || (*str == '\n')) {
      *str = ' ';
    }
    str++;
  }
  return (char*)name;
}

char* cleanUpName(char* name) {
  return cleanUpName((uint8_t *) name);
}

void setup() {
#ifdef TURN_BLUE_LED_OFF
  Bluefruit.autoConnLed(false); // default is Bluefruit.autoConnLed(true);
#endif
  Serial.begin(9600);
  // delay startup a few seconds to give time to open the Monitor
  for (int i = 5; i > 0; i--) {
    delay(1000);
  }

  // Initialize blinkTimer for 200 ms and start it
  blinkTimer.begin(200, blink_timer_callback);
  blinkTimer.start();

  // set up targetDevice info
  if (!bleMACStrToArray(BLE_ADDRESS, targetDevice.addr)) {
#ifdef DEBUG
    Serial.print("BLE_ADDRESS ");
    Serial.print(BLE_ADDRESS);
    Serial.println(" is not a valid BLE mac address.");
#endif
    while (1) {
      delay(1000);
    }
  }
  targetDevice.addr_type = BLE_ADDRESS_TYPE;
  targetDevice.foundName = true;
  targetDevice.connected = false;
  memset(targetDevice.bleName, 0, BLE_NAME_LEN);
  strncpy((char*)targetDevice.bleName, BLE_NAME, BLE_NAME_LEN - 1); // leave a null on the end
  cleanUpName(targetDevice.bleName);


#ifdef DEBUG
  Serial.print("BLE UART Bridge connecting to ");
  printAddress(targetDevice.addr);
  Serial.print("  "); Serial.println((char*)targetDevice.bleName);
  Serial.println("-----------------------------------------\n");
#endif

  // Enable both peripheral and central
  Bluefruit.begin(true, true);
  Bluefruit.setName("BLE Bridge Central");


  // Init peripheral pool
  for (uint8_t idx = 0; idx < BLE_CENTRAL_MAX_CONN; idx++) { // BLE_CENTRAL_MAX_CONN == 4 so max 4 BLE devices connected to this one.
    // Invalid all connection handle
    prphs[idx].conn_handle = BLE_CONN_HANDLE_INVALID;
    memset(prphs[idx].name, 0, NAME_SIZE); // set to nulls
    // All of BLE Central Uart Serivce
    prphs[idx].bleuart.begin();
    prphs[idx].bleuart.setRxCallback(bleuart_rx_callback);
  }

  // Callbacks for Central
  Bluefruit.Central.setConnectCallback(connect_callback);
  Bluefruit.Central.setDisconnectCallback(disconnect_callback);

  connection_num = 0;
  hasConnected = false;
  /* Start Central Scanning
     - Enable auto scan if disconnected
     - Interval = 100 ms, window = 80 ms
     - Don't use active scan (used to retrieve the optional scan response adv packet)
     - Start(0) = will scan forever since no timeout is given
  */
  Bluefruit.Scanner.setRxCallback(scan_callback);
  Bluefruit.Scanner.restartOnDisconnect(true);
  Bluefruit.Scanner.setInterval(160, 80);       // in units of 0.625 ms
  Bluefruit.Scanner.useActiveScan(false);       // Don't request scan response data
  Bluefruit.Scanner.start(0);                   // 0 = Don't stop scanning after n seconds
}

/**
   Callback invoked when scanner picks up an advertising packet
   @param report Structural advertising data
   This method used to check if advertising data contains the BleUart service UUID
   but now skip this check as some BLE devices do not provide this info in the advertising packet
*/
void scan_callback(ble_gap_evt_adv_report_t* report) {
  // check ignore devices
  if (!checkEqualAddress(report->peer_addr, targetDevice)) {
#ifdef DEBUG
    //Serial.print(" Ignoring "); printAddress(report->peer_addr.addr); Serial.println();
#endif
    return;
  }


#ifdef DEBUG_REPORT
  Serial.println("report");
  //  ble_gap_addr_t peer_addr;                     /**< Bluetooth address of the peer device. */
  //  int8_t         rssi;                          /**< Received Signal Strength Indication in dBm. */
  //  uint8_t        scan_rsp : 1;                  /**< If 1, the report corresponds to a scan response and the type field may be ignored. */
  //  uint8_t        type     : 2;                  /**< See @ref BLE_GAP_ADV_TYPES. Only valid if the scan_rsp field is 0. */
  //  uint8_t        dlen     : 5;                  /**< Advertising or scan response data length. */
  //  uint8_t        data[BLE_GAP_ADV_MAX_SIZE];
  Serial.println(" peer_addr");
  Serial.print("  addr_type: "); Serial.println(report->peer_addr.addr_type);                    /**< See @ref BLE_GAP_ADDR_TYPES. */
  Serial.print("  addr: "); printAddress(report->peer_addr.addr); Serial.println();
  //for(int i=0; i<BLE_GAP_ADDR_LEN;i++) { Serial.print(report->peer_addr.addr[i]);Serial.print(' ');}    /**< 48-bit address, LSB format. */
  Serial.print(" rssi: "); Serial.println(report->rssi);
  Serial.print(" scan_rsp "); Serial.println(report->scan_rsp);
  Serial.print(" type "); Serial.println(report->type);
  Serial.print(" dlen "); Serial.println(report->dlen);
  Serial.println(" data");
  for (int i = 0; i < report->dlen; i++) {
    if (i >= BLE_GAP_ADV_MAX_SIZE) {
      break;
    }
    Serial.print(report->data[i], HEX);
    Serial.print(' ');
  }
  Serial.println();
#endif
  Bluefruit.Central.connect(report); // always connect and check then
}

/**
   Callback invoked when an connection is established
   @param conn_handle
*/
void connect_callback(uint16_t conn_handle) {
  connectionError = NONE;

  // Find an available ID to use
  int id  = findConnHandle(BLE_CONN_HANDLE_INVALID);
  // Eeek: Exceeded the number of connections !!!
  if ( id < 0 ) {
#ifdef DEBUG
    Serial.println(" Exceeded the number of connections !!!");
#endif
    return;
  }

  prph_info_t* peer = &prphs[id];
  peer->conn_handle = conn_handle;

  Bluefruit.Gap.getPeerName(conn_handle, peer->name, 32);
  Bluefruit.Gap.getPeerAddr(conn_handle, peer->addr);
#if defined(CONNECT_MSGS) || defined(DEBUG)
  Serial.print("Connected to ");
  //Serial.print(cleanUpName(peer->name));
  Serial.print(" Addr ");
  printAddress(peer->addr);
  Serial.print("  "); Serial.print((char*)targetDevice.bleName);
  Serial.println();
#endif
#ifdef DEBUG
  Serial.print("Discovering BLE Nordic UART service ... ");
#endif
  if ( peer->bleuart.discover(conn_handle) )  {
#ifdef DEBUG
    Serial.println("Found it");
    Serial.println("Enabling TXD characteristic's CCCD notify bit");
#endif
    if (peer->bleuart.enableTXD()) {
      connection_num++;
      hasConnected = true;
    } else {
      connectionError = NoNotify;
#ifdef DEBUG
      Serial.println("TX Notify enable failed!!");
      Serial.println(" Set Notify for TX characteristic, 6E400003-B5A3-F393-E0A9-E50E24DCCA9E, in BLE device code");
#endif
      // disconect since we couldn't find bleuart service
      Bluefruit.Central.disconnect(conn_handle);
    }
  } else {
    connectionError = NoUART;
#ifdef DEBUG
    Serial.println("No Nordic UART Service Found!");
#endif
    // disconect since we couldn't find bleuart service
    Bluefruit.Central.disconnect(conn_handle);
  }
}

/**
   Callback invoked when a connection is dropped
   @param conn_handle
   @param reason
*/
void disconnect_callback(uint16_t conn_handle, uint8_t reason) {
  (void) conn_handle;
  (void) reason;
  if (connection_num > 0) {
    connection_num--;
  }
  hasConnected = false;

  // Mark the ID as invalid
  int id  = findConnHandle(conn_handle);

  // Non-existant connection, something went wrong, DBG !!!
  if ( id < 0 ) {
    return;
  }

  prph_info_t* peer = &prphs[id];
#if defined(CONNECT_MSGS) || defined(DEBUG)
  Serial.print("Disconnected from ");
  //Serial.print(cleanUpName(peer->name));
  Serial.print(" Addr ");
  printAddress(peer->addr);
  Serial.print("  "); Serial.print((char*)targetDevice.bleName);
  if (connectionError == NoUART) {
    Serial.print("  No Nordic UART Service! (Try moving close to target device)");
  } else if (connectionError == NoNotify) {
    Serial.print("  TX Notify enable failed!!");
  }
  Serial.println();
#endif
  memset(peer->name, 0, NAME_SIZE); // set to nulls
  // Mark conn handle as invalid
  peer->conn_handle = BLE_CONN_HANDLE_INVALID;
}

/**
   Callback invoked when BLE UART data is received
   @param uart_svc Reference object to the service where the data
   arrived.
*/
void bleuart_rx_callback(BLEClientUart & uart_svc) {
  // uart_svc is prphs[conn_handle].bleuart
  uint16_t conn_handle = uart_svc.connHandle();

  int id = findConnHandle(conn_handle);
  prph_info_t* peer = &prphs[id];

  // Print sender's name
#ifdef DEBUG
  Serial.printf("[From %s]: ", cleanUpName(peer->name));
#endif
  // Read then forward to all peripherals
  while ( uart_svc.available() ) {
    // default MTU with an extra byte for string terminator
    char buf[20 + 1] = { 0 };

    if ( uart_svc.read(buf, sizeof(buf) - 1) ) {
      Serial.print(buf);
    }
  }
#ifdef DEBUG
  Serial.println();
#endif
}

/**
   Helper function to send a string to all connected peripherals
*/
void sendAll(const char* str) {
#if defined(ECHO) || defined(DEBUG)
  Serial.print("[Send]: ");
  Serial.println(str);
#endif
  for (uint8_t id = 0; id < BLE_CENTRAL_MAX_CONN; id++)   {
    prph_info_t* peer = &prphs[id];
    if (peer->conn_handle == BLE_CONN_HANDLE_INVALID) {
      continue; // skip this one
    } // else
    if (peer->bleuart.discovered() )     {
      peer->bleuart.print(str);
    }
  }
}

void loop() {
  // default MTU with an extra byte for string terminator
  char buf[20 + 1] = { 0 };

  // Read from HW Serial (normally USB Serial) and send to all peripherals
  // add some buffering here
  if ( Serial.readBytes(buf, sizeof(buf) - 1) )     {
    // Serial.print(buf); // echo
    sendAll(buf);
  }
}

/**
   Find the connection handle in the peripheral array
   @param conn_handle Connection handle
   @return array index if found, otherwise -1
*/
int findConnHandle(uint16_t conn_handle) {
  for (int id = 0; id < BLE_CENTRAL_MAX_CONN; id++)   {
    if (conn_handle == prphs[id].conn_handle)     {
      return id;
    }
  }
  return -1;
}

/**
   Software Timer callback is invoked via a built-in FreeRTOS thread with
   minimal stack size. Therefore it should be as simple as possible. If
   a periodically heavy task is needed, please use Scheduler.startLoop() to
   create a dedicated task for it.

   More information http://www.freertos.org/RTOS-software-timer.html
*/
void blink_timer_callback(TimerHandle_t xTimerID) {
  (void) xTimerID;

  // Period of sequence is 10 times (1 second).
  // RED LED will toggle first 2*n times (on/off) and remain off for the rest of period
  // Where n = number of connection
  static uint8_t count = 0;

  if (connection_num == 0) {
    count = 0;
    digitalWrite(LED_RED, LOW);
    return; // zero count on each reset
  }
  if ( count < 2 * connection_num) {
    digitalToggle(LED_RED);
  }
  count++;
  if (count >= 10) {
    count = 0;
  }
}