#include <Arduino.h>

#include <BLEDevice.h>
#include <BLEScan.h>

#include <FastLED.h>

#include <mutex>

#define BLE_ACTIVE_SCAN false
#define BLE_SCAN_INTERVAL 100
#define BLE_WINDOW 99  // less or equal setInterval value

#define NUM_LEDS 7
#define DATA_PIN 1

#define BRIGHTNESS          96
#define FRAMES_PER_SECOND   24

#define MAX_VANISH_COUNTER  50
#define MAX_MONITOR_DEVICES 10

CRGB leds[NUM_LEDS];

int scanTime = 1;  //In seconds

BLEScan *pBLEScan;

std::map<String,uint8_t> deviceDatabase;

std::mutex callBackMutex;

CRGBPalette16 MyPalette;

class AdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    String addr = advertisedDevice.getAddress().toString();
    String color = String("0x"+addr.substring(9,11)+addr.substring(12,14)+addr.substring(15,17));
    color.toUpperCase();
    color[1] = 'x';
    if (deviceDatabase.contains(color) || deviceDatabase.size() + 1 < MAX_MONITOR_DEVICES) {
      deviceDatabase[color] = MAX_VANISH_COUNTER;
    }
    Serial.printf("Address: %s RSSI: %d TX Power: %d Calculated color: %s \n", addr.c_str(), advertisedDevice.getRSSI(), advertisedDevice.getTXPower(), color.c_str());
    for(int i=0; i<NUM_LEDS;i++) {
      leds[i] = strtol(color.c_str(), NULL, 0);
    }
    std::vector<String> keys;
    for (auto &itr : deviceDatabase) {
      if (color != itr.first) {
        itr.second--;
      }
      if(itr.second == 0) {
        keys.push_back(itr.first);
      }
    }
    for (const auto &itr : keys) {
      deviceDatabase.erase(itr);
      Serial.printf("Have not seen %s for %d ticks, removing!\n", itr, MAX_VANISH_COUNTER);
    }
    Serial.printf("Devicedatabase size: %d\n", deviceDatabase.size());
  }
};

void setup() {
  FastLED.delay(1000/FRAMES_PER_SECOND); 
  FastLED.addLeds<NEOPIXEL,DATA_PIN>(leds, NUM_LEDS);
  for(int i=0; i<NUM_LEDS;i++) {
    leds[i] = CRGB::Red;
  }
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND); 
  for(int i=0; i<NUM_LEDS;i++) {
    leds[i] = CRGB::Blue;
  }
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND); 
  for(int i=0; i<NUM_LEDS;i++) {
    leds[i] = CRGB::Green;
  }
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND); 
  for(int i=0; i<NUM_LEDS;i++) {
    leds[i] = CRGB::Black;
  }
  FastLED.setBrightness(BRIGHTNESS);
  FastLED.show();
    
  
  Serial.begin(115200);
  Serial.println("Scanning...");

  BLEDevice::init("");
  pBLEScan = BLEDevice::getScan();  //create new scan
  pBLEScan->setAdvertisedDeviceCallbacks(new AdvertisedDeviceCallbacks());
  pBLEScan->setActiveScan(BLE_ACTIVE_SCAN);  //active scan uses more power, but get results faster
  pBLEScan->setInterval(BLE_SCAN_INTERVAL);
  pBLEScan->setWindow(BLE_WINDOW);  // less or equal setInterval value
}

void loop() {
  {
    std::lock_guard<std::mutex> lck(callBackMutex);

    BLEScanResults *foundDevices = pBLEScan->start(scanTime, false);
    pBLEScan->clearResults();  // delete results fromBLEScan buffer to release memory
  }
  FastLED.show();
  FastLED.delay(1000/FRAMES_PER_SECOND); 
}