#include <stddef.h>
#include <stdio.h>
#include <unistd.h>

#define ALIGN4(s) (((((s)-1)>>2)<<2)+4)
#define BLOCK_SIZE sizeof(struct block)

struct block
{
	size_t size;
	struct block* next;
	int free;
};

struct block* freeList = NULL;

struct block* extend_heap(struct block* last, size_t s)
{
	struct block* b = sbrk(0);

	if (sbrk(BLOCK_SIZE + s) == (void*)-1)
		return NULL;

	b->size = s;
	b->next = NULL;
	b->free = 0;

	if (last)
		last->next = b;

	return b;
}

struct block* find_first(size_t s)
{
	struct block* current = freeList;
	while (current && (!current->free || current->size < s))
		current = current->next;

	return current;
}

struct block* find_last()
{
	struct block* current = freeList;
	while (current && current->next)
		current = freeList->next;

	return current;
}

void* malloc(size_t size)
{
	struct block* b;

	size_t alignedSize = ALIGN4(size);

	if (freeList)
	{
		b = find_first(alignedSize);
		if (!b)
			b = extend_heap(find_last(), alignedSize);
	}
	else
	{
		b = extend_heap(NULL, alignedSize);
		if (!b)
			return NULL;

		freeList = b;
	}

	return (char*)b + BLOCK_SIZE;
}

void free(void* ptr)
{
	if (!ptr)
		return;

	struct block* b = (struct block*)((char*)ptr - BLOCK_SIZE);
	b->free = 1;

	struct block* link = b->next;
	while (b->next && b->next->free)
	{
		b->size += BLOCK_SIZE + b->next->size;
		b->next = link->next;
	}

	if (!b->next)
		brk(b);
}

int main()
{
	int* a = (int*)malloc(sizeof(int));
	int* b = (int*)malloc(sizeof(int));

	*a = 5;
	*b = 12;

	printf("Test 1: %i\n", *a);
	printf("Test 2: %i", *b);

	free(b);
	free(a);

	return 0;
}