#ifndef _PBUF_H
#define _PBUF_H


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

#include <assert.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;
}pbuf_pool_t;



#define PBUF_DECLARE_POOL(pool_name, pbuf_count, pbuf_size) \
static uint8_t pool_name##_payload_memory[(pbuf_count) * (pbuf_size)] __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)] __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);


void pbuf_dump_pool(pbuf_pool_t *pool);


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_alloc(pbuf_pool_t *pool)
{
	assert(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_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();
	}
}


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



static inline void pbuf_insert_uint8(pbuf_t* p, uint8_t value)
{
	assert((p->length + sizeof(value)) <= p->capacity);
	
	*p->payload++ = value;
	
	p->length += sizeof(value);
}


static inline void pbuf_insert_uint16(pbuf_t* p, uint16_t value)
{
	assert((p->length + sizeof(value)) <= p->capacity);
	
	*p->payload++ = value;
	*p->payload++ = value >> 8;

	p->length += sizeof(value);
}


static inline void pbuf_insert_uint32(pbuf_t* p, uint32_t value)
{
	assert((p->length + sizeof(value)) <= p->capacity);

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


static inline void pbuf_insert_uint64(pbuf_t* p, uint64_t value)
{
	assert((p->length + sizeof(value)) <= p->capacity);

	*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);
}


static inline void pbuf_insert_float32(pbuf_t* p, float value)
{
	assert((p->length + sizeof(value)) <= p->capacity);
	assert(sizeof(float) == sizeof(uint32_t));
	
	const uint32_t* const value_ptr = (uint32_t*)&value;

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


static inline void pbuf_insert_data(pbuf_t* p, const void* data, size_t size)
{
	assert((p->length + size) <= p->capacity);
	
	memcpy(p->payload, data, size);
	p->payload += size;
	p->length += size;
}


static inline void pbuf_insert_str(pbuf_t* p, const char* str)
{
	while(*str && (p->length < p->capacity))
	{
		*p->payload++ = *str++;
		p->length++;
	}
}


static inline void pbuf_insert_nstr(pbuf_t* p, const char* str, size_t count)
{
	while(*str && (p->length < p->capacity) && count)
	{
		*p->payload++ = *str++;
		p->length++;
		count--;
	}
}


static inline uint8_t pbuf_extract_uint8(pbuf_t* p)
{
	uint8_t value = 0;

	assert(p->length >= sizeof(value));

	value = *p->payload++;

	p->length -= sizeof(value);

	return value;
}


static inline uint16_t pbuf_extract_uint16(pbuf_t* p)
{
	uint16_t value = 0;

	assert(p->length >= sizeof(value));
	
	value = ((uint16_t)p->payload[1] << 8) + 
			p->payload[0];
	
	p->payload += sizeof(value);
	p->length -= sizeof(value);
	
	return value;
}


static inline uint32_t pbuf_extract_uint32(pbuf_t* p)
{
	uint32_t value = 0;

	assert(p->length >= sizeof(value));

	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(value);
	p->length -= sizeof(value);

	return value;
}


static inline uint64_t pbuf_extract_uint64(pbuf_t* p)
{
	uint64_t value = 0;

	assert(p->length >= sizeof(value));

	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(value);
	p->length -= sizeof(value);

	return value;
}


static inline float pbuf_extract_float32(pbuf_t* p)
{
	assert(p->length >= sizeof(float));
	assert(sizeof(float) == sizeof(uint32_t));

	const uint32_t value_u32 = ((uint32_t)p->payload[3] << 24) + ((uint32_t)p->payload[2] << 16) + ((uint32_t)p->payload[1] << 8) + p->payload[0];
	const float* const value_ptr = (float*)&value_u32;

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

	return *value_ptr;
}


static inline void pbuf_extract_data(pbuf_t* p, void* data, size_t size)
{
	assert(p->length >= size);

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



static inline void pbuf_extract_str(pbuf_t* p, char* str, size_t max_count)
{
	while(*p->payload && p->length && max_count)
	{
		*str++ = *p->payload++;
		p->length--;
		max_count--;
	}
	
	*str = 0;
}


static inline void pbuf_extract_nstr(pbuf_t* p, char* str, size_t count)
{
	while(*p->payload && p->length && count)
	{
		*str++ = *p->payload++;
		p->length--;
		count--;
	}
	
	*str = 0;
}



#ifdef __cplusplus
}
#endif

#endif