/**
 * @file    queue.c
 * @brief   Create by AnKun on 2019/10/30
 * @version 1.0.0
 */
#include "queue.h"

static void qmemcpy(void* dst, const void* src, unsigned int size)
{
    char *d = (char *) dst;
    const char *s = (const char *) src;
    if(((int)d) % 4 == 0 && (((int)s) % 4) == 0)  /* 源地址与目标地址都对其4字节的情况下那么使用4字节拷贝的方式 */
    {
        while (size >= sizeof(int))
        {
            *(int *) d = *(int *) s;
            d += sizeof(int);
            s += sizeof(int);
            size -= sizeof(int);
        }
    }
    while (size--)
    {
        *d++ = *s++;
    }
}

/**
 * 获得队列使用量
 * @param  queue 队列句柄
 * @return       返回已使用量
 */
uint32_t Queue_GetUsed(const QueuePtr queue)
{
	int len = queue->write - queue->read;
	return len >= 0 ? len : (queue->size + len);
}

/**
 * 获得队列空余空间
 * @param  queue 队列句柄
 * @return       队列空余数
 */
uint32_t Queue_GetFree(const QueuePtr queue)
{
	int len = queue->write - queue->read;
	return len >= 0 ? (queue->size - 1 - len) : (((int) -1) - len);
}

/**	
 * 获得队列大小
 * @param  queue 队列句柄
 * @return       队列大小
 */
uint32_t Queue_GetSize(const QueuePtr queue)
{
	return queue->size - 1;
}

/**
 * 检测队列是否满
 * @param  queue
 * @return       1:满    0:非满
 */
bool Queue_isFull(const QueuePtr queue)
{
	if (queue->read == (queue->write + 1) % queue->size)
		return true;
	else
		return false;
}

/**
 * 检测队列是否为空
 * @param  queue
 * @return       1:空    fasle:非空
 */
bool Queue_isEmpty(const QueuePtr queue)
{
	if (queue->read == queue->write)
		return true;
	else
		return false;
}

/**
 * 数据入队
 * @param  queue  队列句柄
 * @param  buf    要入队的数据
 * @param  length 要入队的数据长度
 * @return        返回入队的字节数
 */
uint32_t Queue_Write(QueuePtr queue, const void* buf, uint32_t length)
{
	uint8_t *dataptr = (uint8_t *) buf;
	uint32_t offset = 0;
	uint32_t nwrite = 0;
	uint32_t nfree = 0;

	if(queue->lock != NULL)
	{
		queue->lock();
	}

	/* 传入的数据长度为0, 直接返回 */
	if (length == 0)
	{
		goto __exit;
	}

	/* 队列没有空间, 直接返回 */
	if ((nfree = Queue_GetFree(queue)) == 0)
	{
		goto __exit;
	}

	/* 计算实际能够入队的数据长度 */
	nwrite = nfree >= length ? length : nfree;

	/* 判断队列是否跨尾 */
	offset = queue->size - queue->write;
	if (offset >= nwrite)
	{
		qmemcpy(queue->payload + queue->write, dataptr, nwrite);
		queue->write += nwrite;
	}
	else
	{
		qmemcpy(queue->payload + queue->write, dataptr, offset);
		qmemcpy(queue->payload, dataptr + offset, nwrite - offset);
		queue->write = nwrite - offset;
	}

__exit:

	if(queue->unlock != NULL)
	{
		queue->unlock();
	}

	return nwrite;
}

/**
 * 数据出队
 * @param  queue  队列句柄
 * @param  buf    存放出队的数据
 * @param  length 出队的数据长度
 * @return        返回出队字节数
 */
uint32_t Queue_Read(QueuePtr queue, void* buf, uint32_t length)
{
	uint8_t *dataptr = (uint8_t *) buf;
	uint32_t offset = 0;
	uint32_t nused = 0;
	uint32_t nread = 0;

	if(queue->lock != NULL)
	{
		queue->lock();
	}

	/* 出队数据长度为0, 直接返回 */
	if (length == 0)
	{
		goto __exit;
	}

	/* 计算实际能够出队的数据长度 */
	if ((nused = Queue_GetUsed(queue)) == 0)
	{
		goto __exit;
	}

	/* 计算实际能够读到的数据长度 */
	nread = nused >= length ? length : nused;

	/* 判断要读的数据是否跨尾 */
	offset = queue->size - queue->read;
	if (offset >= nread)
	{
		qmemcpy(dataptr, queue->payload + queue->read, nread);
		queue->read += nread;
	}
	else
	{
		qmemcpy(dataptr, queue->payload + queue->read, offset);
		qmemcpy(dataptr + offset, queue->payload, nread - offset);
		queue->read = nread - offset;
	}

__exit:

	if(queue->unlock != NULL)
	{
		queue->unlock();
	}

	return nread;
}

