#include "stdlib.h"
#include "stdio.h"

#include "macro.h"

#define SIZE_HDR ALIGN_VALUE_UP(sizeof(struct mem_block), 4)

struct mem_block
{
  uint32_t* addr;
  size_t size;

  struct mem_block* prev;
  struct mem_block* next;
};

struct mem_block *free_block = NULL;
struct mem_block *alloc_block = NULL;

uint32_t *heap_end = NULL;

void mem_init(uint32_t* addr_heap_start)
{
   heap_end = addr_heap_start;
}

void *mem_get_block(size_t size)
{
  if (heap_end == NULL)
  {
    printk("memory allocator is not initialized\n");
    return (void *)NULL;
  }

  if (size == 0)
    return (void *)NULL;

  volatile uint32_t *block_addr = (volatile uint32_t*)heap_end;
  size_t size_hdr = SIZE_HDR; 
  size_t new_size = ALIGN_VALUE_UP(size_hdr + size, 4);

  struct mem_block* new_block;

  if (free_block == NULL)
    goto newblock;

  struct mem_block* itr = free_block;
  do
  {
    if (itr->size > size)
    {
      if (itr->prev == NULL)
      {
        free_block = itr->next;
        if (itr->next != NULL)
          itr->next->prev = NULL;
      }
      else
      {
        itr->prev->next = itr->next;
        if (itr->next != NULL)
          itr->next->prev = itr->prev;
      }

      if (alloc_block == NULL)
      {
        alloc_block = itr;
        itr->next = NULL;
      }
      else
      {
        alloc_block->prev = itr;
        itr->next = alloc_block;
        itr->prev = NULL;
        alloc_block = itr;
      }

      return (void*)itr->addr;
    }
  }
  while ((itr = itr->next) != NULL);

newblock:
  new_block = (struct mem_block *)block_addr;
  new_block->size = new_size - (ALIGN_VALUE_UP(sizeof(struct mem_block),4));
  new_block->addr = (uint32_t *)block_addr + (uint32_t)(SIZE_HDR)/sizeof(uint32_t);
  new_block->prev = 0;
  new_block->next = alloc_block;

  if (alloc_block != NULL)
    alloc_block->prev = new_block;

  alloc_block = new_block;

  heap_end = (uint32_t *)alloc_block + new_size/sizeof(uint32_t);

  return new_block->addr;
}

void mem_free_block(void *ptr)
{
  if (ptr == NULL)
    return;

  uint32_t *mem_block_addr = (uint32_t*)ptr - SIZE_HDR/sizeof(uint32_t);
  struct mem_block *block = (struct mem_block*)mem_block_addr;

  if (block->next != NULL)
    block->next->prev = block->prev;

  if (block->prev != NULL)
    block->prev->next = block->next;

  if (free_block != NULL)
  {
    free_block->prev = block;
    block->next = free_block;
    block->prev = NULL;
  }
  else
  {
    free_block = block;
    block->next = NULL;
    block->prev = NULL;
  }
}
