#include "heap4_common.h"

#define HEAP_BYTES 192U
#define MIN_SPLIT_SIZE ((size_t)(sizeof(HeapBlock) * 2U))

static uint8_t heap_area[HEAP_BYTES + HEAP4_ALIGNMENT];
static HeapBlock start;
static HeapBlock *end_marker;

volatile size_t g_allocated_size;
volatile size_t g_split_free_size;
volatile int g_free_nodes;

static void heap_init_one_block(void)
{
    uintptr_t aligned;
    HeapBlock *first;

    aligned = heap4_align_address((uintptr_t)&heap_area[0]);
    first = (HeapBlock *)aligned;
    end_marker = (HeapBlock *)(aligned + 176U);
    end_marker->next = 0;
    end_marker->size = 0U;
    first->next = end_marker;
    first->size = (uint8_t *)end_marker - (uint8_t *)first;
    start.next = first;
    start.size = 0U;
}

static void *heap_malloc_split(size_t wanted)
{
    HeapBlock *previous;
    HeapBlock *block;
    HeapBlock *remainder;
    size_t total;
    size_t original;

    total = heap4_align_up(wanted + sizeof(HeapBlock));
    previous = &start;
    block = start.next;

    while (block != end_marker) {
        if (block->size >= total) {
            original = block->size;
            if (original - total > MIN_SPLIT_SIZE) {
                remainder = (HeapBlock *)((uint8_t *)block + total);
                remainder->size = original - total;
                remainder->next = block->next;
                previous->next = remainder;
                block->size = total;
            } else {
                previous->next = block->next;
            }
            block->next = 0;
            block->size = heap4_mark_allocated(block->size);
            return (uint8_t *)block + sizeof(HeapBlock);
        }
        previous = block;
        block = block->next;
    }

    return 0;
}

static int count_free_nodes(void)
{
    HeapBlock *block;
    int count;

    count = 0;
    for (block = start.next; block != end_marker; block = block->next)
        count++;
    return count;
}

int main(void)
{
    void *p;
    HeapBlock *allocated;

    heap_init_one_block();
    p = heap_malloc_split(32U);
    allocated = (HeapBlock *)((uint8_t *)p - sizeof(HeapBlock));

    g_allocated_size = heap4_clear_allocated(allocated->size);
    g_split_free_size = start.next->size;
    g_free_nodes = count_free_nodes();

    TRACE_PRINTF("allocated=%u split=%u nodes=%d\n",
                 (unsigned)g_allocated_size, (unsigned)g_split_free_size,
                 g_free_nodes);
    return 0;
}