/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include <Box2D/Common/b2BlockAllocator.h>
#include <cstdlib>
#include <climits>
#include <cstring>
#include <memory>

using namespace std;

int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = 
{
    16,        // 0
    32,        // 1
    64,        // 2
    96,        // 3
    128,    // 4
    160,    // 5
    192,    // 6
    224,    // 7
    256,    // 8
    320,    // 9
    384,    // 10
    448,    // 11
    512,    // 12
    640,    // 13
};
uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1];
bool b2BlockAllocator::s_blockSizeLookupInitialized;

struct b2Chunk
{
    int32 blockSize;
    b2Block* blocks;
};

struct b2Block
{
    b2Block* next;
};

b2BlockAllocator::b2BlockAllocator()
{
    b2Assert(b2_blockSizes < UCHAR_MAX);

    m_chunkSpace = b2_chunkArrayIncrement;
    m_chunkCount = 0;
    m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
    
    memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
    memset(m_freeLists, 0, sizeof(m_freeLists));

    if (s_blockSizeLookupInitialized == false)
    {
        int32 j = 0;
        for (int32 i = 1; i <= b2_maxBlockSize; ++i)
        {
            b2Assert(j < b2_blockSizes);
            if (i <= s_blockSizes[j])
            {
                s_blockSizeLookup[i] = (uint8)j;
            }
            else
            {
                ++j;
                s_blockSizeLookup[i] = (uint8)j;
            }
        }

        s_blockSizeLookupInitialized = true;
    }
}

b2BlockAllocator::~b2BlockAllocator()
{
    for (int32 i = 0; i < m_chunkCount; ++i)
    {
        b2Free(m_chunks[i].blocks);
    }

    b2Free(m_chunks);
}

void* b2BlockAllocator::Allocate(int32 size)
{
    if (size == 0)
        return NULL;

    b2Assert(0 < size);

    if (size > b2_maxBlockSize)
    {
        return b2Alloc(size);
    }

    int32 index = s_blockSizeLookup[size];
    b2Assert(0 <= index && index < b2_blockSizes);

    if (m_freeLists[index])
    {
        b2Block* block = m_freeLists[index];
        m_freeLists[index] = block->next;
        return block;
    }
    else
    {
        if (m_chunkCount == m_chunkSpace)
        {
            b2Chunk* oldChunks = m_chunks;
            m_chunkSpace += b2_chunkArrayIncrement;
            m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
            memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));
            memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));
            b2Free(oldChunks);
        }

        b2Chunk* chunk = m_chunks + m_chunkCount;
        chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);
#if defined(_DEBUG)
        memset(chunk->blocks, 0xcd, b2_chunkSize);
#endif
        int32 blockSize = s_blockSizes[index];
        chunk->blockSize = blockSize;
        int32 blockCount = b2_chunkSize / blockSize;
        b2Assert(blockCount * blockSize <= b2_chunkSize);
        for (int32 i = 0; i < blockCount - 1; ++i)
        {
            b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);
            b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));
            block->next = next;
        }
        b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
        last->next = NULL;

        m_freeLists[index] = chunk->blocks->next;
        ++m_chunkCount;

        return chunk->blocks;
    }
}

void b2BlockAllocator::Free(void* p, int32 size)
{
    if (size == 0)
    {
        return;
    }

    b2Assert(0 < size);

    if (size > b2_maxBlockSize)
    {
        b2Free(p);
        return;
    }

    int32 index = s_blockSizeLookup[size];
    b2Assert(0 <= index && index < b2_blockSizes);

#ifdef _DEBUG
    // Verify the memory address and size is valid.
    int32 blockSize = s_blockSizes[index];
    bool found = false;
    for (int32 i = 0; i < m_chunkCount; ++i)
    {
        b2Chunk* chunk = m_chunks + i;
        if (chunk->blockSize != blockSize)
        {
            b2Assert(    (int8*)p + blockSize <= (int8*)chunk->blocks ||
                        (int8*)chunk->blocks + b2_chunkSize <= (int8*)p);
        }
        else
        {
            if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize)
            {
                found = true;
            }
        }
    }

    b2Assert(found);

    memset(p, 0xfd, blockSize);
#endif

    b2Block* block = (b2Block*)p;
    block->next = m_freeLists[index];
    m_freeLists[index] = block;
}

void b2BlockAllocator::Clear()
{
    for (int32 i = 0; i < m_chunkCount; ++i)
    {
        b2Free(m_chunks[i].blocks);
    }

    m_chunkCount = 0;
    memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));

    memset(m_freeLists, 0, sizeof(m_freeLists));
}