#include "esp32_presense.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;

        DynamicJsonDocument doc(1024);
        std::string _id, roomsTopic;

        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::set_room(std::string room) { 
            _id = slugify(room);
            roomsTopic = std::string(CHANNEL) + std::string("/rooms/") + _id;
        }
        
        void ESP32Presense::set_max_distance(float maxDistance) {
            BleFingerprintCollection::maxDistance = maxDistance;
        }

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

        void ESP32Presense::loop()
        {
            reportLoop();
        }

        bool ESP32Presense::reportBuffer(BleFingerprint *f) {
            auto report = f->getReport();
            std::string topic = Sprintf(CHANNEL "/devices/%s/%s/%s", f->getId().c_str(), _id.c_str(), report.getId().c_str());
            return this->publish(topic, report.getPayload());
        }

        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 (this->reportDevice(f)) {
                    totalFpReported++;
                    reported++;
                }
            }
        }

        bool ESP32Presense::reportDevice(BleFingerprint *f) {
            doc.clear();
            JsonObject obj = doc.to<JsonObject>();
            if (!f->report(&obj))
                return false;

            std::string buffer;
            serializeJson(doc, buffer);
            std::string devicesTopic = Sprintf(CHANNEL "/devices/%s/%s", f->getId().c_str(), _id.c_str());

            bool p1 = false, p2 = false;
            for (int i = 0; i < 10; i++) {
                if (!p1 && (!publishRooms || this->publish(roomsTopic.c_str(), buffer.c_str())))
                p1 = true;

                if (!p2 && (!publishDevices || this->publish(devicesTopic.c_str(), buffer.c_str())))
                    p2 = true;

                if (p1 && p2)
                    return true;
                delay(20);
            }

            reportFailed++;
            return false;
        }
        
        bool ESP32Presense::sendTelemetry(
            unsigned int totalSeen, 
            unsigned int totalFpSeen, 
            unsigned int totalFpQueried, 
            unsigned int totalFpReported, 
            unsigned int count
        ) {
            this->publish(roomsTopic + "/status", "online");
            this->publish(roomsTopic + "/max_distance", BleFingerprintCollection::maxDistance);
            this->publish(roomsTopic + "/absorption", BleFingerprintCollection::absorption);
            this->publish(roomsTopic + "/tx_ref_rssi", BleFingerprintCollection::txRefRssi);
            this->publish(roomsTopic + "/rx_adj_rssi", BleFingerprintCollection::rxAdjRssi);
            this->publish(roomsTopic + "/query", BleFingerprintCollection::query);
            this->publish(roomsTopic + "/include", BleFingerprintCollection::include);
            this->publish(roomsTopic + "/exclude", BleFingerprintCollection::exclude);
            this->publish(roomsTopic + "/known_macs", BleFingerprintCollection::knownMacs);
            this->publish(roomsTopic + "/known_irks", BleFingerprintCollection::knownIrks);
            this->publish(roomsTopic + "/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