#include "MicroBit.h"
#include "ble/DiscoveredCharacteristic.h"
#include "ble/DiscoveredService.h"

uint8_t counter;
MicroBit uBit;

void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) 
{

  if (params->advertisingDataLen == 31)
  {
    //len=31
    //data=02011A03036FFD17166FFD86BFAF5ACE16AB0C809B47092C07E20DF6017412
    //     ^ ^ ^ ^ ^ ^   ^ ^ ^   ^                               ^
    //     | | | | | |   | | |   |                               +-- Associated Encrypted Metadata
    //     | | | | | |   | | |   +-- Rolling Proximity Identifier
    //     | | | | | |   | | +------ Service - 0xFD6F
    //     | | | | | |   | +-------- Type    - 0x16
    //     | | | | | |   +---------- Length  - 0x17
    //     | | | | | +-------------- Service - 0xFD6F
    if (params->advertisingData[0] == 0x02 &&
        params->advertisingData[1] == 0x01 &&
        params->advertisingData[2] == 0x1a &&
        params->advertisingData[3] == 0x03 &&
        params->advertisingData[4] == 0x03 &&
        params->advertisingData[5] == 0x6f &&
        params->advertisingData[6] == 0xfd
        ) {
      int x;
      int y;
      x = (counter) % 5;
      y = (counter) / 5;
      uBit.display.image.setPixelValue(x, y, 0);
      counter++;
      counter = counter % 25;
      x = counter % 5;
      y = counter / 5;
      char id[33];
      int8_t rssi = params->rssi;
      uBit.display.image.setPixelValue(x, y, (uint8_t)rssi);
      BLEProtocol::AddressBytes_t peer;
      memcpy(peer, params->peerAddr, BLEProtocol::ADDR_LEN);

      for (int i = 0; i < 16; i++)
      {
        sprintf(&id[i * 2], "%02X", params->advertisingData[i + 11]);
      }
      uBit.serial.printf("id=%s - %i (%02x:%02x:%02x:%02x:%02x:%02x)\r\n", 
          id, (uint8_t)rssi, peer[5], peer[4], peer[3], peer[2], peer[1], peer[0]);
    }
  }
}

int main() 
{
  counter = 0;
  uBit.display.setDisplayMode(DISPLAY_MODE_GREYSCALE);
  scheduler_init(uBit.messageBus);
  uBit.serial.printf("Scanner.....\r\n");

  uBit.ble = new BLEDevice();
  uBit.ble->init();
  uBit.ble->gap().setScanParams(500, 400);
  uBit.ble->gap().startScan(advertisementCallback);

  while (true) {
    uBit.ble->waitForEvent();
  }
  return 0;
}