#include "type.h"

static_queue*	static_queue_init(unsigned int data_size) {
	static_queue *q;
	q = (static_queue *)malloc(sizeof(static_queue));
	assert(q);
	q->front = 0;
	q->rear = 0;
	q->size = 0;
	q->data_size = data_size;
	q->capacity = MAX_QUEUE_SIZE;
	q->data = (char *)malloc(q->capacity * q->data_size);
	assert(q->data);
	if (!q->data) {
		q->capacity = 0;
	}
}

int	static_queue_push(static_queue *q, const void *value) {
	assert(q);
	if (q->size == q->capacity) {
		return -1; // Queue full
	}
	char *target = q->data + (q->rear * q->data_size);
	memcpy(target, value, q->data_size);
	q->rear = (q->rear + 1) % q->capacity;
	q->size++;
	return 0;
}

int	static_queue_pop(static_queue *q, void *value) {
	assert(q);
	if (q->size == 0) {
		return -1; // Queue empty
	}
	const char *source = q->data + (q->front * q->data_size);
	memcpy(value, source, q->data_size);
	q->front = (q->front + 1) % q->capacity;
	q->size--;
	return 0;
}

void	static_queue_destroy(static_queue *q) {
	assert(q);
	free(q->data);
	q->data = NULL;
	q->capacity = 0;
	q->size = 0;
}

/*
	DYNAMIC QUEUE
*/

void	dynamic_queue_init(dynamic_queue *q, unsigned int data_size) {
	assert(q);
	q->front = 0;
	q->rear = 0;
	q->size = 0;
	q->data_size = data_size;
	q->capacity = MAX_QUEUE_SIZE;
	q->data = (char *)malloc(q->capacity * q->data_size);
	assert(q->data);
	if (!q->data) {
		// Handle allocation failure (e.g., set capacity to 0)
		q->capacity = 0;
	}
}

int	dynamic_queue_push(dynamic_queue *q, const void *value) {
	assert(q);
	if (q->size == q->capacity) {
		return -1; // Queue full
	}
	char *target = q->data + (q->rear * q->data_size);
	memcpy(target, value, q->data_size);
	q->rear = (q->rear + 1) % q->capacity;
	q->size++;
	return 0;
}

int	dynamic_queue_pop(dynamic_queue *q, void *value) {
	assert(q);
	if (q->size == 0) {
		return -1; // Queue empty
	}
	const char *source = q->data + (q->front * q->data_size);
	memcpy(value, source, q->data_size);
	q->front = (q->front + 1) % q->capacity;
	q->size--;
	return 0;
}

void	dynamic_queue_destroy(dynamic_queue *q) {
	assert(q);
	free(q->data);
	q->data = NULL;
	q->capacity = 0;
	q->size = 0;
}

/*	OCTREE	*/

octree_node*	octree_node_create(float center[3], float extent) {
	octree_node* node = (octree_node*)malloc(sizeof(octree_node));
	assert(node);
	memcpy(node->center, center, sizeof(float) * 3);
	node->extent = extent;
	node->child_mask = 0;
	node->data = 0x00;
	memset(node->children, 0, sizeof(octree_node *) * 8);
	return (node);
}

int	octree_node_child_index(octree_node* node, float point[3]) {
	assert(node);
	int index = 0;
	if (point[0] >= node->center[0]) {
		index |= 1;
	}
	if (point[1] >= node->center[1]) {
		index |= 2;
	}
	if (point[2] >= node->center[2]) {
		index |= 4;
	}
	return (index);
}

bool	octree_node_isleaf(octree_node* node, int index) {
	assert(node);
	return ((node->child_mask & (1 << index)) != 0);
}

octree_node*	octree_node_get_or_create(octree_node* parent, int index) {
	assert(parent);
	if (parent->children[index]) {
		return (parent->children[index]);
	}

	float half = parent->extent * 0.5f;
	float child_center[3] = {
		parent->center[0] + ((index & 1) ? half : -half),
		parent->center[1] + ((index & 2) ? half : -half),
		parent->center[2] + ((index & 4) ? half : -half),
	};
	octree_node* child = octree_node_create(child_center, half);
	assert(child);

	parent->children[index] = child;
	parent->child_mask |= (1 << index);
	return (child);
}

octree_node* octree_node_lookup(octree_node* root, float point[3]) {
	assert(root);
	octree_node* current = root;

	while(current) {
		int idx = octree_node_child_index(current, point);
		if (!octree_node_isleaf(current, idx)) {
			break;
		}
		current = current->children[idx];
	}
	return (current);
}

