#include "bitmap.h"

#define BITMAP_MASK_FULL (~((bmcell_t)0))

#define BITMAP_CELL_SIZE (sizeof(bmcell_t) * 8)
#define BITMAP_CELL_SHIFT 6
#define BITMAP_SUBCELL_SHIFT 3

#define BITMAP_TEST(map, i) (map[i >> BITMAP_CELL_SHIFT] & (((bmcell_t)1) << i))
#define BITMAP_SET(map, i)  (map[i >> BITMAP_CELL_SHIFT] |= (((bmcell_t)1) << i))
#define BITMAP_USET(map, i) (map[i >> BITMAP_CELL_SHIFT] &= ~(((bmcell_t)1) << i))

void
bmvl_setup(struct bitmap_vl *bm, size_t bits)
{
    bm->head.length = bm->head.bits_free = bits;
    bm->head.last_free = 0;
    for(size_t i = 0; i < (bits / BITMAP_BITS_PER_CELL); i++) {
        bm->cells[i] = 0;
    }
}

bool 
__bm_test(bmcell_t *cells, size_t i)
{
    return BITMAP_TEST(cells, i) != 0;
}

bm_signed_t
__bm_take(struct bitmap_head *head, bmcell_t *cells)
{
    /*Definitely OOM*/
    if(head->bits_free == 0) return -1;
    /*If last_free is already free, we can just return that*/
    if(BITMAP_TEST(cells, head->last_free) == 0) {
        BITMAP_SET(cells, head->last_free);
        return head->last_free++;
    }

    /*Search all cells after last_free if last_free was taken. 
     * It could mean we're OOM, or that we've missed a gap somewhere*/
    for(size_t cell = (head->last_free >> BITMAP_CELL_SHIFT);
            cell < (head->length >> BITMAP_CELL_SHIFT);
            cell++)
    {
        /*A full bitmask means that every bit in the cell is taken*/
        if(cells[cell] == BITMAP_MASK_FULL) continue;

        /*We sub-divide the search into bytes, so that we can take advantage
         * of mask searches*/
        uint8_t *subcells = (uint8_t*)(&cells[cell]);
        for(uint8_t subcell = 0; subcell < 8; subcell++) {
            /*If the byte is full, we search the next byte*/
            if(subcells[subcell] == 0xFF) continue;
            /*A per-bit search is necessary to find the exact free bit*/
            for(uint8_t bit = 0; bit < 8; bit++) {
                if(subcells[subcell] & (1 << bit)) continue;
                /*Mark the bit as taken*/
                subcells[subcell] |= ((bmcell_t)1 << bit);

                /*Because we're operating on pow2, we can use bitshifts to
                 * simplify 2^x.
                 * Since subcell and bit can only be 1-7, we can OR it into
                 * the shifted cell value*/
                head->last_free =
                    (cell << BITMAP_CELL_SHIFT) |
                    (subcell << BITMAP_SUBCELL_SHIFT) |
                    bit;
                /*This returns the last_free, then increments so that the next
                 * call can (possibly) get a free value*/
                head->bits_free--;
                return head->last_free++;
            }
        }
    }
    /*Getting here means the bitmap couldn't find a free bit and we are
     * presumably OOM*/
    return -1;
}

