/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
 * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
 *
 * THE BSD LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/

#ifndef OPENCV_FLANN_ALLOCATOR_H_
#define OPENCV_FLANN_ALLOCATOR_H_

#include <stdlib.h>
#include <stdio.h>


namespace cvflann
{

/**
 * Allocates (using C's malloc) a generic type T.
 *
 * Params:
 *     count = number of instances to allocate.
 * Returns: pointer (of type T*) to memory buffer
 */
template <typename T>
T* allocate(size_t count = 1)
{
    T* mem = (T*) ::malloc(sizeof(T)*count);
    return mem;
}


/**
 * Pooled storage allocator
 *
 * The following routines allow for the efficient allocation of storage in
 * small chunks from a specified pool.  Rather than allowing each structure
 * to be freed individually, an entire pool of storage is freed at once.
 * This method has two advantages over just using malloc() and free().  First,
 * it is far more efficient for allocating small objects, as there is
 * no overhead for remembering all the information needed to free each
 * object or consolidating fragmented memory.  Second, the decision about
 * how long to keep an object is made at the time of allocation, and there
 * is no need to track down all the objects to free them.
 *
 */

const size_t     WORDSIZE=16;
const  size_t     BLOCKSIZE=8192;

class PooledAllocator
{
    /* We maintain memory alignment to word boundaries by requiring that all
        allocations be in multiples of the machine wordsize.  */
    /* Size of machine word in bytes.  Must be power of 2. */
    /* Minimum number of bytes requested at a time from	the system.  Must be multiple of WORDSIZE. */


    int     remaining;  /* Number of bytes left in current block of storage. */
    void*   base;     /* Pointer to base of current block of storage. */
    void*   loc;      /* Current location in block to next allocate memory. */
    int     blocksize;


public:
    int     usedMemory;
    int     wastedMemory;

    /**
        Default constructor. Initializes a new pool.
     */
    PooledAllocator(int blockSize = BLOCKSIZE)
    {
        blocksize = blockSize;
        remaining = 0;
        base = NULL;

        usedMemory = 0;
        wastedMemory = 0;
    }

    /**
     * Destructor. Frees all the memory allocated in this pool.
     */
    ~PooledAllocator()
    {
        void* prev;

        while (base != NULL) {
            prev = *((void**) base); /* Get pointer to prev block. */
            ::free(base);
            base = prev;
        }
    }

    /**
     * Returns a pointer to a piece of new memory of the given size in bytes
     * allocated from the pool.
     */
    void* allocateMemory(int size)
    {
        int blockSize;

        /* Round size up to a multiple of wordsize.  The following expression
            only works for WORDSIZE that is a power of 2, by masking last bits of
            incremented size to zero.
         */
        size = (size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);

        /* Check whether a new block must be allocated.  Note that the first word
            of a block is reserved for a pointer to the previous block.
         */
        if (size > remaining) {

            wastedMemory += remaining;

            /* Allocate new storage. */
            blockSize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
                        size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;

            // use the standard C malloc to allocate memory
            void* m = ::malloc(blockSize);
            if (!m) {
                fprintf(stderr,"Failed to allocate memory.\n");
                return NULL;
            }

            /* Fill first word of new block with pointer to previous block. */
            ((void**) m)[0] = base;
            base = m;

            int shift = 0;
            //int shift = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);

            remaining = blockSize - sizeof(void*) - shift;
            loc = ((char*)m + sizeof(void*) + shift);
        }
        void* rloc = loc;
        loc = (char*)loc + size;
        remaining -= size;

        usedMemory += size;

        return rloc;
    }

    /**
     * Allocates (using this pool) a generic type T.
     *
     * Params:
     *     count = number of instances to allocate.
     * Returns: pointer (of type T*) to memory buffer
     */
    template <typename T>
    T* allocate(size_t count = 1)
    {
        T* mem = (T*) this->allocateMemory((int)(sizeof(T)*count));
        return mem;
    }

};

}

#endif //OPENCV_FLANN_ALLOCATOR_H_