#include "esp32_presense.h"
#include "defaults.h"

namespace esphome
{
    namespace esp32_presense
    {
        unsigned int totalSeen = 0;
        unsigned int totalFpSeen = 0;
        unsigned int totalFpQueried = 0;
        unsigned int totalFpReported = 0;
        
        TimerHandle_t reconnectTimer;
        TaskHandle_t scanTaskHandle;

        unsigned long updateStartedMillis = 0;
        unsigned long lastTeleMillis = 0;
        int reconnectTries = 0;
        int teleFails = 0;
        int reportFailed = 0;
        bool online = false;         // Have we successfully sent status=online
        bool sentDiscovery = false;  // Have we successfully sent discovery
        UBaseType_t bleStack = 0;

        bool discovery, publishTele, publishRooms, publishDevices;

        class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks {
            void onResult(BLEAdvertisedDevice *advertisedDevice) {
                bleStack = uxTaskGetStackHighWaterMark(nullptr);
                BleFingerprintCollection::Seen(advertisedDevice);
            }
        };

        void scanTask(void *parameter) {
            NimBLEDevice::init("ESPresense");
            // Enrollment::Setup();
            NimBLEDevice::setMTU(23);
    
            auto pBLEScan = NimBLEDevice::getScan();
            pBLEScan->setInterval(BLE_SCAN_INTERVAL);
            pBLEScan->setWindow(BLE_SCAN_WINDOW);
            pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks(), true);
            pBLEScan->setActiveScan(false);
            pBLEScan->setDuplicateFilter(false);
            pBLEScan->setMaxResults(0);
            if (!pBLEScan->start(0, nullptr, false))
                ESP_LOGE(TAG, "Error starting continuous ble scan");

            while (true) {
                for (auto &f : BleFingerprintCollection::fingerprints)
                    if (f->query())
                        totalFpQueried++;

                // Enrollment::Loop();

                if (!pBLEScan->isScanning()) {
                    if (!pBLEScan->start(0, nullptr, true))
                        ESP_LOGE(TAG, "Error re-starting continuous ble scan");
                    delay(3000);  // If we stopped scanning, don't query for 3 seconds in order for us to catch any missed broadcasts
                } else {
                    delay(100);
                }
            }
        }

        void ESP32Presense::setup()
        {
            ESP_LOGD("SETUP1")
            BleFingerprintCollection::Setup();
            ESP_LOGD("SETUP2")
            xTaskCreatePinnedToCore(scanTask, "scanTask", SCAN_TASK_STACK_SIZE, nullptr, 1, &scanTaskHandle, CONFIG_BT_NIMBLE_PINNED_TO_CORE);
            ESP_LOGD("SETUP3")
        }

        void ESP32Presense::loop()
        {
            ESP_LOGD("LOOP1")
            reportLoop();
            ESP_LOGD("LOOP2")
        }

        void ESP32Presense::reportLoop() 
        {
            auto copy = BleFingerprintCollection::GetCopy();
            unsigned int count = 0;

            for (auto &i : copy)
                if (i->shouldCount())
                    count++;

            yield();
            sendTelemetry(totalSeen, totalFpSeen, totalFpQueried, totalFpReported, count);
            yield();

            auto reported = 0;
            for (auto &f : copy) {
                auto seen = f->getSeenCount();
                if (seen) {
                    totalSeen += seen;
                    totalFpSeen++;
                }

                /*
                if (f->hasReport()) {
                    if (reportBuffer(f))
                        f->clearReport();
                }
                
                if (reportDevice(f)) {
                    totalFpReported++;
                    reported++;
                }
                */
            }
        }

        bool ESP32Presense::sendTelemetry(
            unsigned int totalSeen, 
            unsigned int totalFpSeen, 
            unsigned int totalFpQueried, 
            unsigned int totalFpReported, 
            unsigned int count
        ) {
            this->publish(this->room_ + "/status", "online");
            this->publish(this->room_ + "/max_distance", BleFingerprintCollection::maxDistance);
            this->publish(this->room_ + "/absorption", BleFingerprintCollection::absorption);
            this->publish(this->room_ + "/tx_ref_rssi", BleFingerprintCollection::txRefRssi);
            this->publish(this->room_ + "/rx_adj_rssi", BleFingerprintCollection::rxAdjRssi);
            this->publish(this->room_ + "/query", BleFingerprintCollection::query);
            this->publish(this->room_ + "/include", BleFingerprintCollection::include);
            this->publish(this->room_ + "/exclude", BleFingerprintCollection::exclude);
            this->publish(this->room_ + "/known_macs", BleFingerprintCollection::knownMacs);
            this->publish(this->room_ + "/known_irks", BleFingerprintCollection::knownIrks);
            this->publish(this->room_ + "/count_ids", BleFingerprintCollection::countIds);

            return true;
        }
        /*
        void ESP32NimbleMQTTRoom::on_result(nimble_distance_custom::NimbleDistanceCustomResult& result)
        {
            auto address = result.address.toString();

            this->publish_json(
                this->base_topic_ + "/devices/" + address + "/" + this->room_,
                [=](ArduinoJson::JsonObject root) -> void {
                    root["id"] = address;
                    root["distance"] = result.distance;
                }
            );
        };
        */
    } // namespace esp32_nimble_tracker
} // namespace esphome