#include "lib/config.h"
#include "meshcore/meshframing.h"
#include "meshcore/packetstructs.h"
#include "control.h"
#include "meshcore/stats.h"
#include "string.h"
#include "util/hexdump.h"
#include "util/log.h"
#include <stdio.h>

#define TAG "Control"

void sendDiscoverRequest (const DiscoverRequestPayload *discReq) {

    FrameStruct frame;
    frame.path.pathLen = 0;
    uint8_t offset = 0;

    // Build payload
    frame.payload[offset++] = (discReq->prefixOnly & 0x01) | CONTROL_DATA_FLAG_TYPE_NODE_DISCOVER_REQ;

    frame.payload[offset++] = discReq->typeFilter;

    frame.payload[offset++] = (discReq->tag >> 0) & 0xFF;
    frame.payload[offset++] = (discReq->tag >> 8) & 0xFF;
    frame.payload[offset++] = (discReq->tag >> 16) & 0xFF;
    frame.payload[offset++] = (discReq->tag >> 24) & 0xFF;

    // optional `since`
    if (discReq->since != 0) {  // or another condition if you want to always include
        frame.payload[offset++] = (discReq->since >> 0) & 0xFF;
        frame.payload[offset++] = (discReq->since >> 8) & 0xFF;
        frame.payload[offset++] = (discReq->since >> 16) & 0xFF;
        frame.payload[offset++] = (discReq->since >> 24) & 0xFF;
    }

    frame.payloadLen = offset;

    LoRaTransmit (&frame);
}

void sendDiscoverResponse (const DiscoverResponsePayload *discResp) {
    FrameStruct frame;
    frame.path.pathLen = 0;
    uint8_t offset = 0;

    /* Control type + node type (lower nibble) */
    frame.payload[offset++] =
        (discResp->nodeType & 0x0F) |
        CONTROL_DATA_FLAG_DISCOVER_RESP;

    /* SNR */
    frame.payload[offset++] = (uint8_t)discResp->snr;

    /* Tag (LE) */
    frame.payload[offset++] = (discResp->tag >> 0) & 0xFF;
    frame.payload[offset++] = (discResp->tag >> 8) & 0xFF;
    frame.payload[offset++] = (discResp->tag >> 16) & 0xFF;
    frame.payload[offset++] = (discResp->tag >> 24) & 0xFF;

    /* Pubkey */
    if (discResp->pubkeyLen > 0) {
        memcpy (&frame.payload[offset],
                discResp->pubkey,
                discResp->pubkeyLen);
        offset += discResp->pubkeyLen;
    }

    frame.payloadLen = offset;

    LoRaTransmit (&frame);
}

void printDiscoverRequest (const DiscoverRequestPayload *p) {
    printf ("=== Discover Request ===\n");
    printf ("prefixOnly : %u\n", p->prefixOnly);
    printf ("typeFilter : 0x%02X\n", p->typeFilter);
    printf ("tag        : 0x%08lX\n", (unsigned long)p->tag);
    printf ("since      : 0x%08lX\n", (unsigned long)p->since);
}

void printDiscoverResponse (const DiscoverResponsePayload *p) {
    printf ("=== Discover Response ===\n");
    printf ("nodeType : %u\n", p->nodeType);
    printf ("snr      : %u\n", p->snr);
    printf ("tag      : 0x%08lX\n", (unsigned long)p->tag);

    hexdump ("pubkey   : ", p->pubkey, p->pubkeyLen);
    printf ("\n");
}

void decodeControlFrame (const FrameStruct * frame) {
    uint8_t index = 0;
    uint8_t type = frame->payload[index] & 0xF0;
    if (type == CONTROL_DATA_FLAG_TYPE_NODE_DISCOVER_REQ) {
        DiscoverRequestPayload discReq;
        discReq.prefixOnly = frame->payload[index++] & 0x01;

        discReq.typeFilter = frame->payload[index++];

        discReq.tag = frame->payload[index++];
        discReq.tag |= frame->payload[index++] << 8;
        discReq.tag |= frame->payload[index++] << 16;
        discReq.tag |= frame->payload[index++] << 24;

        if (index < frame->payloadLen) {
            discReq.since = frame->payload[index++];
            discReq.since |= frame->payload[index++] << 8;
            discReq.since |= frame->payload[index++] << 16;
            discReq.since |= frame->payload[index++] << 24;
        }
        printDiscoverRequest (&discReq);
        if ((discReq.typeFilter >> 2) & persistent.nodeType) {
            DiscoverResponsePayload discResp;

            discResp.tag = discReq.tag;
            discResp.nodeType = persistent.nodeType;
            discResp.pubkeyLen = sizeof (persistent.pubkey);
            memcpy (discResp.pubkey, persistent.pubkey, discResp.pubkeyLen);
            discResp.snr = stats.lastSNR;  // hopefully the correct one

            sendDiscoverResponse (&discResp);
            MESH_LOGD(TAG, "Replying to a discover request with tag %d", discResp.tag);
        }

    } else if (type == CONTROL_DATA_FLAG_DISCOVER_RESP) {
        DiscoverResponsePayload discResp;
        discResp.nodeType = frame->payload[index++] & 0x0F;

        discResp.snr = frame->payload[index++];

        discResp.tag = frame->payload[index++];
        discResp.tag |= frame->payload[index++] << 8;
        discResp.tag |= frame->payload[index++] << 16;
        discResp.tag |= frame->payload[index++] << 24;

        uint8_t remainingLen = frame->payloadLen - index;

        uint8_t pubKeyLen = (remainingLen > 8) ? sizeof (discResp.pubkey) : 8;
        discResp.pubkeyLen = pubKeyLen;
        memcpy (discResp.pubkey, &(frame->payload[index]), discResp.pubkeyLen);
        index += pubKeyLen;
        printDiscoverResponse (&discResp);
    }
}