bm_signed_t
__bm_take_contig(struct bitmap_head *head, bmcell_t *cells, size_t l)
{
    /*If the bitmap doesn't have enough free bits for the span, it will never
     * have enough contiguous bits*/
    if(head->bits_free < l) return -1;
    /*Simul completion flag and utility value for calculating offsets for return
     * value*/
    size_t left = l;
    for(size_t cell = (head->last_free >> BITMAP_CELL_SHIFT);
            cell < (head->length >> BITMAP_CELL_SHIFT);
            cell++)
    {
        /*Return case for breakout when we find the contig span*/
        if(left == 0) break;
        /*If the bitmask is full, there won't be room to fit the requested span*/
        if(cells[cell] == BITMAP_MASK_FULL) {
            left = l;
            continue;
        }
        /*If the bitmask is 0, there are atleast BITMAP_CELL_SIZE contig bits
         * in the bitmap starting here*/
        if(cells[cell] == 0) {
            /*Only continue the loop if there aren't enough bits in this current
             * cell.*/
            if(left > BITMAP_CELL_SIZE) {
                left -= BITMAP_CELL_SIZE;
                continue;
            }
            /*Because there was room for the rest of the span, we can return
             * this address without worrying about fetching other bits.
             * The cell index is subtracted by the inverse of left*/
            head->last_free = 
                (cell << BITMAP_CELL_SHIFT) - (l - left);
            /*Mark left so that we break out of this loop and call the proper
             * return function*/
            left = 0;
            break;
        }
        /*Divide search into bytes for faster search of bmcell_t structure*/
        uint8_t *subcells = (uint8_t*)(&cells[cell]);
        for(uint8_t subcell = 0; subcell < 8; subcell++) {
            if(left == 0) break;
            /*Subcell is full, we refill left and stop searching this subcell*/
            if(subcells[subcell] == 0xFF) {
                left = l;
                continue;
            }
            /*Since the subcell is empty, there are atleast 8 contig bits*/
            if(subcells[subcell] == 0) {
                /*If we need more than 8 bits, we subtract 8 from left and
                 * continue*/
                if(left > 8) {
                    left -= 8;
                    continue;
                }
                /*Otherwise, we return the index of this particular subcell
                 * minus the inverse of left*/
                head->last_free = 
                    ((cell << BITMAP_CELL_SHIFT) |
                     (subcell << BITMAP_SUBCELL_SHIFT)) -
                    (l - left);
                /*Mark left for proper exit and return*/
                left = 0;
                break;
            }
            /*Search the entire subcell for free bits.*/
            for(uint8_t bit = 0; bit < 8; bit++) {
                /*When we find a taken bit, and there are still bits to fetch,
                 * then the block will never be contiguous and we need to
                 * start again*/
                if(subcells[subcell] & (1 << bit)) {
                    left = l;
                    continue;
                }
                left--;
                if(left == 0) {
                    /*When we have found all the bits we need, we return the
                     * index to the start of the contig span*/
                    head->last_free =
                        ((cell << BITMAP_CELL_SHIFT) |
                         (subcell << BITMAP_SUBCELL_SHIFT) |
                         bit) -
                        (l - left);
                    break;
                }
            }
        }
    }
    /*We didn't find a contiguous span. Either the bitmap is fragmented or
     * there's not enough free space to fit the span*/
    if(left != 0) return -1;

    /*Mark all of the bits in the span as taken*/
    for(size_t i = 0; i < l; i++) {
        BITMAP_SET(cells, (head->last_free + i));
    }
    /*Setup to return last_free, while incrementing last_free by l*/
    head->last_free += l;
    head->bits_free -= l;
    return head->last_free - l;
    
}

void
__bm_reserve(struct bitmap_head *head, bmcell_t *cells, size_t i)
{
    /*Take the bit at i and update the bits_free counter*/
    BITMAP_SET(cells, i);
    head->bits_free--;
}

void
__bm_reserve_contig(
        struct bitmap_head *head, 
        bmcell_t *cells, 
        size_t i, 
        size_t l)
{
    /*For every bit between i and i+l, mark as reserved*/
    head->bits_free -= l;
    for(; l != 0; l--, i++) {
        BITMAP_SET(cells, i);
    }
}

void
__bm_release(struct bitmap_head *head, bmcell_t *cells, size_t i)
{
    /*Release the bit at i, update last_free, and update the bits_free counter*/
    BITMAP_USET(cells, i);
    if(head->last_free > i) head->last_free = i;
    head->bits_free++;
}

void 
__bm_release_contig(
        struct bitmap_head *head, 
        bmcell_t *cells, 
        size_t i, 
        size_t l)
{
    /*Update the last_free and bits_free counters*/
    head->bits_free += i;
    if(head->last_free > i) head->last_free = i;
    /*For every bit between i and i+l, mark as free*/
    for(; l != 0; l--, i++) {
        BITMAP_USET(cells, i);
    }
}