#include "meshcore/meshframing.h"
#include "meshcore/packetstructs.h"
#include "control.h"
#include "string.h"

#define TAG "Control"

void sendDiscoverRequest(const DiscoverRequestPayload *discReq) {

    FrameStruct frame;
    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;

    sendFrame(frame);
}


void decodeControlFrame(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;
            }

        } 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;
            memcpy(discResp.pubkey, &(frame.payload[index]), pubKeyLen);
            index += pubKeyLen;

        }
}