/**
 * 初始化一个队列
 * @param queue   队列句柄
 * @param size    队列大小
 * @param payload 队列缓存地址
 */
void Queue_Init(QueuePtr queue, uint32_t size, void* payload, void (*lock)(void), void(*unlock)(void))
{
	queue->lock   = lock;
	queue->unlock = unlock;

	if(queue->lock != NULL)
	{
		queue->lock();
	}

	queue->read = 0;
	queue->write = 0;
	queue->payload = (uint8_t *) payload;
	queue->size = size + 1;

	if(queue->unlock != NULL)
	{
		queue->unlock();
	}
}

/**
 * 清空队列
 * @param queue 队列句柄
 */
void Queue_Clear(QueuePtr queue)
{
	if(queue->lock != NULL)
	{
		queue->lock();
	}
	queue->write = queue->read;
	if(queue->unlock != NULL)
	{
		queue->unlock();
	}
}

/**
 * 动态创建一个队列
 * @param  size 队列大小
 * @return      成功返回队列对象指针, 失败返回NULL
 */
QueuePtr Queue_Create(uint32_t size, void (*lock)(void), void(*unlock)(void))
{
	QueuePtr queue = NULL;

	if(lock != NULL)
	{
		lock();
	}

	if ((queue = (QueuePtr) malloc(sizeof(Queue))) == NULL)
	{
		goto __exit;
	}

	queue->size = size + 1;
	queue->read = 0;
	queue->write = 0;
	queue->payload = NULL;

	if ((queue->payload = (uint8_t *) malloc(QALIGN4(size))) == NULL)
	{
		free(queue);
		queue = NULL;
		goto __exit;
	}

	queue->lock = lock;
	queue->unlock = unlock;

__exit:

	if(unlock != NULL)
	{
		unlock();
	}

	return queue;
}

/**
 * 对于动态创建的队列进行清理工作
 * @param queue 队列句柄
 */
void Queue_Destory(QueuePtr queue)
{
	void (*lock)(void) = queue->lock;
	void (*unlock)(void) = queue->unlock;
	if(lock != NULL)
	{
		lock();
	}
	free(queue->payload);
	queue->payload = NULL;
	free(queue);
	queue = NULL;
	if(unlock != NULL)
	{
		unlock();
	}
}

/**
 * 撤销上次读取的数据！！！慎用！！！
 * @param queue 队列句柄
 * @param len   要退后的字节数
 */
void Queue_UndoRead(const QueuePtr queue, uint32_t len)
{
	int tmp;
	if(queue->lock != NULL)
	{
		queue->lock();
	}
	tmp = queue->read - len;
	queue->read = tmp < 0 ? ((uint32_t) (tmp + queue->size)) : ((uint32_t) tmp);
	if(queue->unlock != NULL)
	{
		queue->unlock();
	}
}

/**
 * 撤销上次写入的数据！！！慎用！！！
 * @param queue 队列句柄
 * @param len   要退后的字节数
 */
void Queue_UndoWrite(const QueuePtr queue, uint32_t len)
{
	int tmp;
	if(queue->lock != NULL)
	{
		queue->lock();
	}
	tmp = queue->write - len;
	queue->write = tmp < 0 ? ((uint32_t) (tmp + queue->size)) : (uint32_t) tmp;
	if(queue->unlock != NULL)
	{
		queue->unlock();
	}
}

bool Queue_WriteByte(QueuePtr queue, uint8_t dat)
{
	if(queue->lock != NULL)
	{
		queue->lock();
	}

	if (Queue_isFull(queue))
	{
		return false;
	}

	/* 防止跨尾 */
	queue->write %= queue->size;

	/* 写入数据 */
	queue->payload[queue->write++] = dat;

	if(queue->unlock != NULL)
	{
		queue->unlock();
	}

	return true;
}

bool Queue_ReadByte(QueuePtr queue, uint8_t* dat)
{
	if(queue->lock != NULL)
	{
		queue->lock();
	}

	if (Queue_isEmpty(queue))
	{
		return false;
	}

	/* 防止跨尾 */
	queue->read %= queue->size;

	/* 读取数据 */
	*dat = queue->payload[queue->read++];

	if(queue->unlock != NULL)
	{
		queue->unlock();
	}

	return true;
}