#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 100

CRGB leds[NUM_LEDS];

CRGBPalette256 pal;

int scanTime = 1;  //In seconds

BLEScan *pBLEScan;

std::map<String,uint8_t> deviceDatabase;

std::mutex callBackMutex;
std::mutex blendMutex;

volatile uint8_t startindex;
volatile uint8_t stepSegments; 

void updatePalette();
uint64_t StrToHex(const char* str);
CRGB blendCRGB(CRGB a, CRGB b, uint8_t blendAmount);
void FillLEDsFromPaletteColors( uint8_t colorIndex);

class AdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    // TODO: switching of does not work because this function is only triggered when new devices are detected, ergo it will always be stuck with the last device
    // the decrementing in line 58 needs to be moved to a seperate timebased function
    String addr = advertisedDevice.getAddress().toString();
    String color = String(addr.substring(9,11)+addr.substring(12,14)+addr.substring(15,17));
    color.toUpperCase();
    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());
    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());
    updatePalette();
  }
};

void setup() {
  updatePalette();
  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...");

  startindex = 0;

  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> lock(callBackMutex);

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

uint64_t StrToHex(const char* str)
{
  return (uint64_t) strtoull(str, 0, 16);
}

void FillLEDsFromPaletteColors( uint8_t colorIndex)
{
    uint8_t brightness = 255;
    
    for( int i = 0; i < NUM_LEDS; ++i) {
        leds[i] = ColorFromPalette( pal, colorIndex, brightness, LINEARBLEND);
        colorIndex += stepSegments;
    }
}

// Blends two CRGB colors using a uint8_t blend amount (0-255)
CRGB blendCRGB(CRGB a, CRGB b, uint8_t blendAmount) {
  CRGB result;
  result.r = lerp8by8(a.r, b.r, blendAmount);
  result.g = lerp8by8(a.g, b.g, blendAmount);
  result.b = lerp8by8(a.b, b.b, blendAmount);
  return result;
}

void updatePalette() {
  std::lock_guard<std::mutex> lock(blendMutex);
  uint8_t numColors = 2;
  String colors[MAX_MONITOR_DEVICES];
  switch (deviceDatabase.size()) {
    case 0:
      numColors = 2;
      colors[0] = "000000";
      colors[1] = "000000";
    case 1:
      numColors = 2;
        colors[0] = deviceDatabase.begin()->first.substring(2,8);
        colors[1] = deviceDatabase.begin()->first.substring(2,8);
      break;
    default:
        numColors = deviceDatabase.size();
        int pos = 0 ;
        for (auto itr: deviceDatabase) {
          colors[pos++] = itr.first.substring(2,8);
        }
      break;
  }
  uint8_t segmentSize = 255 / (numColors - 1); // Divide the palette into segments

  if(numColors < 1) {
    Serial.println("Less than 2 colors? Something is fucky?");
  }
  for (uint8_t i = 0; i < numColors - 1; i++) {
    CRGB startColor =  CRGB(StrToHex(colors[i].c_str()));
    CRGB endColor = CRGB(StrToHex(colors[i + 1].c_str()));

        for (uint8_t j = 0; j < segmentSize; j++) {
      uint16_t index = i * segmentSize + j;
      if (index >= 256) break;

      uint8_t blendAmount = (j * 255) / segmentSize;  // Integer blend factor
      pal[index] = blendCRGB(startColor, endColor, blendAmount);

      if (index % 16 == 0) yield();  // Prevent watchdog reset
    }
  }
  stepSegments = segmentSize;

  // Make sure the last color slot is correctly set
  pal[255] = CRGB(StrToHex(colors[numColors - 1].c_str()));
}