
#include "FreeRTOS.h"
#include "static-allocator.h"
#include <stdlib.h>
#include <stdio.h>
#include "task.h"

void poolInit( static_pool_t* inPool, uint8_t inItemSize, uint8_t inNumItems)
{
    uint8_t *entry_ptr = (uint8_t*)&inPool->pool[0];
    unsigned int i = 0;
    inPool->pool_header.item_size = inItemSize;
    inPool->pool_header.num_items = inPool->pool_header.free_items = inNumItems;
    inPool->pool_header.first_free_item_index = 0;
    // create a list of free entries. Last item points to
    // non-existent iNumItems value, but that's okay
    // because we use free_items to track empty and never
    // use it.
    while (i < inNumItems) {
        *entry_ptr = ++i;
        entry_ptr += inItemSize;
    }
}

void *poolAllocateBuffer( static_pool_t* inPool )
{
    vTaskSuspendAll();

    uint8_t *free_entry = NULL;
    /* take first item off of free list, if any */
    if (inPool->pool_header.free_items == 0) {
        /* pool is empty! */
        configASSERT(pdFALSE);
    } else {
        inPool->pool_header.free_items--;
        free_entry = (uint8_t*)&inPool->pool[0] + (inPool->pool_header.first_free_item_index * inPool->pool_header.item_size);
        inPool->pool_header.first_free_item_index = *free_entry;
    }

    xTaskResumeAll();

    return (void*)free_entry;
}

void poolFreeBuffer( static_pool_t* inPool, void* inPointer)
{
    vTaskSuspendAll();

    uint8_t *free_entry = (uint8_t*)inPointer;

    // check that the object belongs in this pool!
    int entry_index = (free_entry - &inPool->pool[0]) / inPool->pool_header.item_size;
    configASSERT((entry_index >= 0) && (entry_index < inPool->pool_header.num_items));

    inPool->pool_header.free_items++;
    configASSERT(inPool->pool_header.free_items <= inPool->pool_header.num_items);
    /* "point" entry's next at old head */
    *free_entry = inPool->pool_header.first_free_item_index;
    /* "point" head at new entry */
    inPool->pool_header.first_free_item_index = entry_index;

    xTaskResumeAll();
}