#include <stdlib.h>
#include <assert.h>
#include <string.h>

unsigned long long next_power_of_two(unsigned long long v);

struct dynarray {
    unsigned long long size;
    unsigned long long value_size;
    unsigned long long capacity;
    void* data;
};

struct dynarray* dynarray_new(unsigned long long value_size);
void dynarray_init(struct dynarray* array, unsigned long long value_size);
void dynarray_clear(struct dynarray* array);
void dynarray_free(struct dynarray* array);
void dynarray_push(struct dynarray* array, void* value);
void* dynarray_element_at(struct dynarray* array, unsigned long long index);
void* dynarray_element_begin(struct dynarray* array);
void* dynarray_element_end(struct dynarray* array);
void dynarray_erase(struct dynarray* array, unsigned long long index);
void dynarray_reserve(struct dynarray* array, unsigned long long new_capacity);
void dynarray_resize(struct dynarray* array, unsigned long long new_size);

unsigned long long next_power_of_two(unsigned long long v) {
    v--;
    v |= v >> 1;
    v |= v >> 2;
    v |= v >> 4;
    v |= v >> 8;
    v |= v >> 16;
    v |= v >> 32;
    v++;
    return v;
}

struct dynarray* dynarray_new(unsigned long long value_size) {
    struct dynarray* array = malloc(sizeof(struct dynarray));
    dynarray_init(array, value_size);
    return array;
}

void dynarray_init(struct dynarray* array, unsigned long long value_size) {
    array->value_size = value_size;
    array->data = NULL;
    dynarray_clear(array);
}

void dynarray_clear(struct dynarray* array) {
    free(array->data);
    array->data = NULL;
    array->capacity = 0;
    array->size = 0;
}

void dynarray_free(struct dynarray* array) {
    dynarray_clear(array);
    free(array);
}

void dynarray_push(struct dynarray* array, void* value) {
    if (array->size + 1 > array->capacity) {
        dynarray_reserve(array, next_power_of_two(array->capacity + 1));
    }
    memcpy(dynarray_element_end(array), value, array->value_size);
    ++array->size;
}

void* dynarray_element_at(struct dynarray* array, unsigned long long index) {
    return (unsigned char*)array->data + index * array->value_size;
}

void* dynarray_element_begin(struct dynarray* array) {
    return dynarray_element_at(array, 0);
}

// returns first element past the end
void* dynarray_element_end(struct dynarray* array) {
    return dynarray_element_at(array, array->size);
}

void dynarray_erase(struct dynarray* array, unsigned long long index) {
    assert(index < array->size);
    const unsigned long long bytes_to_move = dynarray_element_end(array) - dynarray_element_at(array, index + 1);
    if (bytes_to_move > 0) {
        memmove(dynarray_element_at(array, index), dynarray_element_at(array, index + 1), bytes_to_move);
    }
    dynarray_resize(array, array->size - 1);
}

void dynarray_reserve(struct dynarray* array, unsigned long long new_capacity) {
    void* new_data = realloc(array->data, new_capacity * array->value_size);
    assert(new_data != NULL);
    array->data = new_data;
    array->capacity = new_capacity;
    if (array->size > array->capacity) {
        array->size = array->capacity;
    }
}

void dynarray_resize(struct dynarray* array, unsigned long long new_size) {
    if (array->capacity < new_size) {
        dynarray_reserve(array, next_power_of_two(new_size));
    }
    array->size = new_size;
}

struct foo {
    int i;
};

int main() {
    struct dynarray* array = dynarray_new(sizeof(struct foo));

    for (int i = 0; i < 1024; ++i) {
        struct foo f;
        f.i = i;
        dynarray_push(array, &f);
        assert(i == ((struct foo*)dynarray_element_at(array, i))->i);
    }

    dynarray_reserve(array, 500);

    assert(array->size == 500);
    assert(array->capacity == 500);

    for (int i = 0; i < 500; ++i) {
        assert(((struct foo*)dynarray_element_at(array, i))->i == i);
    }

    dynarray_erase(array, 0);

    for (int i = 0; i < 499; ++i) {
        assert(((struct foo*)dynarray_element_at(array, i))->i == i + 1);
    }

    dynarray_erase(array, array->size - 1);

    for (int i = 0; i < 498; ++i) {
        assert(((struct foo*)dynarray_element_at(array, i))->i == i + 1);
    }

    dynarray_erase(array, 256);

    for (int i = 0; i < 497; ++i) {
        if (i < 256) {
            assert(((struct foo*)dynarray_element_at(array, i))->i == i + 1);
        } else {
            assert(((struct foo*)dynarray_element_at(array, i))->i == i + 2);
        }
    }

    dynarray_clear(array);
    dynarray_free(array);
}