void*	octree_node_insert(octree_node* root, float point[3], void* data, float min_extent) {
	assert(root);
	octree_node* current = root;
	while(current->extent > min_extent) {
		int idx = octree_node_child_index(current, point);
		if (!octree_node_isleaf(current, idx) && !octree_node_get_or_create(current, idx)) {
				return (NULL);
		}
		current = current->children[idx];
	}
	void* old_data = current->data;
	current->data = data;
	return (old_data);
}

void	octree_node_free(octree_node *node) {
	if (!node) {
		return;
	}
	for (int i = 0; i < 8; i++) {
		if (node->children[i]) {
			node_free(node->children[i]);
		}
	}
	free(node);
}

octree_node*	octree_traversal() {
	octree_node* node = 0x00;

	return (node);
}

bool	octree_insert() {

	return (false);
}

void*	octree_extract() {

	return (0x00);
}

octree* octree_init(const uint8_t max_depth) {
	octree* tree = 0x00;


	return (tree);
}

/*	dynamic array	*/

void	dynamic_array_add(dynamic_array *array, const void *element) {
	assert(array);
	if ((array->size + 1) * array->data_size >= array->capacity) {
		const uint32_t new_capacity = (unsigned int)(array->capacity * ARRAY_ALLOC_STEP);
		void *tmp = _aligned_malloc(new_capacity, array->alignement);
		assert(tmp);

		memcpy(tmp, array->data, array->size * array->data_size);

		_aligned_free(array->data);
		array->data = tmp;
		array->capacity = new_capacity;
	}

	memcpy((unsigned char *)array->data + array->size * array->data_size, element, array->data_size);
	array->size++;
}

void	dynamic_array_del(dynamic_array *array, const unsigned int idx) {
	assert(array);
	if (idx >= array->size) return;

	memmove((unsigned char *)array->data + idx * array->data_size,
		(unsigned char *)array->data + (idx + 1) * array->data_size,
		(array->size - idx - 1) * array->data_size);
	array->size--;

	if (array->size * array->data_size < array->capacity * 0.5 && array->capacity > 8 * array->data_size) {
		const uint32_t new_capacity = (unsigned int)(array->capacity * 0.5);
		void *tmp = _aligned_malloc(new_capacity, array->alignement);
		assert(tmp);

		memcpy(tmp, array->data, array->size * array->data_size);

		_aligned_free(array->data);
		array->data = tmp;
		array->capacity = new_capacity;
	}
}

void	dynamic_array_clear(dynamic_array *array) {
	assert(array);
	memset(array->data, 0, array->capacity);
	array->size = 0;
}

dynamic_array*	dynamic_array_init(const char data_size, const size_t alignement) {
	dynamic_array* array = (dynamic_array *)malloc(sizeof(dynamic_array));
	assert(array);

	array->data = _aligned_malloc(data_size * 8, array->alignement);
	assert(array->data);

	array->capacity = data_size * 8;
	array->data_size = data_size;
	array->size = 0;

	haven_darray_clear(array);
	return array;
}

void	dynamic_array_destroy(dynamic_array* array) {
	assert(array);
	_aligned_free(array->data);
	free(array);
}

void	dynamic_array_range_remove(dynamic_array* array, const uint32_t start, const uint32_t end) {
	assert(array);
	if (start >= array->size || end >= array->size || start > end) {
	    return;
	}

	const uint32_t num_elements_to_remove = end - start + 1;
	memmove(
		(unsigned char *)array->data + start * array->data_size,
		(unsigned char *)array->data + (end + 1) * array->data_size,
		(array->size - end - 1) * array->data_size
	);
	array->size -= num_elements_to_remove;

	if (array->size * array->data_size < array->capacity * 0.5 && array->capacity > 8 * array->data_size) {
		const uint32_t new_capacity = (unsigned int)(array->capacity * 0.5);
		void *tmp = _aligned_malloc(new_capacity, array->alignement);
		assert(tmp);

		memcpy(tmp, array->data, array->size * array->data_size);

		_aligned_free(array->data);
		array->data = tmp;
		array->capacity = new_capacity;
	}
}

void	dynamic_array_sort(const dynamic_array *array, int (*comparator)(const void *, const void *)) {
	assert(array);
	qsort(array->data, array->size, array->data_size, comparator);
}

//return a pointer to data idx;
void*	dynamic_array_get(const dynamic_array *array, const uint32_t idx) {
	assert(array);
	void *ret = (unsigned char *)array->data + (idx * array->data_size);
	return (ret);
}

void	dynamic_array_foreach(dynamic_array *array, void (*callback)(void *)) {
	assert(array);
	for (uint32_t i = 0; i < array->size; i++) {
		callback(haven_darray_get(array, i));
	}
}