#include <euler/syscall.hpp>
#include <euler/heap.hpp>
#include <cstdint>

namespace euler::heap {

  struct heap_aligned_entry {
    size_t start_vaddr;
    size_t length;
    heap_aligned_entry *prev;
    heap_aligned_entry *next;
  };

  struct unused_heap_entries_page {
    heap_aligned_entry *unused[510];
    unused_heap_entries_page *prev;
    uint16_t unused_in_this_page;
  };

  heap_aligned_entry *first_entry = 0;
  heap_aligned_entry *last_entry = 0;
  unused_heap_entries_page *last_page = 0;

  void mark_entry_unused(heap_aligned_entry *entry) {

    for (unused_heap_entries_page *page = last_page; page != 0; page = page->prev) {
      if (page->unused_in_this_page == 510)
        continue;
      for (uint16_t i = 0; i < 510; ++i)
        if (page->unused[i] == 0) {
          page->unused[i] = entry;
          ++page->unused_in_this_page;
          return;
        }
    }

    unused_heap_entries_page *page = (unused_heap_entries_page *)euler::syscall::get_new_pages(1);
    page->prev = last_page;
    last_page = page;

    page->unused_in_this_page = 1;
    page->unused[0] = entry;

    for (uint16_t i = 1; i < 510; ++i)
      page->unused[i] = 0;

  }

  void remove_entry(heap_aligned_entry *entry) {

    if (entry->prev)
      entry->prev->next = entry->next;
    else
      first_entry = entry->next;
    if (entry->next)
      entry->next->prev = entry->prev;
    else
      last_entry = entry->prev;

    mark_entry_unused(entry);

  }

  heap_aligned_entry *get_new_entry() {

    for (unused_heap_entries_page *page = last_page; page != 0; page = page->prev) {
      if (page->unused_in_this_page == 0)
        continue;
      for (uint16_t i = 0; i < 510; ++i)
        if (page->unused[i] != 0) {
          heap_aligned_entry *entry = page->unused[i];
          page->unused[i] = 0;
          --page->unused_in_this_page;
          return entry;
        }
    }

    heap_aligned_entry *new_entries = (heap_aligned_entry *)euler::syscall::get_new_pages(1);

    for (uint8_t i = 1; i < 128; ++i)
      mark_entry_unused(new_entries + i);

    return new_entries;

  }

  void *get_block(size_t length) {

    for (heap_aligned_entry *entry = first_entry; entry != 0; entry = entry->next) {
      if (entry->length == length) {
        void *start = (void *)entry->start_vaddr;
        remove_entry(entry);
        return start;
      }
      if (entry->length > length) {
        void *start = (void *)entry->start_vaddr;
        entry->start_vaddr += length;
        entry->length -= length;
        return start;
      }
    }

    size_t pages = (length - 1) / 4096 + 1;
    void *new_memory = euler::syscall::get_new_pages(pages);
    if (pages * 4096 != length)
      return_block((uint8_t *)new_memory + length, pages * 4096 - length);
    return new_memory;

  }

  void return_block(void *start, size_t length) {

    heap_aligned_entry *after = first_entry;
    while (after != 0 && after->start_vaddr < (size_t)start + length)
      after = after->next;

    heap_aligned_entry *before;
    if (after == 0)
      before = last_entry;
    else
      before = after->prev;

    heap_aligned_entry *us;

    if (after && after->start_vaddr == (size_t)start + length) {
      after->start_vaddr -= length;
      after->length += length;
      us = after;
    }

    else {
      us = get_new_entry();
      us->prev = before;
      us->next = after;
      us->start_vaddr = (size_t)start;
      us->length = length;
      if (before)
        before->next = us;
      else
        first_entry = us;
      if (after)
        after->prev = us;
      else
        last_entry = us;
    }

    if (before && before->start_vaddr + before->length == (size_t)start) {
      us->start_vaddr -= before->length;
      us->length += before->length;
      remove_entry(before);
    }

  }

}