#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include "ik/ordered_vector.h"
#include "ik/memory.h"

/* ----------------------------------------------------------------------------
 * Static functions
 * ------------------------------------------------------------------------- */

/*!
 * @brief Expands the underlying memory.
 *
 * This implementation will expand the memory by a factor of 2 each time this
 * is called. All elements are copied into the new section of memory.
 * @param[in] insertion_index Set to -1 if (no space should be made for element
 * insertion. Otherwise this parameter specifies the index of the element to
 * "evade" when re-allocating all other elements.
 * @param[in] target_size If set to 0, target size is calculated automatically.
 * Otherwise the vector will expand to the specified target size.
 * @note No checks are performed to make sure the target size is large enough.
 */
static int
ordered_vector_expand(ordered_vector_t *vector,
                      uintptr_t insertion_index,
                      uint32_t target_size);

/* ----------------------------------------------------------------------------
 * Exported functions
 * ------------------------------------------------------------------------- */
ordered_vector_t*
ordered_vector_create(const uint32_t element_size)
{
    ordered_vector_t* vector;
    if (!(vector = (ordered_vector_t*)MALLOC(sizeof(ordered_vector_t))))
        return NULL;
    ordered_vector_construct(vector, element_size);
    return vector;
}

/* ------------------------------------------------------------------------- */
void
ordered_vector_construct(ordered_vector_t* vector, const uint32_t element_size)
{
    assert(vector);
    memset(vector, 0, sizeof(ordered_vector_t));
    vector->element_size = element_size;
}

/* ------------------------------------------------------------------------- */
void
ordered_vector_destroy(ordered_vector_t* vector)
{
    assert(vector);
    ordered_vector_clear_free(vector);
    FREE(vector);
}

/* ------------------------------------------------------------------------- */
void
ordered_vector_clear(ordered_vector_t* vector)
{
    assert(vector);
    /*
     * No need to free or overwrite existing memory, just reset the counter
     * and let future insertions overwrite
     */
    vector->count = 0;
}

/* ------------------------------------------------------------------------- */
void
ordered_vector_clear_free(ordered_vector_t* vector)
{
    assert(vector);

    if (vector->data)
        FREE(vector->data);

    vector->data = NULL;
    vector->count = 0;
    vector->capacity = 0;
}

/* ------------------------------------------------------------------------- */
int
ordered_vector_resize(ordered_vector_t* vector, uint32_t size)
{
    int result = 0;

    assert(vector);

    if (vector->count < size)
        result = ordered_vector_expand(vector, -1, size);
    vector->count = size;

    return result;
}

/* ------------------------------------------------------------------------- */
void*
ordered_vector_push_emplace(ordered_vector_t* vector)
{
    void* data;

    assert(vector);

    if (vector->count == vector->capacity)
        if (ordered_vector_expand(vector, -1, 0) < 0)
            return NULL;
    data = vector->data + (vector->element_size * vector->count);
    ++(vector->count);
    return data;
}

/* ------------------------------------------------------------------------- */
int
ordered_vector_push(ordered_vector_t* vector, void* data)
{
    void* emplaced;

    assert(vector);
    assert(data);

    emplaced = ordered_vector_push_emplace(vector);
    if (!emplaced)
        return -1;
    memcpy(emplaced, data, vector->element_size);
    return 0;
}

/* ------------------------------------------------------------------------- */
int
ordered_vector_push_vector(ordered_vector_t* vector, ordered_vector_t* source_vector)
{
    assert(vector);
    assert(source_vector);

    /* make sure element sizes are equal */
    if (vector->element_size != source_vector->element_size)
        return -1;

    /* make sure there's enough space in the target vector */
    if (vector->count + source_vector->count > vector->capacity)
        if (ordered_vector_expand(vector, -1, vector->count + source_vector->count) < 0)
            return -1;

    /* copy data */
    memcpy(vector->data + (vector->count * vector->element_size),
           source_vector->data,
           source_vector->count * vector->element_size);
    vector->count += source_vector->count;

    return 0;
}

/* ------------------------------------------------------------------------- */
void*
ordered_vector_pop(ordered_vector_t* vector)
{
    assert(vector);

    if (!vector->count)
        return NULL;

    --(vector->count);
    return vector->data + (vector->element_size * vector->count);
}

/* ------------------------------------------------------------------------- */
void*
ordered_vector_back(const ordered_vector_t* vector)
{
    assert(vector);

    if (!vector->count)
        return NULL;

    return vector->data + (vector->element_size * (vector->count - 1));
}

