#include "SSDP_discover.h"
#include <ESP8266WiFi.h>

#ifdef _DEBUG
#include <utils/utils.h>
#define _DEBUG_PRINT(x) utils::debug_print(x)
#else
#define _DEBUG_PRINT(x)
#endif

namespace Control_System
{

SSDP_discover::SSDP_discover() : discoveredServices(new std::vector<IPAddress>()), server(new WiFiUDP()) {}

SSDP_discover::~SSDP_discover()
{
    server->flush();
    yield();
    server->stopAll();
}

bool SSDP_discover::discover()
{
    auto success = false;

    for (size_t i = 0; i < SEARCH_RETRY && success == false; i++)
    {
        _DEBUG_PRINT("Sending M-SEARCH");
        _DEBUG_PRINT(IPAddress(MULTICAST_ADDRESS).toString());
        auto mcast1 = server->beginPacketMulticast(IPAddress(MULTICAST_ADDRESS), SSDP_PORT, WiFi.localIP());
        if (mcast1 != 1)
        {
            _DEBUG_PRINT("begin packet failed");
            continue;
        }

        auto write = server->write(SSDP_MSEARCH, strnlen(SSDP_MSEARCH, 500));
        if (write <= 0)
        {
            _DEBUG_PRINT("write failed");
            continue;
        }

        auto end = server->endPacket();
        if (end != 1)
        {
            _DEBUG_PRINT("end failed");
            continue;
        }

        await_response();

        _DEBUG_PRINT("done awaiting");

        if (discoveredServices->empty())
        {
            _DEBUG_PRINT("No services found");
            continue;
        }

        for (auto &c : *discoveredServices)
        {
            _DEBUG_PRINT(c.toString());
        }

        success = true;
    }

    return success;
}

void SSDP_discover::await_response()
{
    server->begin(SSDP_PORT);
    byte buffer[INPUT_BUFFER_SIZE + 1] = {0};
    size_t bufferContentLength = 0;
    _DEBUG_PRINT("UDP Await");

    //Count down from delay time to 0, assume this takes ~1ms
    for (auto delaytime = SEARCH_TIMEOUT * 1000; delaytime > 0; delaytime--)
    {
        auto correctService = false;
        auto location = IPAddress();

        // Process a packet
        while (server->available() > 0)
        {
            bufferContentLength = server->readBytesUntil('\n', buffer, INPUT_BUFFER_SIZE);
            buffer[bufferContentLength] = '\0';
            _DEBUG_PRINT((char *)buffer);

            // strlen, but they're contexpr, so its ok?...
            if (bufferContentLength >= strlen(SERVICE) &&
                memcmp(SERVICE, buffer, strlen(SERVICE)) == 0)
            {
                _DEBUG_PRINT(F("Service line"));
                
                if (strstr(reinterpret_cast<char*>(buffer), SERVICE_NAME) != nullptr)
                {
                    _DEBUG_PRINT("Service valid");
                    correctService = true;
                }
            }
                            // + 2 for ": "
            else if (bufferContentLength >= strlen(LOCATION) + 2 &&
                     memcmp(LOCATION, buffer, strlen(LOCATION)) == 0)
            {
                _DEBUG_PRINT(F("Location Line"));
             
                auto offset = buffer + (strlen(LOCATION) + 2);
                char ipBuffer[IP4ADDR_STRLEN_MAX + 1] = {0};

                strncpy(ipBuffer, reinterpret_cast<char*>(offset), IP4ADDR_STRLEN_MAX);

                _DEBUG_PRINT((char *)offset);
                if (IPAddress::isValid(ipBuffer))
                {
                    location.fromString(ipBuffer);
                }
                else
                {
                    _DEBUG_PRINT("Resolve hostname");
                    // If the ip was invalid, maybe its a hostname?
                    if (WiFi.hostByName(reinterpret_cast<char*>(offset), location) != 1)
                    {
                        // Nope it was just invalid
                        _DEBUG_PRINT("Invalid Service");
                    }
                }                
            }
        }

        if (location.isSet() && correctService)
        {
            _DEBUG_PRINT(F("Add as valid response"));
            discoveredServices->push_back(location);
        }

        delay(1);
    }
    server->stop();
}
} // namespace Control_System