#include "os/queue.h"
#include "os/co_task.h"

typedef struct {
	u8 * buffer;
	int16_t count;
	int16_t esize;
	int16_t w_pos;
	int16_t r_pos;
}queue_t;
static __inline__ bool _have_write_space(queue_t *queue){
	int16_t buffer_len = queue->count * queue->esize;
	int16_t size = (queue->w_pos >= queue->r_pos)?(buffer_len - queue->w_pos + queue->r_pos):(queue->r_pos - queue->w_pos);
	size -= queue->esize;
	if (size <= 0){
		return false;
	}
	return true;
}

static __inline__ bool _have_read_space(queue_t *queue){
	int16_t buffer_len = queue->count * queue->esize;
	int16_t size = (queue->r_pos > queue->w_pos) ? (buffer_len - queue->r_pos + queue->w_pos) : (queue->w_pos - queue->r_pos);
	if (size < queue->esize){
		return false;
	}
	return true;
}

void *queue_create(u16 queue_count, u16 esize){
	queue_t *queue = (queue_t *)co_malloc(queue_count * esize + sizeof(queue_t));
	if (queue == NULL) {
		return NULL;
	}
	queue->buffer = (u8 *)(queue + 1);
	queue->count = queue_count;
	queue->esize = esize;
	queue->w_pos = queue->r_pos = 0;
	return queue;
}
bool queue_put(co_queue_t vqueue, void *data){
	queue_t *queue = (queue_t *)vqueue;
	if (!_have_write_space(queue)){
		return false;
	}

	memcpy(queue->buffer + queue->w_pos, data, queue->esize);

	queue->w_pos = queue->w_pos + queue->esize;
	if (queue->w_pos == queue->count * queue->esize){
		queue->w_pos = 0;
	}
	return true;

}

bool queue_get(co_queue_t vqueue, void *data) {
	queue_t *queue = (queue_t *)vqueue;
	if(!_have_read_space(queue)){
		return false;
	}
	if (data) {
		memcpy(data, queue->buffer + queue->r_pos, queue->esize);
	}
	queue->r_pos = queue->r_pos + queue->esize;
	if (queue->r_pos == queue->count * queue->esize){
		queue->r_pos = 0;
	}
	return true;
}