#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;
}

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;
    frame->headerCrc = Xil_EndianSwap16(crc16((uint8_t*)frame, 2));
//    LOG_INFO("Make Mac Frame Done");
}

void ParseMacFrame(MacFrame *frame)
{

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

	for(int i =0;i<frame->udpNum;i++)
	{
		USDP * usdp = (USDP*)p;
		UdpSendData(usdp->dataBuf, usdp->len);
		usleep(10);
//        xemacif_input(netif);
//        xemacif_input(netif);

		p+=usdp->len + 8;
	}
	LOG_INFO("Parse Mac Done");
}
