#include "dvec.h"

#include <stddef.h>
#include <string.h>

#define ERR(x) ((x) >= 0 ? DVEC_BAD : (x))

struct sort_order_user_proxy {
	dvec_sort_order_fn order_fn;
	const void *user;
};

int
dvec_init(struct dvec *dvec, size_t dsize, size_t cap,
	const struct allocator *allocator)
{
	int rc = 0;

	LIBDVEC_ABORT_ON_ARGS(!dvec || !dsize || !allocator);

	dvec->allocator = allocator;
	dvec->dsize = dsize;
	dvec->cap = cap;
	dvec->len = 0;
	dvec->data = NULL;
	dvec->locked = false;
	dvec->fused = false;
	if (dvec->cap) {
		dvec->data = allocator->alloc(allocator, cap * dsize, &rc);
		LIBDVEC_ABORT_ON_ALLOC(!dvec->data);
		if (!dvec->data) return ERR(rc);
	}

	return DVEC_OK;
}

int
dvec_deinit(struct dvec *dvec)
{
	int rc;

	LIBDVEC_ABORT_ON_ARGS(!dvec);

	rc = dvec->allocator->free(dvec->allocator, dvec->data);
	LIBDVEC_ABORT_ON_ALLOC(rc);

	return rc >= 0 ? DVEC_OK : ERR(rc);
}

struct dvec *
dvec_alloc(size_t dsize, size_t cap,
	const struct allocator *allocator, int *rc)
{
	struct dvec *dvec;
	int _rc;

	LIBDVEC_ABORT_ON_ARGS(!allocator);

	dvec = allocator->alloc(allocator, sizeof(struct dvec), &_rc);
	LIBDVEC_ABORT_ON_ALLOC(!dvec);
	if (!dvec) {
		if (rc) *rc = ERR(_rc);
		return NULL;
	}

	_rc = dvec_init(dvec, dsize, cap, allocator);
	if (_rc < 0) {
		if (rc) *rc = _rc;
		allocator->free(allocator, dvec);
		return NULL;
	}

	return dvec;
}

static size_t
aligned_size(size_t size, size_t dsize)
{
	return size + (size % dsize ? dsize - size % dsize : 0);
}

struct dvec *
dvec_alloc_fused(size_t dsize, size_t cap,
	const struct allocator *allocator, int *rc)
{
	struct dvec *dvec;
	size_t off;
	int _rc = 0;

	LIBDVEC_ABORT_ON_ARGS(!allocator);

	off = aligned_size(sizeof(struct dvec), dsize);
	dvec = allocator->alloc(allocator, off + cap * dsize, &_rc);
	if (!dvec) {
		if (_rc) *rc = ERR(_rc);
		return NULL;
	}

	dvec->allocator = allocator;
	dvec->dsize = dsize;
	dvec->cap = cap;
	dvec->len = 0;
	dvec->data = (uint8_t *) dvec + off;
	dvec->locked = false;
	dvec->fused = true;

	return dvec;
}

int
dvec_free(struct dvec *dvec)
{
	const struct allocator *allocator;
	int rc;

	if (!dvec) return DVEC_OK;

	allocator = dvec->allocator;

	if (dvec->fused) {
		rc = allocator->free(allocator, dvec);
		if (rc < 0) return rc;
	} else {
		rc = dvec_deinit(dvec);
		if (rc) return rc;

		rc = allocator->free(allocator, dvec);
		LIBDVEC_ABORT_ON_ALLOC(rc);
		if (rc < 0) return rc;
	}

	return DVEC_OK;
}

int
dvec_copy(struct dvec *dst, struct dvec *src)
{
	int rc;

	LIBDVEC_ABORT_ON_ARGS(!dst || !src || dst->dsize != src->dsize);

	rc = dvec_reserve(dst, src->len);
	if (rc) return rc;

	dst->len = src->len;
	memcpy(dst->data, src->data, src->len * src->dsize);

	return DVEC_OK;
}

void
dvec_move(struct dvec *dst, struct dvec *src)
{
	LIBDVEC_ABORT_ON_ARGS(!dst || !src);

	memcpy(dst, src, sizeof(struct dvec));
}

void
dvec_swap(struct dvec *dst, struct dvec *src)
{
	struct dvec tmp;

	LIBDVEC_ABORT_ON_ARGS(!dst || !src);

	memcpy(&tmp, dst, sizeof(struct dvec));
	memcpy(dst, src, sizeof(struct dvec));
	memcpy(src, &tmp, sizeof(struct dvec));
}

void
dvec_clear(struct dvec *dvec)
{
	LIBDVEC_ABORT_ON_ARGS(!dvec);

	dvec->len = 0;
}

