#include "msg_buf.h"

typedef uint16_t len_prefix_t;
#define _PADDING_BYTE (0xFF)
#define _PADDING_VALUE ((len_prefix_t)0xFFFF)
#define _LENGTH_SIZE (sizeof(len_prefix_t))



int msg_buf_init(msg_buf_t *buf, void *storage, uint32_t capacity)
{
	if ((capacity % _LENGTH_SIZE) != 0)
		return ERR_ARGS;

	buf->storage = storage;
	buf->capacity = capacity;
	buf->head = buf->count = 0;

	return ERR_OK;
}

static inline void pad_to_end(msg_buf_t *buf)
{
	//Pad to end, advancing head and count
	uint32_t len = buf->capacity - buf->head;
	memset(buf->storage + buf->head, _PADDING_BYTE, len);
	buf->count += len;
	buf->head = 0;
}

int msg_buf_push(msg_buf_t *buf, void *data, uint32_t len)
{
	if (len > MAX_LEN)
		return ERR_ARGS;

	//Length of prefix+message
	uint32_t len_f = _LENGTH_SIZE + len;

	if (buf->capacity - buf->head < len_f) //No space ahead
	{
		if (buf->head - buf->count < len_f) //No space behind
			return ERR_FULL;

		pad_to_end(buf);

		//Reset head
		buf->head = 0;
	}

	//At this point head is at the first free location and there is enough space after it
	*(len_prefix_t*)(buf->storage + buf->head) = len;
	memcpy(buf->storage + buf->head + _LENGTH_SIZE, buf, len);
	buf->head += len_f;
	buf->count += len_f;

	return ERR_OK;
}

static inline uint32_t buf_tail(msg_buf_t *buf)
{
	if (buf->head >= buf->count)
		return buf->head - buf->count;

	return buf->capacity + buf->head - buf->count;
}

int msg_buf_peek(msg_buf_t *buf, void **ptr, uint32_t *len)
{
	if (buf->count == 0)
		return ERR_EMPTY;

	uint32_t tail = buf_tail(buf);
	//No need to worry about padding, pop will do cleanups
	*len = *(uint16_t*)(buf->storage + tail);
	*ptr = buf->storage + tail + _LENGTH_SIZE;

	return ERR_OK;
}

int msg_buf_pop(msg_buf_t *buf)
{
	if (buf->count == 0)
		return ERR_EMPTY;

	uint32_t tail = buf_tail(buf);
	uint32_t len = *(uint16_t*)(buf->storage + tail);
	buf->count -= _LENGTH_SIZE + len;

	//If empty, reset head to start (minimize fragmentation)
	if (buf->count == 0)
	{
		buf->head = 0;
		return ERR_OK;
	}

	//At this point tail can either be at thestart of padding
	//  -> Clear padding (wraps back to 0)

	tail = buf_tail(buf);
	len = *(uint16_t*)(buf->storage + tail);

	//Leave as is if not padding
	if (len != _PADDING_VALUE)
		return ERR_OK;

	//Clear padding
	uint32_t padding_len = buf->capacity - tail;
	buf->count -= padding_len;

	//Tail is now at storage start.
	//If the buffer is empty, it is also aligned as a consequence.
	return ERR_OK;
}