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

#include <mutex>

// POWER SAVE ATTEMPTS
//#include <WiFi.h>

// Methods
uint64_t StrToHex(const char* str);

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

// 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   24
#define LED_SETUP_DISPLAY_DELAY 500

#define LED_PALETTE_SIZE 16

// VIBRATION MOTOR configuration
#define VIBRO_PIN 2
#define VIBRO_REPEAT 3
#define VIBRO_SMALL_DELAY 100
#define VIBRO_BIG_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!)
// If you are using more then 16, you have to use CRGBPalette256 instead of CRGBPalette16
// It probably makes sense to align this with LED_NUM_LEDS
#define MAX_MONITOR_DEVICES 7
// Minimum transmit power the device needs to be recognice (this defines the min proximity needed for the device to be used)
// TODO: add a table here that defines distance in meters for scenarios where the device is in plain sight
#define MIN_RSSI_POWER -50

// CONSTS
const std::map<String, String> overrideColors = {
    {"36E898", "FF0000"}, // MiiBand Klaas
};

const std::map<String, void*> blackList = {
  //{ "36E898", nullptr },
};

#define DEVICE_TYPE_PUBLIC_LAMP 1
#define DEVICE_TYPE_PRIVATE_LAMP 2
#define DEVICE_TYPE_AMULETT 3

#define DEVICE_TYPE DEVICE_TYPE_PUBLIC_LAMP

#ifndef DEVICE_TYPE
  // WE DO NOT HAVE A DEVICETYPE, so we do not have any variables here
  // to make sure the code compile fails
  #error "DEVICE_TYPE is not defined"

#elif DEVICE_TYPE == DEVICE_TYPE_PUBLIC_LAMP
  // This is a PUBLIC LAMP, no restrictions here
  // Does not have a vibration motor
  static const bool useVibro = false;
  // WARNING: IF THIS LIST CONTAINS > 0 VALUES, NO OTHER DEVICES WILL BE ALLOWED
  //          IF THIS LIST IS EMPTY IT WILL NOT BE USED
  static const std::map<String, void*> whiteList = {};

#elif DEVICE_TYPE == DEVICE_TYPE_PRIVATE_LAMP
  // This is a PRIVATE LAMP, restricted to a list of users
  // Does not have a vibration motor
  static const bool useVibro = false;
  // WARNING: IF THIS LIST CONTAINS > 0 VALUES, NO OTHER DEVICES WILL BE ALLOWED
  //          IF THIS LIST IS EMPTY IT WILL NOT BE USED
  static const std::map<String, void*> whiteList = {
    { "36E898", nullptr },
  };

#elif DEVICE_TYPE == DEVICE_TYPE_AMULETT
  // This is an portable device, restricted to a list of users
  // Comes with a vibration motor to alert wearer
  static const bool useVibro = true;
  // WARNING: IF THIS LIST CONTAINS > 0 VALUES, NO OTHER DEVICES WILL BE ALLOWED
  //          IF THIS LIST IS EMPTY IT WILL NOT BE USED  
  static const std::map<String, void*> whiteList = {
    { "36E898", nullptr },
  };
  
#else
  #error "Unknown DEVICE_TYPE"
#endif

// VARIABLES
BLEScan *pBLEScan;

CRGB leds[LED_NUM_LEDS];
CRGBPalette16 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) {
      return;
    }

    String addr = advertisedDevice.getAddress().toString();
    String color = String(addr.substring(9,11)+addr.substring(12,14)+addr.substring(15,17));
    color.toUpperCase();

    // check if whitelist applies and device is allowed
    {
      if (whiteList.size() > 0) {
        auto it = whiteList.find(color);
        if (it == whiteList.end()) {
          return;
        }
      }
    }

    // check if device is blacklisted
    {
      auto it = blackList.find(color);
      if (it != blackList.end()) {
        return;
      }
    }
    
    // check if an override color for the found device exists
    {
      auto it = overrideColors.find(color);
      if (it != overrideColors.end() && !it->second.isEmpty()) {
        color = it->second;
      }
    }

    // when the device is not in the database yet, vibrate on first connect
    if (useVibro) {
      auto it = deviceDatabase.find(color);
      if (it != deviceDatabase.end()) {
        Vibrate();
      }
    }

    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);
  
  if (useVibro) {
    pinMode(VIBRO_PIN, OUTPUT);
  }

  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

  // POWER SAVING!
  //WiFi.disconnect(true);  // Disconnect from the network
  //WiFi.mode(WIFI_OFF);
}

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++) % LED_PALETTE_SIZE);
  FastLED.show();
  FastLED.delay(1000/LED_FRAMES_PER_SECOND); 
}

// 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() || deviceDatabase.size() < 1) {
    // 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];
    }
}

// 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);
  std::vector<String> colors;

  int numColors = deviceDatabase.size();
  int pos = 0 ;
  for (auto itr: deviceDatabase) {
    colors.push_back(itr.first);
  }

  if (colors.empty()) {
    fill_solid(pal.entries, LED_PALETTE_SIZE, CRGB::Black);
  } else {
    for (int i=0; i<LED_PALETTE_SIZE; i++) {
      pal[i] = CRGB(StrToHex(colors[ i % deviceDatabase.size() ].c_str()));
    }
  }
}

// Causes the vibration motor to vibrate
void Vibrate() {
  for (int i = 0; i < VIBRO_REPEAT; i++) {
    for (int j = 0; j < 3; j++) {
      delay(VIBRO_SMALL_DELAY);
      digitalWrite(VIBRO_PIN, HIGH);
      delay(VIBRO_SMALL_DELAY);
      digitalWrite(VIBRO_PIN, LOW);
    }
    delay(VIBRO_BIG_DELAY);
  }
}