#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEScan.h>
#include <FastLED.h>
#include <TimerEvent.h>

#include <mutex>

// Methods
CRGB BlendCRGB(CRGB a, CRGB b, uint8_t blendAmount);

uint64_t StrToHex(const char* str);

void CleanDatabase();
void FillLEDsFromPaletteColors( uint8_t colorIndex);
void UpdatePalette();


// Bluetooth configuration
#define BLE_ACTIVE_SCAN false
#define BLE_SCAN_INTERVAL 100
#define BLE_SCAN_TIME 1 // in seconds
#define BLE_WINDOW 99  // less or equal setInterval value

// LED configuration
#define LED_TYPE    WS2812B
#define LED_DATA_PIN 1
#define LED_NUM_LEDS 7
#define LED_COLOR_ORDER RGB

#define LED_BRIGHTNESS          96
#define LED_FRAMES_PER_SECOND   144
#define LED_SETUP_DISPLAY_DELAY 500

// DATA maintenance configuration
#define UPDATE_INTERVAL 10000 // how often do we update de database?

#define MAX_VANISH_COUNTER  3 // if updateinterval is 10000 => 30s
// Number of devices that can be monitored (might cause a reset because of memory oversaturation!)
#define MAX_MONITOR_DEVICES 5
// Minimum transmit power the device needs to be recognice (this defines the min proximity needed for the device to be used)
#define MIN_RSSI_POWER -50



// VARIABLES
BLEScan *pBLEScan;

CRGB leds[LED_NUM_LEDS];
CRGBPalette256 pal;

std::map<String,uint8_t> deviceDatabase;

std::mutex databaseMutex;
std::mutex blendMutex;

volatile uint8_t startindex;
volatile uint8_t stepSegments; 

TimerEvent updateTimer;

class AdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    if (advertisedDevice.getRSSI() < MIN_RSSI_POWER) {
      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());
      UpdatePalette();
    }
  }
};

void setup() {
  Serial.begin(115200);
  
  UpdatePalette();

  FastLED.setBrightness(LED_BRIGHTNESS);

  FastLED.addLeds<LED_TYPE,LED_DATA_PIN,LED_COLOR_ORDER>(leds, LED_NUM_LEDS).setCorrection(TypicalLEDStrip);

  fill_solid(leds, LED_NUM_LEDS, CRGB::Red );
  FastLED.show();
  FastLED.delay(LED_SETUP_DISPLAY_DELAY); 

  fill_solid(leds, LED_NUM_LEDS, CRGB::Green );
  FastLED.show();
  FastLED.delay(LED_SETUP_DISPLAY_DELAY); 

  fill_solid(leds, LED_NUM_LEDS, CRGB::Blue );
  FastLED.show();
  FastLED.delay(LED_SETUP_DISPLAY_DELAY); 

  fill_solid(leds, LED_NUM_LEDS, CRGB::Black );
  FastLED.show();    

  startindex = 0;
  updateTimer.set(UPDATE_INTERVAL, CleanDatabase);

  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() {
  updateTimer.update();
  {
    std::lock_guard<std::mutex> lock(databaseMutex);

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

// 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;
}

// Converts a string to a 64bit HEX value
uint64_t StrToHex(const char* str) {
  return (uint64_t) strtoull(str, 0, 16);
}

// Cleans the database, updates VANISH_TICKERS
void CleanDatabase() {
  std::lock_guard<std::mutex> lock(databaseMutex);
  std::vector<String> keys;
  for (auto &itr : deviceDatabase) {
    Serial.printf("Decrement: %s -> %d\n", itr.first, itr.second);
    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);
  }
  if (!keys.empty()) {
    // there are changes to the database, we need to update the palette
    UpdatePalette();
  }
}

// Renderes Colors from the calculated palette to the led strip
void FillLEDsFromPaletteColors( uint8_t colorIndex) {    
    for( int i = 0; i < LED_NUM_LEDS; ++i) {
      leds[i] = pal[i+colorIndex];
    }
}

// Renderes Colors from the calculated palette to the led strip
void FillLEDsFromPaletteColorsOld( uint8_t colorIndex) {
    for( int i = 0; i < LED_NUM_LEDS; ++i) {
      leds[i] = ColorFromPalette( pal, colorIndex, LED_BRIGHTNESS, LINEARBLEND);
      colorIndex += stepSegments;
    }
}

// Updates the palette using module to create a repeated fillpatern of all detected devices using the lower 24bits of the btle address
void UpdatePalette() {
  std::lock_guard<std::mutex> lock(blendMutex);
  String colors[MAX_MONITOR_DEVICES];
  int numColors = deviceDatabase.size();
  int pos = 0 ;
  for (auto itr: deviceDatabase) {
    colors[pos++] = itr.first;
  }
  if (deviceDatabase.size() > 0 ) {
    for (int i=0; i<256; i++) {
      pal[i] = CRGB(StrToHex(colors[ i % deviceDatabase.size() ].c_str()));
    }
  }
}

// Updates the palette with gradients calculated from the lower 24 bits of the btle address
void UpdatePaletteOld() {
  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;
        colors[1] = deviceDatabase.begin()->first;
      break;
    default:
        numColors = deviceDatabase.size();
        int pos = 0 ;
        for (auto itr: deviceDatabase) {
          colors[pos++] = itr.first;
        }
      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()));
}