#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/net/openthread.h>
#include <openthread/thread.h>
#include <openthread/dataset.h>
#include <openthread/instance.h>
#include <openthread/udp.h>
#include <openthread/coap.h>
#include <openthread/ip6.h>
#include <lasertag_utils.h>
#include <thread_mgmt.h>
#include <string.h>
#include <stdio.h>

LOG_MODULE_REGISTER(thread_mgmt, CONFIG_THREAD_MGMT_LOG_LEVEL);

#define UDP_PORT 1234
#define MAX_DISCOVERED_NODES 10

int thread_mgmt_init(void)
{
    struct otInstance *instance = openthread_get_default_instance();
    otOperationalDataset dataset;

    if (!instance) return -ENODEV;

    /* Dataset Setup */
    memset(&dataset, 0, sizeof(otOperationalDataset));
    dataset.mActiveTimestamp.mSeconds = 1;
    dataset.mComponents.mIsActiveTimestampPresent = true;
    
    const char *net_name = lasertag_get_thread_network_name();
    memcpy(dataset.mNetworkName.m8, net_name, strlen(net_name));
    dataset.mComponents.mIsNetworkNamePresent = true;
    dataset.mPanId = lasertag_get_thread_pan_id();
    dataset.mComponents.mIsPanIdPresent = true;
    dataset.mChannel = lasertag_get_thread_channel();
    dataset.mComponents.mIsChannelPresent = true;
    memcpy(dataset.mExtendedPanId.m8, lasertag_get_thread_ext_pan_id(), 8);
    dataset.mComponents.mIsExtendedPanIdPresent = true;
    memcpy(dataset.mNetworkKey.m8, lasertag_get_thread_network_key(), 16);
    dataset.mComponents.mIsNetworkKeyPresent = true;
    uint8_t ml_prefix[] = {0xfd, 0xde, 0xad, 0x00, 0xbe, 0xef, 0x00, 0x00};
    memcpy(dataset.mMeshLocalPrefix.m8, ml_prefix, 8);
    dataset.mComponents.mIsMeshLocalPrefixPresent = true;

    otDatasetSetActive(instance, &dataset);
    otIp6SetEnabled(instance, true);
    otThreadSetEnabled(instance, true);

    LOG_INF("Thread MGMT: Initialized, UDP %d & CoAP %d open.", UDP_PORT, OT_DEFAULT_COAP_PORT);
    return 0;
}

void thread_mgmt_restart_thread_stack(bool force)
{
    struct otInstance *instance = openthread_get_default_instance();
    otOperationalDataset active_dataset;
    bool changed = false;

    // Get current active dataset. Force restart if unable to get it.
    if (otDatasetGetActive(instance, &active_dataset) != OT_ERROR_NONE) {
        LOG_WRN("Failed to get active dataset, forcing Thread stack restart.");
        force = true;
    }
    
    // Compare each relevant field and set 'changed' if any differ
    if (force || active_dataset.mChannel != lasertag_get_thread_channel()) {
        active_dataset.mChannel = lasertag_get_thread_channel();
        active_dataset.mComponents.mIsChannelPresent = true;
        changed = true;
    }
    if (force || active_dataset.mPanId != lasertag_get_thread_pan_id()) {
        active_dataset.mPanId = lasertag_get_thread_pan_id();
        active_dataset.mComponents.mIsPanIdPresent = true;
        changed = true;
    }
    if (force || memcmp(active_dataset.mExtendedPanId.m8, lasertag_get_thread_ext_pan_id(), 8) != 0) {
        memcpy(active_dataset.mExtendedPanId.m8, lasertag_get_thread_ext_pan_id(), 8);
        active_dataset.mComponents.mIsExtendedPanIdPresent = true;
        changed = true;
    }
    if (force || memcmp(active_dataset.mNetworkKey.m8, lasertag_get_thread_network_key(), 16) != 0) {
        memcpy(active_dataset.mNetworkKey.m8, lasertag_get_thread_network_key(), 16);
        active_dataset.mComponents.mIsNetworkKeyPresent = true;
        changed = true;
    }
    const char *net_name = lasertag_get_thread_network_name();
    if (force || strncmp(active_dataset.mNetworkName.m8, net_name, strlen(net_name)) != 0) {
        memcpy(active_dataset.mNetworkName.m8, net_name, strlen(net_name));
        active_dataset.mComponents.mIsNetworkNamePresent = true;
        changed = true;
    }

    if (changed) {
        active_dataset.mActiveTimestamp.mSeconds += 1; // Increment timestamp to signal change
        
        otThreadSetEnabled(instance, false);
        otIp6SetEnabled(instance, false);
        otError error = otDatasetSetActive(instance, &active_dataset);
        if (error != OT_ERROR_NONE) {
            LOG_ERR("Failed to set active dataset: %d", error);
            return;
        }
        otIp6SetEnabled(instance, true);
        otThreadSetEnabled(instance, true);

        LOG_INF("Thread stack restarted with updated dataset.");
    } else {
        LOG_INF("Thread stack restart not required; dataset unchanged.");
    }
    
}