int
dvec_reserve(struct dvec *dvec, size_t len)
{
	void *data;
	size_t cap, off;
	int rc = 1;

	LIBDVEC_ABORT_ON_ARGS(!dvec);

	if (len <= dvec->cap) return DVEC_OK;

	if (dvec->locked) return DVEC_LOCKED;

	cap = 2 * dvec->cap;
	if (len > cap) cap = len;

	if (dvec->fused) {
		off = aligned_size(sizeof(struct dvec), dvec->dsize);
		data = dvec->allocator->realloc(dvec->allocator, dvec,
			off + dvec->cap * dvec->dsize, &rc);
		LIBDVEC_ABORT_ON_ALLOC(!data);
		if (!data) return ERR(rc);
		dvec = data;
		dvec->data = (uint8_t *) dvec + off;
	} else {
		data = dvec->allocator->realloc(dvec->allocator,
			dvec->data, cap * dvec->dsize, &rc);
		LIBDVEC_ABORT_ON_ALLOC(!data);
		if (!data) return ERR(rc);
		dvec->data = data;
	}

	dvec->cap = cap;

	return DVEC_OK;
}

int
dvec_shrink(struct dvec *dvec)
{
	void *data;
	size_t cap, off;
	int rc = 0;

	LIBDVEC_ABORT_ON_ARGS(!dvec);

	if (dvec->locked) return DVEC_LOCKED;

	cap = dvec->len;

	if (dvec->fused) {
		off = aligned_size(sizeof(struct dvec), dvec->dsize);
		data = dvec->allocator->realloc(dvec->allocator, dvec,
			off + cap * dvec->dsize, &rc);
		LIBDVEC_ABORT_ON_ALLOC(!data);
		if (!data) return ERR(rc);
		dvec = data;
		dvec->data = (uint8_t *) dvec + off;
	} else if (!dvec->len) {
		rc = dvec->allocator->free(dvec->allocator, dvec->data);
		LIBDVEC_ABORT_ON_ALLOC(rc);
		if (rc < 0) return rc;
	} else {
		data = dvec->allocator->realloc(dvec->allocator,
			&dvec->data, cap * dvec->dsize, &rc);
		LIBDVEC_ABORT_ON_ALLOC(!data);
		if (!data) return ERR(rc);
		dvec->data = data;
	}

	dvec->cap = cap;

	return DVEC_OK;
}

int
dvec_add(struct dvec *dvec, size_t index, size_t len)
{
	int rc;

	LIBDVEC_ABORT_ON_ARGS(!dvec || index > dvec->len);

	rc = dvec_reserve(dvec, dvec->len + len);
	if (rc) return rc;

	if (index < dvec->len) {
		memmove(dvec->data + (index + len) * dvec->dsize,
			dvec->data + index * dvec->dsize,
			(dvec->len - index) * dvec->dsize);
	}

	dvec->len += len;

	return DVEC_OK;
}

void
dvec_rm(struct dvec *dvec, size_t index, size_t count)
{
	LIBDVEC_ABORT_ON_ARGS(!dvec || index + count > dvec->len);

	if (index + count < dvec->len) {
		memmove(dvec->data + index * dvec->dsize,
			dvec->data + (index + count) * dvec->dsize,
			(dvec->len - (index + count)) * dvec->dsize);
	}

	dvec->len -= count;
}

void
dvec_replace(struct dvec *dvec, size_t index, const void *data, size_t count)
{
	LIBDVEC_ABORT_ON_ARGS(!dvec || !data || index + count > dvec->len);

	memcpy(dvec->data + index * dvec->dsize, data, count * dvec->dsize);
}

void *
dvec_iter_fwd(const struct dvec *dvec, const void *p)
{
	LIBDVEC_ABORT_ON_ARGS(!dvec);
	LIBDVEC_ABORT_ON_ARGS(p && (p < dvec->data));
	LIBDVEC_ABORT_ON_ARGS(p && (p >= dvec->data + dvec->len * dvec->dsize));
	LIBDVEC_ABORT_ON_ARGS(p && ((size_t) (p - dvec->data) % dvec->dsize));

	if (p == NULL) {
		if (dvec->len > 0)
			return dvec->data;
		else
			return NULL;
	} else if ((uint8_t *) p < dvec->data + dvec->dsize * (dvec->len - 1)) {
		return (uint8_t *) p + dvec->dsize;
	} else {
		return NULL;
	}
}

void *
dvec_iter_bwd(const struct dvec *dvec, const void *p)
{
	LIBDVEC_ABORT_ON_ARGS(!dvec);
	LIBDVEC_ABORT_ON_ARGS(p && (p < dvec->data));
	LIBDVEC_ABORT_ON_ARGS(p && (p >= dvec->data + dvec->len * dvec->dsize));
	LIBDVEC_ABORT_ON_ARGS(p && ((size_t) (p - dvec->data) % dvec->dsize));

	if (p == NULL) {
		if (dvec->len > 0)
			return dvec->data + (dvec->len - 1) * dvec->dsize;
		else
			return NULL;
	} else if ((uint8_t *) p > dvec->data) {
		return (uint8_t *) p - dvec->dsize;
	} else {
		return NULL;
	}
}

static void
dvec_quick_sort_swap(void *a, void *b, void *tmp, size_t dsize)
{
	memcpy(tmp, a, dsize);
	memcpy(a, b, dsize);
	memcpy(b, tmp, dsize);
}

