#ifndef _TLPBUF_H
#define _TLPBUF_H


#include <stdint.h>
#include <stddef.h>
#include <stdbool.h>

#include <string.h>


#ifdef __cplusplus
extern "C"
{
#endif


typedef void (*pbuf_callback_t)(void*);		//function pointer that will be called when a pbuf is freed in this pool

struct pbuf_pool_s;

typedef struct pbuf_s
{
	uint8_t* payload;
	uint32_t length;

	uint32_t capacity;
	uint8_t* payload_memory;

	struct pbuf_s* next;			//used to link pbufs
	struct pbuf_pool_s* pool;

	void* user;
}pbuf_t;



typedef struct pbuf_pool_s
{
	pbuf_t* pbuf_memory;
	uint32_t pbuf_count;

	uint8_t* payload_memory;
	uint32_t payload_capacity;
	
	uint32_t pbufs_available;
	pbuf_t* free_list;
	
	pbuf_callback_t free_callback;
	void* free_callback_user;
}pbuf_pool_t;



#define PBUF_DECLARE_POOL(pool_name, pbuf_count, pbuf_size) \
static uint8_t pool_name##_payload_memory[(pbuf_count) * (((pbuf_size) + 3) & ~3)] __attribute__((aligned(4))); \
static pbuf_t pool_name##_memory[pbuf_count]; \
pbuf_pool_t pool_name


#define STATIC_PBUF_DECLARE_POOL(pool_name, pbuf_count, pbuf_size) \
static uint8_t pool_name##_payload_memory[(pbuf_count) * (((pbuf_size) + 3) & ~3)] __attribute__((aligned(4))); \
static pbuf_t pool_name##_memory[pbuf_count]; \
static pbuf_pool_t pool_name



#define PBUF_INIT_POOL(pool_name) \
	pbuf_pool_create(&pool_name, \
						sizeof(pool_name##_memory) / sizeof(pbuf_t), \
						pool_name##_memory, \
						sizeof(pool_name##_payload_memory) / (sizeof(pool_name##_memory) / sizeof(pbuf_t)), \
						pool_name##_payload_memory)


/**
	\brief Creates a new pool of pbufs

	\param pool 		Pointer to a pbuf_pool_t structure that will be initialized
	\param pbuf_count	Number of pbufs to create in the pbuf pool
	\param pbuf_memory	Pointer to a statically allocated area of memory for storing the pbuf structures. 
	\param payload_capacity	Size of the pbuf payload.
	\param payload_memory		Pointer to statically allocated memory that will be used for storing the pbuf attached buffers
									
	pbuf_memory needs to be a statically allocated memory chunk of size: pbuf_count * sizeof(pbuf_t) (bytes)
	payload_memory needs to be a statically allocated memory chunk of size: pbuf_count * pbuf_payload_size (bytes)

*/
void pbuf_pool_create(pbuf_pool_t *pool, uint32_t pbuf_count, void* pbuf_memory, uint32_t payload_capacity, void* payload_memory);
void pbuf_pool_reset(pbuf_pool_t* pool);


#ifdef DEBUG
void pbuf_pool_dump(pbuf_pool_t* pool);
else
static inline void pbuf_pool_dump(pbuf_pool_t* pool) {}
#endif


static inline uint32_t pbuf_pool_get_available(pbuf_pool_t* pool)
{
	return pool->pbufs_available;
}


static inline void pbuf_reset(pbuf_t* p)
{
	p->payload = p->payload_memory;
	p->capacity = p->pool->payload_capacity;
}


static inline uint32_t pbuf_offset(pbuf_t* p)
{
	return p->payload - p->payload_memory;
}


static inline pbuf_t* pbuf_pool_alloc(pbuf_pool_t* pool)
{
	pbuf_t* p = pool->free_list;

	if (p)
	{
		pool->free_list = pool->free_list->next;		//remove it from the list
		pool->pbufs_available--;
		
		pbuf_reset(p);
	}

	return p;
}


static inline void pbuf_pool_free(pbuf_t* p)
{
	if (p)
	{
		p->next = p->pool->free_list;		//add it back to the free list
		p->pool->free_list = p;
		p->pool->pbufs_available++;

		if (p->pool->free_callback)				//call the hook if we have one
			p->pool->free_callback(p->pool->free_callback_user);
	}
}


static inline void pbuf_queue_free(pbuf_t* queue)
{
	while(queue)
	{
		pbuf_t *p = queue;
		queue = queue->next;
		
		pbuf_free(p);
	}
}



static inline bool pbuf_insert_uint8(pbuf_t* p, uint8_t value)
{
	if ((p->length + sizeof(value)) > p->capacity)
		return false;
	
	*p->payload++ = value;
	
	p->length += sizeof(value);

	return true;
}


static inline bool pbuf_insert_uint16(pbuf_t* p, uint16_t value)
{
	if ((p->length + sizeof(value)) > p->capacity)
		return false;
	
	*p->payload++ = value;
	*p->payload++ = value >> 8;

	p->length += sizeof(value);

	return true;
}


static inline bool pbuf_insert_uint32(pbuf_t* p, uint32_t value)
{
	if ((p->length + sizeof(value)) > p->capacity)
		return false;

	*p->payload++ = value;
	*p->payload++ = value >> 8;
	*p->payload++ = value >> 16;
	*p->payload++ = value >> 24;
	
	p->length += sizeof(value);

	return true;
}


static inline bool pbuf_insert_uint64(pbuf_t* p, uint64_t value)
{
	if ((p->length + sizeof(value)) > p->capacity)
		return false;

	*p->payload++ = value;
	*p->payload++ = value >> 8;
	*p->payload++ = value >> 16;
	*p->payload++ = value >> 24;

	*p->payload++ = value >> 32;
	*p->payload++ = value >> 40;
	*p->payload++ = value >> 48;
	*p->payload++ = value >> 56;
	
	p->length += sizeof(value);

	return true;
}


static inline bool pbuf_insert_float(pbuf_t* p, float value)
{
	if ((p->length + sizeof(value)) > p->capacity)
		return false;
	
	memcpy(p->payload, &value, sizeof(value));
	p->payload += sizeof(value); 
	p->length += sizeof(value);

	return true;
}


static inline bool pbuf_insert_double(pbuf_t* p, double value)
{
	if ((p->length + sizeof(value)) > p->capacity)
		return false;
	
	memcpy(p->payload, &value, sizeof(value));
	p->payload += sizeof(value); 
	p->length += sizeof(value);

	return true;
}


static inline bool pbuf_insert_data(pbuf_t* p, const void* data, size_t size)
{
	if ((p->length + size) > p->capacity)
		return false;
	
	memcpy(p->payload, data, size);
	p->payload += size;
	p->length += size;

	return true;
}



static inline bool pbuf_extract_uint8(pbuf_t* p, uint8_t* value)
{
	if (p->length < sizeof(uint8_t))
		return false;

	*value = *p->payload++;

	p->length -= sizeof(uint8_t);

	return true;
}


static inline bool pbuf_extract_uint16(pbuf_t* p, uint16_t* value)
{
	if (p->length < sizeof(uint16_t))
		return false;
	
	*value = ((uint16_t)p->payload[1] << 8) + p->payload[0];
	
	p->payload += sizeof(uint16_t);
	p->length -= sizeof(uint16_t);
	
	return true;
}


static inline bool pbuf_extract_uint32(pbuf_t* p, uint32_t* value)
{
	if (p->length < sizeof(uint32_t))
		return false;

	*value = ((uint32_t)p->payload[3] << 24) + 
			((uint32_t)p->payload[2] << 16) + 
			((uint32_t)p->payload[1] << 8) + 
			p->payload[0];

	p->payload += sizeof(uint32_t);
	p->length -= sizeof(uint32_t);

	return true;
}


static inline bool pbuf_extract_uint64(pbuf_t* p, uint64_t* value)
{
	if (p->length < sizeof(uint64_t))
		return false;

	*value = ((uint64_t)p->payload[7] << 56) + 
			((uint64_t)p->payload[6] << 48) + 
			((uint64_t)p->payload[5] << 40) + 
			((uint64_t)p->payload[4] << 32) + 
			((uint32_t)p->payload[3] << 24) + 
			((uint32_t)p->payload[2] << 16) + 
			((uint32_t)p->payload[1] << 8) + 
			p->payload[0];

	p->payload += sizeof(uint64_t);
	p->length -= sizeof(uint64_t);

	return true;
}


static inline bool pbuf_extract_float32(pbuf_t* p, float* value)
{
	if (p->length < sizeof(float))
		return false;

	memcpy(value, p->payload, sizeof(float));

	p->payload += sizeof(float);
	p->length -= sizeof(float);

	return true;
}


static inline bool pbuf_extract_data(pbuf_t* p, void* data, size_t size)
{
	if (p->length < size)
		return false;

	memcpy(data, p->payload, size);
	p->payload += size;
	p->length -= size;

	return true;
}


static inline bool pbuf_extract_pointer(pbuf_t* p, void** data, size_t size)
{
	if (p->length < size)
		return false;

	*data = p->payload;
	p->payload += size;
	p->length -= size;

	return true;
}



#ifdef __cplusplus
}
#endif

#endif