// 
#include <SEGGER_RTT.h>
#include <stdio.h>

#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/net/openthread.h>

#include <openthread.h>
#include <openthread/thread.h>
#include <openthread/network_time.h>
#include <openthread/icmp6.h>
#include <openthread/thread_ftd.h>

LOG_MODULE_REGISTER(lat_test, CONFIG_OT_COMMAND_LINE_INTERFACE_LOG_LEVEL);

#define TIME_SYNC_PERIOD_SECONDS    (10)
// NOTE: this can be found out by running "ot ipaddr" on the destination
#define PING_DESTINATION_IP         "fdde:ad00:beef:0:0:ff:fe00:e800"
// 1: if this node is the sender, 0: for destination
#define IS_PING_SENDER              (1)
// Sizeof of icmp message including the header
#define MESSAGE_SIZE                (127)
// Sleep betweeen sending icmp messages
#define SLEEP_DURATION              K_MSEC(250)

typedef enum {
    MESSAGE_SEND,
    MESSAGE_RECEIVE,
} Message_Direction;

typedef struct {
    otInstance*     instance;
    struct k_sem    is_network_synced;
} Context;

static
void thread_sync_event(void* user_data) {
    Context* context = user_data;

    uint64_t network_time = 0;
    otNetworkTimeStatus time_status = otNetworkTimeGet(context->instance, &network_time);
    if (time_status == OT_NETWORK_TIME_SYNCHRONIZED) {
        k_sem_give(&context->is_network_synced);
    }
}

static
void thread_state_changed(otChangedFlags flags, void* user_data) {
    LOG_INF("State changed");
}

static 
struct openthread_state_changed_callback ot_state_chaged_cb = {
    .otCallback = thread_state_changed,
};

static
void log_icmp(Context* context, const otIcmp6Header* icmp_header, Message_Direction direction) {
    char* message_type = "unknown";
    switch (icmp_header->mType) {
        case OT_ICMP6_TYPE_ECHO_REQUEST:
            message_type = "icmp_echo";
        break;
        case OT_ICMP6_TYPE_ECHO_REPLY:
            message_type = "icmp_reply";
        break;
    }

    char* message_direction = "sent";
    if (direction == MESSAGE_RECEIVE) {
        message_direction = "received";
    }

    uint64_t message_time = 0;
    otNetworkTimeStatus time_status = otNetworkTimeGet(context->instance, &message_time);
    if (time_status != OT_NETWORK_TIME_SYNCHRONIZED) {
        SEGGER_RTT_WriteString(0, "Tried to log but network is not synchronized");
    }

    uint16_t identifier = BSWAP_16(icmp_header->mData.m16[0]);

    char print_buffer[128] = {0};
    snprintf(print_buffer, sizeof(print_buffer), "{%u;%s;%s;%u}\n", (uint32_t)message_time, message_direction, message_type, identifier);
    SEGGER_RTT_WriteString(0, print_buffer);
}

static
void icmp_message_handler(void* user_data, otMessage* message, const otMessageInfo* message_info, const otIcmp6Header* icmp_header) {
    Context* context = user_data;
    log_icmp(context, icmp_header, MESSAGE_RECEIVE);
}

static Context thread_context;

int main(void) {
    k_sem_init(&thread_context.is_network_synced, 0, 1);

#if IS_PING_SENDER
    LOG_INF("Sender mode");
#else
    LOG_INF("Receiver mode");
#endif

    thread_context.instance = openthread_get_default_instance();

    openthread_state_changed_callback_register(&ot_state_chaged_cb);

    static otIcmp6Handler icmp_handler = {
        .mContext = &thread_context,
        .mReceiveCallback = icmp_message_handler,
    };
    otError icmp_handler_status = otIcmp6RegisterHandler(thread_context.instance, &icmp_handler);
    if (icmp_handler_status != OT_ERROR_NONE) {
        LOG_ERR("Could not reguster icmp handler");
    }

    otError period_status = otNetworkTimeSetSyncPeriod(thread_context.instance, TIME_SYNC_PERIOD_SECONDS);
    if (period_status != OT_ERROR_NONE) {
        LOG_ERR("Could not set sync period");
        return 0;
    }

    otNetworkTimeSyncSetCallback(thread_context.instance, thread_sync_event, &thread_context);

    openthread_run();

#if IS_PING_SENDER
    uint32_t icmp_identifier = 1;

    LOG_INF("Waiting for network to be synced...");
    k_sem_take(&thread_context.is_network_synced, K_FOREVER);
    LOG_INF("Network synced");

    otIp6Address ping_destination = {0};
    if (otIp6AddressFromString(PING_DESTINATION_IP, &ping_destination) != OT_ERROR_NONE) {
        LOG_ERR("Could not parse ip addr");
    }

    while (true) {
        otMessage* message = otIp6NewMessage(thread_context.instance, NULL);
        const otMessageInfo message_info = {
            .mPeerAddr = ping_destination,
        };

        otIcmp6Header header = {
            .mType = OT_ICMP6_TYPE_ECHO_REQUEST,
            // We alrady set this with the otIcmp6SendEchoRequest api but because logging looks at this value
            // before we call the api we fill it out anyway
            .mData.m16[0] = BSWAP_16(icmp_identifier),
        };
        otMessageAppend(message, &header, sizeof(header));

        static uint8_t fill_payload[MESSAGE_SIZE - sizeof(header)] = {0};
        otMessageAppend(message, fill_payload, sizeof(fill_payload));

        log_icmp(&thread_context, &header, MESSAGE_SEND);
        otIcmp6SendEchoRequest(thread_context.instance, message, &message_info, icmp_identifier);

        k_sleep(SLEEP_DURATION);

        icmp_identifier += 1;   
    }
#endif

    return 0;
}