/* ------------------------------------------------------------------------- */
void*
ordered_vector_insert_emplace(ordered_vector_t* vector, uint32_t index)
{
    uint32_t offset;

    assert(vector);

    /*
     * Normally the last valid index is (capacity-1), but in this case it's valid
     * because it's possible the user will want to insert at the very end of
     * the vector.
     */
    if (index > vector->count)
        return NULL;

    /* re-allocate? */
    if (vector->count == vector->capacity)
    {
        if (ordered_vector_expand(vector, index, 0) < 0)
            return NULL;
    }
    else
    {
        /* shift all elements up by one to make space for insertion */
        uint32_t total_size = vector->count * vector->element_size;
        offset = vector->element_size * index;
        memmove((void*)((intptr_t)vector->data + offset + vector->element_size),
                (void*)((intptr_t)vector->data + offset),
                total_size - offset);
    }

    /* return pointer to memory of new element */
    ++vector->count;
    return (void*)(vector->data + index * vector->element_size);
}

/* ------------------------------------------------------------------------- */
int
ordered_vector_insert(ordered_vector_t* vector, uint32_t index, void* data)
{
    void* emplaced;

    assert(vector);
    assert(data);

    emplaced = ordered_vector_insert_emplace(vector, index);
    if (!emplaced)
        return -1;
    memcpy(emplaced, data, vector->element_size);
    return 0;
}

/* ------------------------------------------------------------------------- */
void
ordered_vector_erase_index(ordered_vector_t* vector, uint32_t index)
{
    assert(vector);

    if (index >= vector->count)
        return;

    if (index == vector->count - 1)
        /* last element doesn't require memory shifting, just pop it */
        ordered_vector_pop(vector);
    else
    {
        /* shift memory right after the specified element down by one element */
        uint32_t offset = vector->element_size * index;  /* offset to the element being erased in bytes */
        uint32_t total_size = vector->element_size * vector->count; /* total current size in bytes */
        memmove((void*)((intptr_t)vector->data + offset),   /* target is to overwrite the element specified by index */
                (void*)((intptr_t)vector->data + offset + vector->element_size),    /* copy beginning from one element ahead of element to be erased */
                total_size - offset - vector->element_size);     /* copying number of elements after element to be erased */
        --vector->count;
    }
}

/* ------------------------------------------------------------------------- */
void
ordered_vector_erase_element(ordered_vector_t* vector, void* element)
{
    uintptr_t last_element;

    assert(vector);
    last_element = (uintptr_t)vector->data + (vector->count-1) * vector->element_size;
    assert(element);
    assert((uintptr_t)element >= (uintptr_t)vector->data);
    assert((uintptr_t)element <= (uintptr_t)last_element);

    if (element != (void*)last_element)
    {
        memmove(element,    /* target is to overwrite the element */
                (void*)((uintptr_t)element + vector->element_size), /* read everything from next element */
                last_element - (uintptr_t)element);
    }
    --vector->count;
}

/* ------------------------------------------------------------------------- */
void*
ordered_vector_get_element(ordered_vector_t* vector, uint32_t index)
{
    assert(vector);

    if (index >= vector->count)
        return NULL;
    return vector->data + (vector->element_size * index);
}

/* ----------------------------------------------------------------------------
 * Static functions
 * ------------------------------------------------------------------------- */
static int
ordered_vector_expand(ordered_vector_t *vector,
                      uintptr_t insertion_index,
                      uint32_t target_count)
{
    uintptr_t new_count;
    DATA_POINTER_TYPE* old_data;
    DATA_POINTER_TYPE* new_data;

    /* expand by factor 2, or adopt target count if (it is not 0 */
    if (target_count)
        new_count = target_count;
    else
        new_count = vector->capacity << 1;

    /*
     * If vector hasn't allocated anything yet, just allocated the requested
     * amount of memory and return immediately.
     */
    if (!vector->data)
    {
        new_count = (new_count == 0 ? 2 : new_count);
        vector->data = MALLOC(new_count * vector->element_size);
        if (!vector->data)
            return -1;
        vector->capacity = new_count;
        return 0;
    }

    /* prepare for reallocating data */
    old_data = vector->data;
    new_data = (DATA_POINTER_TYPE*)MALLOC(new_count * vector->element_size);
    if (!new_data)
        return -1;

    /* if (no insertion index is required, copy all data to new memory */
    if (insertion_index == (uintptr_t)-1 || insertion_index >= new_count)
        memcpy(new_data, old_data, vector->count * vector->element_size);

    /* keep space for one element at the insertion index */
    else
    {
        /* copy old data up until right before insertion offset */
        uint32_t offset = vector->element_size * insertion_index;
        uint32_t total_size = vector->element_size * vector->count;
        memcpy(new_data, old_data, offset);
        /* copy the remaining amount of old data shifted one element ahead */
        memcpy((void*)((intptr_t)new_data + offset + vector->element_size),
               (void*)((intptr_t)old_data + offset),
               total_size - offset);
    }

    vector->data = new_data;
    vector->capacity = new_count;
    FREE(old_data);

    return 0;
}
