#pragma once

#include"node.h"

static inline size_t bisect(const void *key, const void *base, size_t nmemb, size_t size, ssize_t(*compar)(const void*, const void*)) {
	size_t low = 0, high = nmemb;
	
	while(low < high) {
		size_t middle = (low + high) / 2;
		if(compar((const void*) ((uintptr_t) base + size * middle), key) < 0) {
			low = middle + 1;
		} else {
			high = middle;
		}
	}
	
	return low;
}

static inline ssize_t float_compar(const void *A, const void *B) {
	float a = *(float*) A;
	float b = *(float*) B;
	return (a > b) - (a < b);
}

static inline int adjacencycmp(const void *a, const void *b) {
	size_t v = (uintptr_t) ((CHiAdjacency*) a)[0] - (uintptr_t) ((CHiAdjacency*) b)[0];
	return v ? v : (uintptr_t) ((CHiAdjacency*) a)[1] - (uintptr_t) ((CHiAdjacency*) b)[1];
}

static inline void adjacency_add(CHiPubNode *source, CHiPubNode *sink) {
	CHiNodeGraph *ng = source->ng;
	
	if(ng->adjacencyCount == ng->adjacencyCapacity) {
		ng->adjacencies = realloc(ng->adjacencies, sizeof(CHiAdjacency) * (ng->adjacencyCapacity *= 2));
	}
	
	ng->adjacencies[ng->adjacencyCount][0] = source;
	ng->adjacencies[ng->adjacencyCount][1] = sink;
	ng->adjacencyCount++;
	
	qsort(ng->adjacencies, ng->adjacencyCount, sizeof(CHiAdjacency), adjacencycmp);
}

static inline void adjacency_remove(CHiPubNode *source, CHiPubNode *sink) {
	CHiNodeGraph *ng = source->ng;
	
	CHiAdjacency *adj = bsearch(&(CHiAdjacency) {source, sink}, ng->adjacencies, ng->adjacencyCount, sizeof(CHiAdjacency), adjacencycmp);
	if(adj) {
		memmove(adj, adj + 1, sizeof(CHiAdjacency) * (ng->adjacencyCount - (adj - ng->adjacencies) - 1));
		ng->adjacencyCount--;
	}
}

static inline void update_keyed_values(CHiNodeGraph *ng) {
	for(size_t kfsIdx = 0; kfsIdx < ng->keyframesList.count; kfsIdx++) {
		CHiKeyframes *kfs = ng->keyframesList.keyframes[kfsIdx];
		
		kfs->current.type = kfs->type;
		
		float now = ng->time;
		
		size_t idx = bisect(&now, kfs->times, kfs->count, sizeof(now), float_compar);
		
		if(idx == 0) {
			kfs->current.data = kfs->values[idx];
			
			if(kfs->current.type == CUTIHI_VAL_VEC4 && kfs->extrapolationMode == CUTIHI_EXTRAPOLATION_CONSTANT) {
				kfs->current.data.vec4[0] += (now - kfs->times[0]) * kfs->extrapolationParameter[0];
				kfs->current.data.vec4[1] += (now - kfs->times[0]) * kfs->extrapolationParameter[1];
				kfs->current.data.vec4[2] += (now - kfs->times[0]) * kfs->extrapolationParameter[2];
				kfs->current.data.vec4[3] += (now - kfs->times[0]) * kfs->extrapolationParameter[3];
			}
		} else if(idx == kfs->count) {
			kfs->current.data = kfs->values[idx - 1];
			
			if(kfs->current.type == CUTIHI_VAL_VEC4 && kfs->extrapolationMode == CUTIHI_EXTRAPOLATION_CONSTANT) {
				kfs->current.data.vec4[0] += (now - kfs->times[kfs->count - 1]) * kfs->extrapolationParameter[0];
				kfs->current.data.vec4[1] += (now - kfs->times[kfs->count - 1]) * kfs->extrapolationParameter[1];
				kfs->current.data.vec4[2] += (now - kfs->times[kfs->count - 1]) * kfs->extrapolationParameter[2];
				kfs->current.data.vec4[3] += (now - kfs->times[kfs->count - 1]) * kfs->extrapolationParameter[3];
			}
		} else {
			if(kfs->type == CUTIHI_VAL_VEC4) {
				float alpha = (now - kfs->times[idx - 1]) / (kfs->times[idx] - kfs->times[idx - 1]);
				
				kfs->current.data.vec4[0] = kfs->values[idx - 1].vec4[0] + (kfs->values[idx].vec4[0] - kfs->values[idx - 1].vec4[0]) * alpha;
				kfs->current.data.vec4[1] = kfs->values[idx - 1].vec4[1] + (kfs->values[idx].vec4[1] - kfs->values[idx - 1].vec4[1]) * alpha;
				kfs->current.data.vec4[2] = kfs->values[idx - 1].vec4[2] + (kfs->values[idx].vec4[2] - kfs->values[idx - 1].vec4[2]) * alpha;
				kfs->current.data.vec4[3] = kfs->values[idx - 1].vec4[3] + (kfs->values[idx].vec4[3] - kfs->values[idx - 1].vec4[3]) * alpha;
			} else {
				kfs->current.data = kfs->values[idx - 1];
			}
		}
	}
}