#include "mac.h"
#include "string.h"
#include "system.h"
static uint8_t macId = MAC_ID;
// ethernet send data queue
Queue *macSendQueue;

// Singleton
MacFrame txMacFrame __attribute__((aligned(128)));
MacFrame rxMacFrame __attribute__((aligned(128)));

/// @brief Create QNode, the buf data will be copy, and will not free buf inside.
/// @param buf data buffer
/// @param len length of the valid data
/// @return QNode created
QNode *CreateNode(uint8_t *buf, uint32_t len)
{
    QNode *newNode = (QNode *)malloc(sizeof(QNode));
    uint8_t *tempBuf = (uint8_t *)malloc(len * sizeof(uint8_t));
    memcpy(tempBuf, buf, len);
    if (newNode == NULL || tempBuf == NULL)
    {
        xil_printf("Memory allocation failed!\r\n");
        free(newNode);
        free(tempBuf);
        return NULL;
    }
    newNode->buf = tempBuf;
    newNode->len = len;
    newNode->next = NULL;
    return newNode;
}

Queue *CreateQueue()
{
    Queue *queue = (Queue *)malloc(sizeof(Queue));
    if (queue == NULL)
    {
        LOG_ERROR("Memory allocation failed!");
        return NULL;
    }
    queue->head = NULL;
    queue->tail = NULL;
    queue->nodeNumber = 0;
    queue->size = 0;
    return queue;
}

int IsEmpty(Queue *queue)
{
    return queue->head == NULL;
}

void Push(Queue *queue, uint8_t *buf, uint32_t len)
{
    QNode *newNode = CreateNode(buf, len);
    if (newNode == NULL)
    {
        LOG_INFO("Queue Push Failed");
        return;
    }
    if (IsEmpty(queue))
    {
        queue->head = newNode;
        queue->tail = newNode;
    }
    else
    {
        queue->tail->next = newNode;
        queue->tail = newNode;
    }
    queue->nodeNumber++;
    queue->size += len;
}

// void PushEthPacket(Queue *queue, uint32_t ipAddr, uint16_t port, uint8_t len, uint8_t *buf, uint32_t len)
// {
//     QNode *newNode = CreateNode(buf, len);
//     if (newNode == NULL)
//     {
//         LOG_INFO("Queue Push Failed");
//         return;
//     }
//     if (IsEmpty(queue))
//     {
//         queue->head = newNode;
//         queue->tail = newNode;
//     }
//     else
//     {
//         queue->tail->next = newNode;
//         queue->tail = newNode;
//     }
//     queue->nodeNumber++;
//     queue->size += len;
// }

QNode *Pop(Queue *queue)
{
    if (IsEmpty(queue))
    {
        xil_printf("Queue is empty!\r\n");
        return NULL;
    }
    QNode *temp = queue->head;
    queue->head = queue->head->next;
    if (queue->head == NULL)
    {
        queue->tail = NULL;
    }
    queue->nodeNumber--;
    queue->size -= temp->len;
    return temp;
}

void FreeNode(QNode *node)
{
    free(node->buf);
    free(node);
}

void MakeMacFrame(MacFrame *frame, Queue *macSendQueue)
{

    uint32_t remainLen = MAC_FRAME_PAYLOAD_LEN;
    uint32_t idx = 0;
    uint8_t udpNum = 0;

    while (!IsEmpty(macSendQueue) && remainLen >= macSendQueue->head->len)
    {
        QNode *q = Pop(macSendQueue);
        uint8_t *dst = ((uint8_t *)(frame->payload) + idx);
        memcpy(dst, q->buf, q->len);
        idx += q->len;
        remainLen -= q->len;
        FreeNode(q);
        udpNum++;
    }
    frame->id = macId;
    frame->udpNum = udpNum;

    //    LOG_INFO("Make Mac Frame Done");
}

int ParseMacFrame(MacFrame *frame)
{

    uint8_t val = 0;
    if (val != 0 || frame->udpNum == 0 || frame->udpNum >= 20)
    {
        //		LOG_WARNING("CRC check failed");
        return;
    }
    uint8_t *p = frame->payload;
//    xil_printf(INFO_MSG("Recv from %d\t%d packet"), frame->id, frame->udpNum);

    PhyUpdateCurSlot(frame->id);
//     !ATTENTION this is parsed by header,
    for (int i = 0; i < frame->udpNum; i++)
    {
        USDP *usdp = (USDP *)p;
		if(usdp->len > 1472)
		{
			break;
		}
        UdpSendData(usdp->dataBuf, usdp->len);
        p += usdp->len + 8;
    }


    //     !ATTENTION this is parsed by byte,
//    uint8_t * idx = p;
//    uint32_t offset = 0;
//    int packetSent = 0;
//    uint32_t ip_mask_cmp = ipaddr.addr & netmask.addr;
//	while(1)
//	{
//		if( (uint32_t)((*(uint32_t*)idx) & netmask.addr) == ip_mask_cmp)
//		{
//			USDP *usdp = (USDP *)idx;
//			if(usdp->len > 1472)
//			{
//				idx+=8;
//				continue;
//			}
//			UdpSendData(usdp->dataBuf, usdp->len);
//			idx += usdp->len + 8;
//			offset += usdp->len + 8;
//			packetSent ++;
//		}
//		else
//		{
//			idx++;
//		}
//		if(packetSent >= frame->udpNum || offset >= MAC_FRAME_PAYLOAD_LEN)
//		{
//			break;
//		}
//	}

    //	LOG_INFO("Parse Mac Done");
    return frame->id;
}