int
dvec_quick_sort(struct dvec *dvec, const struct allocator *allocator,
	void *tmp, bool reverse, dvec_sort_order_fn in_order, const void *user)
{
	const size_t dsize = dvec->dsize;
	struct dvec stack;
	uint8_t *start, *end;
	uint8_t *lo, *hi, *pivot;
	bool lo_order, hi_order;
	bool modified = false;
	int rc;

	if (!dvec->len) return DVEC_OK;

	rc = dvec_init(&stack, sizeof(uint8_t *), 32, allocator);
	if (rc) return rc;

	*(uint8_t **)dvec_push(&stack) = dvec_front(dvec);
	*(uint8_t **)dvec_push(&stack) = dvec_back(dvec);

	while (!dvec_empty(&stack)) {
		end = *(uint8_t **)dvec_pop(&stack);
		start = *(uint8_t **)dvec_pop(&stack);

		if (start == end) continue;

		if (start + dsize >= end) {
			if (!in_order(start, end, user)) {
				dvec_quick_sort_swap(start, end, tmp, dsize);
				modified = true;
			}
			continue;
		}

		lo = start;
		hi = end;
		pivot = start + (size_t) (end - start) / dsize / 2 * dsize;

		while (lo < hi) {
			lo_order = lo != pivot && in_order(lo, pivot, user);
			hi_order = hi != pivot && in_order(pivot, hi, user);
			if (!lo_order && !hi_order) {
				if (lo == pivot) pivot = hi;
				else if (hi == pivot) pivot = lo;
				dvec_quick_sort_swap(lo, hi, tmp, dsize);
				modified = true;
			}
			if (lo_order) lo += dsize;
			if (hi_order) hi -= dsize;
		}

		if (start != pivot) {
			*(uint8_t **)dvec_push(&stack) = start;
			*(uint8_t **)dvec_push(&stack) = pivot - dsize;
		}

		if (end != pivot) {
			*(uint8_t **)dvec_push(&stack) = pivot + dsize;
			*(uint8_t **)dvec_push(&stack) = end;
		}
	}

	dvec_deinit(&stack);

	return modified ? DVEC_MODIFIED : DVEC_OK;
}

static bool
dvec_quick_sort_ex_order(const void *p1, const void *p2, const void *user)
{
	const struct sort_order_user_proxy *user_proxy = user;
	const void *d1 = *(void **)p1, *d2 = *(void **)p2;

	return user_proxy->order_fn(d1, d2, user_proxy->user);
}

int
dvec_quick_sort_ex(struct dvec *dvec, const struct allocator *allocator,
	void *tmp, bool reverse, dvec_sort_order_fn in_order, const void *user)
{
	const struct sort_order_user_proxy user_proxy = {
		.order_fn = in_order,
		.user = user
	};
	struct dvec ptrmap;
	void *p, **pp, **dst, **src;
	size_t dsize;
	int rc;

	if (!dvec->len) return DVEC_OK;

	dsize = dvec->dsize;

	rc = dvec_init(&ptrmap, sizeof(void *), dvec->len, allocator);
	if (rc) return rc;

	for (DVEC_ITER(dvec, p))
		*(void **)dvec_push(&ptrmap) = p;

	rc = dvec_quick_sort(&ptrmap, allocator, tmp, reverse,
		dvec_quick_sort_ex_order, &user_proxy);
	if (rc) return rc;

	for (DVEC_ITER(&ptrmap, pp)) {
		if (!*pp || dvec_idx(&ptrmap, pp) == dvec_idx(dvec, *pp))
			continue;
		dst = pp;
		memcpy(tmp, *dst, dsize);
		while (1) {
			src = dvec_at(&ptrmap, dvec_idx(dvec, *dst));
			if (!*src) break;
			memcpy(*dst, *src, dsize);
			*dst = NULL;
			dst = src;
		}
		memcpy(*dst, tmp, dsize);
	}

	rc = dvec_deinit(&ptrmap);
	if (rc) return rc;

	return DVEC_OK;
}

ssize_t
dvec_binary_search(struct dvec *dvec, dvec_search_cmp_fn search_cmp,
	void *user, ssize_t *lower, ssize_t *higher)
{
	size_t min, max, pos;
	int rc;

	min = 0;
	max = dvec->len;
	if (lower) *lower = -1;
	if (higher) *higher = -1;

	while (min < max) {
		pos = min + (max - min) / 2;
		rc = search_cmp(dvec_at(dvec, pos), user);
		if (!rc) {
			if (lower && pos > 0)
				*lower = (ssize_t) pos - 1;
			if (higher && pos < dvec->len - 1)
				*higher = (ssize_t) pos + 1;
			return (ssize_t) pos;
		} else if (rc > 0) {
			if (pos + 1 == max) {
				if (lower) *lower = (ssize_t) min;
				if (higher && min < dvec->len - 1)
					*higher = (ssize_t) min + 1;
				break;
			}
			min = pos + 1;
		} else {
			if (min == pos) {
				if (higher) *higher = (ssize_t) min;
				if (lower && min > 0)
					*lower = (ssize_t) min - 1;
				break;
			}
			max = pos;
		}
	}

	return -1;
}