void BlueNRGGattClient::primaryServicesCB(Gap::Handle_t connectionHandle,
                                          uint8_t event_data_length,
                                          uint8_t attribute_data_length,
                                          uint8_t *attribute_data_list)
{
  GattAttribute::Handle_t startHandle, endHandle;
  UUID uuid;
  uint8_t i, offset, numAttr;
  /* avoid compiler warnings about unused variables */
  (void)connectionHandle;

  numAttr = (event_data_length - 1) / attribute_data_length;

  offset = 0;
  for (i=0; i<numAttr; i++) {
    startHandle = attribute_data_list[offset];
    endHandle = attribute_data_list[offset+2];

    // UUID Type
    if (attribute_data_length == 6) {
      
      PRINTF("UUID_TYPE_16\n\r");
      uuid = attribute_data_list[offset+5]<<8|attribute_data_list[offset+4];
      PRINTF("S UUID-%X attrs[%u %u]\r\n", uuid.getShortUUID(), startHandle, endHandle);
      
    } else {
      
      PRINTF("UUID_TYPE_128\n\r");
      uuid.setupLong(attribute_data_list+offset+4);
      
      PRINTF("S UUID-");
#ifdef DEBUG
      const uint8_t *longUUIDBytes = uuid.getBaseUUID();
      for (unsigned j = 0; j < UUID::LENGTH_OF_LONG_UUID; j++) {
        PRINTF("%02x", longUUIDBytes[j]);
      }
#endif
      PRINTF(" attrs[%u %u]\r\n", startHandle, endHandle);
      
    }
    
    PRINTF("Setup serviceIndex = %d\n\r", _numServices);
    discoveredService[_numServices].setup(uuid, startHandle, endHandle);
    
    if(serviceDiscoveryCallback) {
      if(_matchingServiceUUID == BLE_UUID_UNKNOWN || _matchingServiceUUID == discoveredService[_numServices].getUUID()) {
        serviceDiscoveryCallback(&discoveredService[_numServices]);
      }
    }
    _numServices++;

    offset += attribute_data_length;
  }

  PRINTF("!!!Service Discovery complete (numAttr=%u)!!!\n\r", numAttr);

}
void BlueNRGGattClient::serviceCharByUUIDCB(Gap::Handle_t connectionHandle,
                                            uint8_t event_data_length,
                                            uint16_t attr_handle,
                                            uint8_t *attr_value)
{
  // Charac Properties(1), Charac Value Handle(2), Charac UUID(2/16)
  GattAttribute::Handle_t declHandle, valueHandle;
  UUID uuid;
  
  PRINTF("serviceCharByUUIDCB\n\r");
  
  // UUID Type
  if (event_data_length == 7) {
    PRINTF("Char UUID_TYPE_16\n\r");
    uuid = attr_value[4]<<8|attr_value[3];
    PRINTF("C UUID-%X\r\n", uuid.getShortUUID());
  } else {
    PRINTF("Char UUID_TYPE_128\n\r");
    uuid.setupLong(attr_value+3);
    PRINTF("C UUID-");
#ifdef DEBUG
    const uint8_t *longUUIDBytes = uuid.getBaseUUID();
    for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
      PRINTF("%02X", longUUIDBytes[i]);
    }
    PRINTF("\r\n");
#endif
  }

  // Properties
  DiscoveredCharacteristic::Properties_t p;
  
  p._broadcast = (props_mask[0] & attr_value[0]);
  p._read = (props_mask[1] & attr_value[0])>>1;
  p._writeWoResp = (props_mask[2] & attr_value[0])>>2;
  p._write = (props_mask[3] & attr_value[0])>>3;
  p._notify = (props_mask[4] & attr_value[0])>>4;
  p._indicate = (props_mask[5] & attr_value[0])>>5;
  p._authSignedWrite = (props_mask[6] & attr_value[0])>>6;
  PRINTF("p._broadcast=%d\n\r", p._broadcast);
  PRINTF("p._read=%d\n\r", p._read);
  PRINTF("p._writeWoResp=%d\n\r", p._writeWoResp);
  PRINTF("p._write=%d\n\r", p._write);
  PRINTF("p._notify=%d\n\r", p._notify);
  PRINTF("p._indicate=%d\n\r", p._indicate);
  PRINTF("p._authSignedWrite=%d\n\r", p._authSignedWrite);

  /*
  uint8_t props = attr_value[0];
  PRINTF("CHAR PROPS: %d\n\r", props);
  */

  // Handles
  declHandle = attr_handle;
  valueHandle = attr_value[1];

  discoveredChar[_numChars].setup(this,
                                  connectionHandle,
                                  uuid,
                                  p,
                                  declHandle,
                                  valueHandle);
  
  if(characteristicDiscoveryCallback) {
    characteristicDiscoveryCallback(&discoveredChar[_numChars]);
  }
  _numChars++;
}