//
// Created by epagris on 2022.10.20..
//

#include <stdbool.h>
#include "memory_pool.h"
#include "utils.h"

MP *mp_init(uint8_t *p, uint32_t size) {
    ASSERT_BAD_ALIGN(p); // check for alignment
    MP *pool = (MP *) p; // fill in properties
    pool->p = p + sizeof(MP); // compute beginning of the allocatable area
    pool->poolSize = size; // save total pool size
    pool->blockRegistry = (MPAllocRecord *) ALIGN((p + size - sizeof(MPAllocRecord)), uint32_t); // determine block registry address
    pool->freeSpace = ((uint8_t *) pool->blockRegistry) - pool->p - sizeof(MPAllocRecord); // calculate free space size

    // place sentry element
    pool->blockRegistry[-1].addrStart = 0;
    pool->blockRegistry[-1].size = 0;
    pool->blockRegistry[-1].type = MPRT_SENTRY;

    // place element record holding info on the single, undivided, unallocated block
    pool->blockRegistry[0].addrStart = pool->p;
    pool->blockRegistry[0].size = pool->freeSpace;
    pool->blockRegistry[0].type = MPRT_FREE;

    pool->blockRecCnt = 2; // sentry and free block

    return pool;
}

uint8_t *mp_alloc(MP *mp, uint32_t size) {
    // make the allocation from the beginning of the smallest suitable (large enough)
    // contiguous block

    // round size to make it divisible by 4
    size = ALIGN(size, uint32_t);

    // badness = area left free on the candidate free block after allocating the requested block
    MPAllocRecord *bestBlock = NULL;
    uint32_t leastBadness = ~0;
    MPAllocRecord *recIter = mp->blockRegistry; // allocation record
    while (recIter->type != MPRT_SENTRY && leastBadness > 0) { // at badness = 0 just break, since it's a perfectly fitting block
        // look for suitable free block
        if (recIter->type == MPRT_FREE && recIter->size > size) {
            uint32_t iterBadness = recIter->size - size;
            if (iterBadness < leastBadness) { // calculate badness
                bestBlock = recIter;
                leastBadness = recIter->size - size;
            }
        }
        recIter--; // step to next block
    }

    // allocate block
    uint8_t * ptr = NULL;
    if (leastBadness == 0) { // just change block registry class if block perfectly fits
        bestBlock->type = MPRT_ALLOCATED;
        ptr = bestBlock->addrStart;
    } else { // if there are some bytes left between allocated blocks
        // shift the registry below best block
        MPAllocRecord *rec = mp->blockRegistry - (mp->blockRecCnt + 1); // bottom of the registry
        while (rec != bestBlock) {
            *(rec) = *(rec + 1);
            rec++;
        }

        // store information on allocated
        MPAllocRecord *allocated = bestBlock - 1;
        allocated->type = MPRT_ALLOCATED;
        allocated->size = size;
        allocated->addrStart = bestBlock->addrStart;
        ptr = allocated->addrStart;

        // shrink the remaining free block size
        bestBlock->size -= size;
        bestBlock->addrStart += size;

        // decrease free size with the increase in the registry size
        mp->freeSpace -= sizeof(MPAllocRecord);

        // increase record count
        mp->blockRecCnt++;
    }

    // decrease free space size with the allocated block size
    mp->freeSpace -= size;

    return ptr;
}

// join adjacent free blocks
static void mp_join_free_blocks(MP *mp) {
    MPAllocRecord *recIter = mp->blockRegistry;
    while (recIter->type != MPRT_SENTRY) {
        if (recIter->type == MPRT_FREE && (recIter - 1)->type == MPRT_FREE) { // if two adjacent free blocks have been found...
            // join the blocks
            recIter->size += (recIter - 1)->size;
            recIter->addrStart = (recIter - 1)->addrStart;

            // shift block below joined blocks one upper
            MPAllocRecord *joinIter = (recIter - 1);
            while (joinIter->type != MPRT_SENTRY) {
                *joinIter = *(joinIter - 1);
                joinIter--;
            }
            mp->freeSpace += sizeof(MPAllocRecord);
            mp->blockRecCnt--;
        } else {
            recIter--;
        }
    }
}

void mp_free(MP *mp, const uint8_t *p) {
    // look for registry record
    bool success = false;
    MPAllocRecord *recIter = mp->blockRegistry;
    while (recIter->type != MPRT_SENTRY) {
        if ((recIter->type == MPRT_ALLOCATED) &&
            ((recIter->addrStart <= p) && ((recIter->addrStart + recIter->size) > p))) { // ...block found
            recIter->type = MPRT_FREE;
            mp->freeSpace += recIter->size;
            success = true;
            break;
        }
        recIter--;
    }

    if (success) {
        mp_join_free_blocks(mp);
    }
}

void mp_report(MP *mp) {
    INFO("#     TYPE    BEGIN           SIZE\n");
    MPAllocRecord *recIter = mp->blockRegistry;
    uint32_t bi = 0;
    while (recIter->type != MPRT_SENTRY) {
        INFO("%05u %s %p  %u\n", bi, recIter->type == MPRT_ALLOCATED ? "ALLOC  " : "FREE   ",
             recIter->addrStart, recIter->size);
        recIter--;
        bi++;
    }
    INFO("----------------------\n");
    INFO("Used: %u (mgmt: %u)\nFree: %u of %u\n\n", (mp->poolSize - mp->freeSpace),
         (mp->blockRecCnt + 1) * sizeof(MPAllocRecord), mp->freeSpace, mp->poolSize);
}