/* Silkworm2 - Verlet cloth physics in C
 *
 * Copyright (c) 2022 Evan Hemsley
 *
 * This software is provided 'as-is', without any express or implied warranty.
 * In no event will the authors be held liable for any damages arising from
 * the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software in a
 * product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 *
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 *
 * 3. This notice may not be removed or altered from any source distribution.
 *
 * Evan "cosmonaut" Hemsley <evan@moonside.games>
 *
 */

#include "Silkworm2.h"

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

#define PI 3.14159265358979323846

typedef struct Silkworm_Vector2
{
	float x;
	float y;
} Silkworm_Vector2;

static inline Silkworm_Vector2 Vector2_Rotate(Silkworm_Vector2 vector, float angle)
{
	Silkworm_Vector2 rotated;
	rotated.x = vector.x * cosf(angle) - vector.y * sinf(angle);
	rotated.y = vector.x * sinf(angle) + vector.y * cosf(angle);
	return rotated;
}

static inline Silkworm_Vector2 Vector2_Normalize(Silkworm_Vector2 vector)
{
	float length = sqrtf(vector.x * vector.x + vector.y * vector.y);

	Silkworm_Vector2 normalized;
	normalized.x = vector.x / length;
	normalized.y = vector.y / length;

	return normalized;
}

typedef struct Silkworm_Link Silkworm_Link;

typedef struct Silkworm_NodeClothReference
{
	uint64_t clothId;
	uint32_t horizontalIndex;
	uint32_t verticalIndex;
} Silkworm_NodeClothReference;

typedef struct Silkworm_Node
{
	uint64_t id;
	Silkworm_Vector2 position;
	Silkworm_Vector2 previousPosition;
	Silkworm_Vector2 velocity;
	Silkworm_Vector2 acceleration;
	float mass;
	float friction;
	float radius;
	bool pinned;
	float pushFactor;
	float windFactor;
	bool destroyable;

	bool markedForDestroy; /* mutual recursion on nodes/links so this makes it easier to track destroys */

	Silkworm_Link** links;
	uint32_t linkCount;

	Silkworm_NodeClothReference clothReference;
} Silkworm_Node;

struct Silkworm_Link
{
	uint64_t id;
	Silkworm_Node* a;
	Silkworm_Node* b;
	float distance;
	float tearThreshold;

	bool markedForDestroy; /* mutual recursion on nodes/links so this makes it easier to track destroys */
};

typedef enum Silkworm_ClothTriangleOrientation
{
	UpperLeft,
	BottomRight
} Silkworm_ClothTriangleOrientation;

typedef struct Silkworm_Triangle
{
	uint64_t id;
	Silkworm_Node* a;
	Silkworm_Node* b;
	Silkworm_Node* c;

	uint32_t aHorizontalIndex;
	uint32_t bHorizontalIndex;
	uint32_t cHorizontalIndex;

	uint32_t aVerticalIndex;
	uint32_t bVerticalIndex;
	uint32_t cVerticalIndex;

	Silkworm_ClothTriangleOrientation orientation;
} Silkworm_Triangle;

typedef struct NodeTriangleHashMap
{
	Silkworm_Node* key;
	uint32_t *indexArray;
	uint32_t indexArrayCount;
} NodeTriangleHashMap;

typedef struct NodeTriangleHashArray
{
	NodeTriangleHashMap* elements;
	uint32_t count;
} NodeTriangleHashArray;

#define NUM_NODE_TRIANGLE_HASH_BUCKETS 1031

typedef struct NodeTriangleHashTable
{
	NodeTriangleHashArray buckets[NUM_NODE_TRIANGLE_HASH_BUCKETS];
} NodeTriangleHashTable;

static inline uint64_t NodeTriangleHashTable_GetHashCode(Silkworm_Node *key)
{
	return 97 + (uint64_t)(size_t)key;
}

static inline uint32_t* NodeTriangleHashTable_Fetch(
	NodeTriangleHashTable *table,
	Silkworm_Node *key,
	uint32_t *arrayCount
) {
	uint32_t i;
	uint64_t hashcode = NodeTriangleHashTable_GetHashCode(key);
	NodeTriangleHashArray* arr = &table->buckets[hashcode % NUM_NODE_TRIANGLE_HASH_BUCKETS];

	for (i = 0; i < arr->count; i += 1)
	{
		if (arr->elements[i].key == key)
		{
			*arrayCount = arr->elements[i].indexArrayCount;
			return arr->elements[i].indexArray;
		}
	}

	*arrayCount = 0;
	return NULL;
}

static inline void NodeTriangleHashTable_Insert(
	NodeTriangleHashTable* table,
	Silkworm_Node* key,
	uint32_t index
) {
	uint32_t i;
	uint64_t hashcode = NodeTriangleHashTable_GetHashCode(key);
	NodeTriangleHashArray* arr = &table->buckets[hashcode % NUM_NODE_TRIANGLE_HASH_BUCKETS];
	bool foundKey = false;

	for (i = 0; i < arr->count; i += 1)
	{
		if (arr->elements[i].key == key)
		{
			arr->elements[i].indexArray = realloc(arr->elements[i].indexArray, sizeof(uint32_t) * (arr->elements[i].indexArrayCount + 1));
			arr->elements[i].indexArray[arr->elements[i].indexArrayCount] = index;
			arr->elements[i].indexArrayCount += 1;
			foundKey = true;
			break;
		}
	}

	if (!foundKey)
	{
		arr->elements = realloc(arr->elements, sizeof(NodeTriangleHashMap) * (arr->count + 1));

		arr->elements[arr->count].key = key;
		arr->elements[arr->count].indexArray = malloc(sizeof(uint32_t));
		arr->elements[arr->count].indexArray[0] = index;
		arr->elements[arr->count].indexArrayCount = 1;

		arr->count += 1;
	}
}

typedef struct LinkTriangleHashMap
{
	Silkworm_Link* key;
	uint32_t* indexArray;
	uint32_t indexArrayCount;
} LinkTriangleHashMap;

typedef struct LinkTriangleHashArray
{
	LinkTriangleHashMap* elements;
	uint32_t count;
} LinkTriangleHashArray;

#define NUM_LINK_TRIANGLE_HASH_BUCKETS 1031

typedef struct LinkTriangleHashTable
{
	LinkTriangleHashArray buckets[NUM_LINK_TRIANGLE_HASH_BUCKETS];
} LinkTriangleHashTable;

static inline uint64_t LinkTriangleHashTable_GetHashCode(Silkworm_Link* key)
{
	return 97 + (uint64_t)(size_t)key;
}

static inline uint32_t* LinkTriangleHashTable_Fetch(
	LinkTriangleHashTable* table,
	Silkworm_Link* key,
	uint32_t* arrayCount
) {
	uint32_t i;
	uint64_t hashcode = LinkTriangleHashTable_GetHashCode(key);
	LinkTriangleHashArray* arr = &table->buckets[hashcode % NUM_LINK_TRIANGLE_HASH_BUCKETS];

	for (i = 0; i < arr->count; i += 1)
	{
		if (arr->elements[i].key == key)
		{
			*arrayCount = arr->elements[i].indexArrayCount;
			return arr->elements[i].indexArray;
		}
	}

	*arrayCount = 0;
	return NULL;
}

static inline void LinkTriangleHashTable_Insert(
	LinkTriangleHashTable* table,
	Silkworm_Link* key,
	uint32_t index
) {
	uint32_t i;
	uint64_t hashcode = LinkTriangleHashTable_GetHashCode(key);
	LinkTriangleHashArray* arr = &table->buckets[hashcode % NUM_LINK_TRIANGLE_HASH_BUCKETS];
	bool foundKey = false;

	for (i = 0; i < arr->count; i += 1)
	{
		if (arr->elements[i].key == key)
		{
			arr->elements[i].indexArray = realloc(arr->elements[i].indexArray, sizeof(uint32_t) * (arr->elements[i].indexArrayCount + 1));
			arr->elements[i].indexArray[arr->elements[i].indexArrayCount] = index;
			arr->elements[i].indexArrayCount += 1;
			foundKey = true;
			break;
		}
	}

	if (!foundKey)
	{
		arr->elements = realloc(arr->elements, sizeof(LinkTriangleHashMap) * (arr->count + 1));

		arr->elements[arr->count].key = key;
		arr->elements[arr->count].indexArray = malloc(sizeof(uint32_t));
		arr->elements[arr->count].indexArray[0] = index;
		arr->elements[arr->count].indexArrayCount = 1;

		arr->count += 1;
	}
}

typedef struct Silkworm_Cloth
{
	uint64_t id;

	uint32_t horizontalNodeCount;
	uint32_t verticalNodeCount;

	/* note that the cloth doesn't own the nodes, just has pointers to them */
	Silkworm_Node*** nodes; /* x by y grid of indices */

	Silkworm_Triangle** triangles; /* array of pointers so we can use NULL */
	uint32_t triangleCount;

	NodeTriangleHashTable nodeHash;
	LinkTriangleHashTable linkHash;
} Silkworm_Cloth;

typedef struct Silkworm_Color
{
	uint8_t r;
	uint8_t g;
	uint8_t b;
	uint8_t a;
} Silkworm_Color;

typedef struct Silkworm_Context
{
	Silkworm_Node** nodes;
	uint32_t nodeCount;

	Silkworm_Link** links;
	uint32_t linkCount;

	Silkworm_Cloth** cloths;
	uint32_t clothCount;

	uint64_t* nodeIndexStack;
	uint32_t nodeIndexStackCount;
	uint32_t nodeIndexStackCapacity;

	uint64_t* linkIndexStack;
	uint32_t linkIndexStackCount;
	uint32_t linkIndexStackCapacity;

	uint64_t* clothIndexStack;
	uint32_t clothIndexStackCount;
	uint32_t clothIndexStackCapacity;

	float gravity;
	float xBound;
	float yBound;
	uint32_t clothDensity;

	float timeElapsed;

	/* keep track of these so can do callbacks */
	Silkworm_Vector2* nodeDestructionData;
	uint32_t nodeDestructionDataCount;
	uint32_t nodeDestructionDataCapacity;

	Silkworm_Vector2* linkDestructionData;
	uint32_t linkDestructionDataCount;
	uint32_t linkDestructionDataCapacity;
} Silkworm_Context;

static Silkworm_Context *context = NULL;

#define CONSTRAINT_ITERATION_COUNT 3 /* TODO: make this a parameter? */

void Silkworm_Init()
{
	context = malloc(sizeof(Silkworm_Context));

	context->nodes = NULL;
	context->nodeCount = 0;

	context->links = NULL;
	context->linkCount = 0;

	context->cloths = NULL;
	context->clothCount = 0;

	context->nodeIndexStackCapacity = 16;
	context->nodeIndexStack = malloc(sizeof(uint64_t) * context->nodeIndexStackCapacity);
	context->nodeIndexStackCount = 0;

	context->linkIndexStackCapacity = 16;
	context->linkIndexStack = malloc(sizeof(uint64_t) * context->linkIndexStackCapacity);
	context->linkIndexStackCount = 0;

	context->clothIndexStackCapacity = 16;
	context->clothIndexStack = malloc(sizeof(uint64_t) * context->clothIndexStackCapacity);
	context->clothIndexStackCount = 0;

	context->gravity = 200;
	context->xBound = 1000;
	context->yBound = 1000;
	context->clothDensity = 4;
	context->timeElapsed = 0;

	context->nodeDestructionDataCapacity = 16;
	context->nodeDestructionData = malloc(sizeof(Silkworm_Vector2) * context->nodeDestructionDataCapacity);
	context->nodeDestructionDataCount = 0;

	context->linkDestructionDataCapacity = 16;
	context->linkDestructionData = malloc(sizeof(Silkworm_Vector2) * context->nodeDestructionDataCapacity);
	context->linkDestructionDataCount = 0;
}

static inline Silkworm_Node* LookupNode(uint64_t nodeId)
{
	return context->nodes[nodeId];
}

static inline Silkworm_Link* LookupLink(uint64_t linkId)
{
	return context->links[linkId];
}

static inline Silkworm_Cloth* LookupCloth(uint64_t clothId)
{
	return context->cloths[clothId];
}

/* we defer actual destruction to Update to avoid issues with NULL */

void Silkworm_DestroyNode(void* nodePtr)
{
	uint32_t i;
	Silkworm_Node* node = (Silkworm_Node*) nodePtr;

	if (node != NULL)
	{
		node->markedForDestroy = true;

		for (i = 0; i < node->linkCount; i += 1)
		{
			if (node->links[i] != NULL)
			{
				node->links[i]->markedForDestroy = true;
			}
		}
	}
}

void Silkworm_Internal_DestroyLink(Silkworm_Link* link)
{
	link->markedForDestroy = true;

	if (context->linkDestructionDataCount >= context->linkDestructionDataCapacity)
	{
		context->linkDestructionDataCapacity *= 2;
		context->linkDestructionData = realloc(context->linkDestructionData, sizeof(Silkworm_Vector2) * context->linkDestructionDataCapacity);
	}

	Silkworm_Vector2 position;
	position.x = (link->a->position.x + link->b->position.x) / 2;
	position.y = (link->a->position.y + link->b->position.y) / 2;
	context->linkDestructionData[context->linkDestructionDataCount] = position;
	context->linkDestructionDataCount += 1;
}

void Silkworm_DestroyLink(double linkId)
{
	Silkworm_Internal_DestroyLink(LookupLink((uint64_t)linkId));
}

void Silkworm_ClothDestroy(void* clothPtr)
{
	uint32_t i, j;

	Silkworm_Cloth* cloth = (Silkworm_Cloth*) clothPtr;

	if (cloth != NULL)
	{
		context->cloths[cloth->id] = NULL;

		for (i = 0; i < NUM_NODE_TRIANGLE_HASH_BUCKETS; i += 1)
		{
			for (j = 0; j < cloth->nodeHash.buckets[i].count; j += 1)
			{
				free(cloth->nodeHash.buckets[i].elements[j].indexArray);
			}
			if (cloth->nodeHash.buckets[i].elements != NULL)
			{
				free(cloth->nodeHash.buckets[i].elements);
			}
		}

		for (i = 0; i < NUM_LINK_TRIANGLE_HASH_BUCKETS; i += 1)
		{
			for (j = 0; j < cloth->linkHash.buckets[i].count; j += 1)
			{
				free(cloth->linkHash.buckets[i].elements[j].indexArray);
			}
			if (cloth->linkHash.buckets[i].elements != NULL)
			{
				free(cloth->linkHash.buckets[i].elements);
			}
		}

		for (i = 0; i < cloth->horizontalNodeCount; i += 1)
		{
			for (j = 0; j < cloth->verticalNodeCount; j += 1)
			{
				Silkworm_Node* node = cloth->nodes[i][j];

				if (node != NULL)
				{
					Silkworm_DestroyNode(node);
				}
			}
			free(cloth->nodes[i]);
		}
		free(cloth->nodes);

		for (i = 0; i < cloth->triangleCount; i += 1)
		{
			if (cloth->triangles[i] != NULL)
			{
				free(cloth->triangles[i]);
			}
		}
		free(cloth->triangles);

		if (context->clothIndexStackCount >= context->clothIndexStackCapacity)
		{
			context->clothIndexStackCapacity *= 2;
			context->nodeIndexStack = realloc(context->nodeIndexStack, sizeof(uint64_t) * context->nodeIndexStackCapacity);
		}

		context->clothIndexStack[context->clothIndexStackCount] = cloth->id;
		context->clothIndexStackCount += 1;

		free(cloth);
	}
}

void Silkworm_PerformDestroys()
{
	uint32_t i, j, k;

	for (i = 0; i < context->linkCount; i += 1)
	{
		Silkworm_Link* link = context->links[i];

		if (link != NULL && context->links[i]->markedForDestroy)
		{
			/* find cloth to remove from relevant triangles */
			for (j = 0; j < context->clothCount; j += 1)
			{
				Silkworm_Cloth* cloth = context->cloths[j];

				if (cloth != NULL)
				{
					uint32_t triangleIndexCount = 0;
					uint32_t* triangleIndices = LinkTriangleHashTable_Fetch(&cloth->linkHash, link, &triangleIndexCount);

					for (k = 0; k < triangleIndexCount; k += 1)
					{
						uint32_t triangleIndex = triangleIndices[k];

						if (cloth->triangles[triangleIndex] != NULL)
						{
							free(cloth->triangles[triangleIndex]);
							cloth->triangles[triangleIndex] = NULL;
						}
					}
				}
			}

			for (j = 0; j < link->a->linkCount; j += 1)
			{
				if (link->a->links[j] == context->links[i])
				{
					link->a->links[j] = NULL;
				}
			}

			for (j = 0; j < link->b->linkCount; j += 1)
			{
				if (link->b->links[j] == context->links[i])
				{
					link->b->links[j] = NULL;
				}
			}

			free(context->links[i]);
			context->links[i] = NULL;

			if (context->linkIndexStackCount >= context->linkIndexStackCapacity)
			{
				context->linkIndexStackCapacity *= 2;
				context->linkIndexStack = realloc(context->linkIndexStack, sizeof(uint64_t) * context->linkIndexStackCapacity);
			}

			context->linkIndexStack[context->linkIndexStackCount] = i;
			context->linkIndexStackCount += 1;
		}
	}

	for (i = 0; i < context->nodeCount; i += 1)
	{
		Silkworm_Node* node = context->nodes[i];

		if (node != NULL && node->markedForDestroy)
		{
			Silkworm_Cloth* cloth = LookupCloth(node->clothReference.clothId);

			if (cloth != NULL)
			{
				uint32_t triangleIndexCount = 0;
				uint32_t* triangleIndices = NodeTriangleHashTable_Fetch(&cloth->nodeHash, node, &triangleIndexCount);

				for (k = 0; k < triangleIndexCount; k += 1)
				{
					uint32_t triangleIndex = triangleIndices[k];

					if (cloth->triangles[triangleIndex] != NULL)
					{
						free(cloth->triangles[triangleIndex]);
						cloth->triangles[triangleIndex] = NULL;
					}
				}

				cloth->nodes[node->clothReference.horizontalIndex][node->clothReference.verticalIndex] = NULL;
			}
			
			free(context->nodes[i]->links);
			free(context->nodes[i]);
			context->nodes[i] = NULL;

			if (context->nodeIndexStackCount >= context->nodeIndexStackCapacity)
			{
				context->nodeIndexStackCapacity *= 2;
				context->nodeIndexStack = realloc(context->nodeIndexStack, sizeof(uint64_t) * context->nodeIndexStackCapacity);
			}

			context->nodeIndexStack[context->nodeIndexStackCount] = i;
			context->nodeIndexStackCount += 1;
		}
	}
}

void Silkworm_Update(float delta, float windSpeedX, float windSpeedY)
{
	uint32_t i, j;
	Silkworm_Link *link;
	Silkworm_Node *node;

	context->nodeDestructionDataCount = 0;
	context->linkDestructionDataCount = 0;

	for (i = 0; i < CONSTRAINT_ITERATION_COUNT; i += 1)
	{
		for (j = 0; j < context->linkCount; j += 1)
		{
			link = context->links[j];

			if (link != NULL)
			{
				float diffX = link->a->position.x - link->b->position.x;
				float diffY = link->a->position.y - link->b->position.y;
				float d = (float)sqrt(diffX * diffX + diffY * diffY);

				float difference = (link->distance - d) / d;

				float translateX = diffX * 0.5f * difference;
				float translateY = diffY * 0.5f * difference;

				float distanceMoved = (float)sqrt(translateX * translateX + translateY * translateY);

				if (distanceMoved > link->tearThreshold)
				{
					Silkworm_DestroyLink((double)link->id);
				}
				else
				{
					if (!link->a->pinned)
					{
						link->a->position.x += translateX;
						link->a->position.y += translateY;
					}

					if (!link->b->pinned)
					{
						link->b->position.x -= translateX;
						link->b->position.y -= translateY;
					}
				}
			}
		}
	}

	for (j = 0; j < context->nodeCount; j += 1)
	{
		node = context->nodes[j];

		if (node != NULL)
		{
			if (!node->pinned)
			{
				float velocityX = (node->position.x - node->previousPosition.x) * node->friction;
				float velocityY = (node->position.y - node->previousPosition.y) * node->friction;

				float accelerationX = node->acceleration.x * delta * delta;
				float accelerationY = (node->acceleration.y + node->mass * context->gravity) * delta * delta;

				node->previousPosition.x = node->position.x;
				node->previousPosition.y = node->position.y;

				node->position.x += velocityX + accelerationX;
				node->position.y += velocityY + accelerationY;

				node->position.x += (0.5f * sinf(context->timeElapsed * 4 + node->position.y / 2) + 0.5f) * windSpeedX * 0.05f * node->windFactor;
				node->position.y += (0.5f * sinf(context->timeElapsed * 4 + node->position.x / 3) + 0.5f) * windSpeedY * 0.05f * node->windFactor;

				node->velocity.x = velocityX * delta;
				node->velocity.y = velocityY * delta;

				if (fabs(node->position.x) > context->xBound || fabs(node->position.x) > context->yBound)
				{
					Silkworm_DestroyNode(node);
				}
			}
		}
	}

	Silkworm_PerformDestroys();

	context->timeElapsed += delta;
}

void* Silkworm_CreateNode(Silkworm_NodeCreateInfo* nodeCreateInfo)
{
	Silkworm_Node* node = malloc(sizeof(Silkworm_Node));
	uint64_t id;

	if (context->nodeIndexStackCount > 0)
	{
		id = context->nodeIndexStack[context->nodeIndexStackCount - 1];
		context->nodeIndexStackCount -= 1;
	}
	else
	{
		id = context->nodeCount;

		context->nodes = realloc(context->nodes, sizeof(Silkworm_Node*) * (context->nodeCount + 1));
		context->nodeCount += 1;
	}

	context->nodes[id] = node;

	node->id = id;
	node->position.x = (float) nodeCreateInfo->x;
	node->position.y = (float) nodeCreateInfo->y;
	node->previousPosition.x = (float) nodeCreateInfo->x;
	node->previousPosition.y = (float) nodeCreateInfo->y;
	node->velocity.x = 0;
	node->velocity.y = 0;
	node->acceleration.x = 0;
	node->acceleration.y = 0;
	node->mass = (float) nodeCreateInfo->mass;
	node->friction = (float) nodeCreateInfo->friction;
	node->radius = (float) nodeCreateInfo->radius;
	node->pushFactor = (float) nodeCreateInfo->pushFactor;
	node->windFactor = (float) nodeCreateInfo->windFactor;
	node->pinned = false;
	node->destroyable = false;
	node->markedForDestroy = false;
	node->links = NULL;
	node->linkCount = 0;

	return (void*) node;
}

void Silkworm_ClothNodePin(void* clothPtr, uint32_t i, uint32_t j)
{
	Silkworm_Cloth* cloth = (Silkworm_Cloth*) clothPtr;
	Silkworm_Node* node = cloth->nodes[i][j];
	if (node != NULL)
	{
		node->pinned = true;
	}
}

void Silkworm_ClothNodeUnpin(void* clothPtr, uint32_t i, uint32_t j)
{
	Silkworm_Cloth* cloth = (Silkworm_Cloth*) clothPtr;
	Silkworm_Node* node = cloth->nodes[i][j];
	if (node != NULL)
	{
		node->pinned = false;
	}
}

void Silkworm_ClothNodeDestroy(void* clothPtr, uint32_t i, uint32_t j)
{
	Silkworm_Cloth* cloth = (Silkworm_Cloth*) clothPtr;
	Silkworm_Node* node = cloth->nodes[i][j];
	if (node != NULL)
	{
		Silkworm_DestroyNode(node);
	}
}

void* Silkworm_CreateLink(void* nodeAPtr, void* nodeBPtr, float distance, float tearThreshold)
{
	Silkworm_Node* nodeA = (Silkworm_Node*) nodeAPtr;
	Silkworm_Node* nodeB = (Silkworm_Node*) nodeBPtr;

	uint64_t id;

	Silkworm_Link* link = malloc(sizeof(Silkworm_Link));

	if (context->linkIndexStackCount > 0)
	{
		id = context->linkIndexStack[context->linkIndexStackCount - 1];
		context->linkIndexStackCount -= 1;
	}
	else
	{
		id = context->linkCount;

		context->links = realloc(context->links, sizeof(Silkworm_Link*) * (context->linkCount + 1));
		context->linkCount += 1;
	}

	context->links[id] = link;

	link->id = id;
	link->a = nodeA;
	link->b = nodeB;
	link->distance = (float)distance;
	link->tearThreshold = (float)tearThreshold;
	link->markedForDestroy = false;

	link->a->links = realloc(link->a->links, sizeof(Silkworm_Link*) * (link->a->linkCount + 1));
	link->a->links[link->a->linkCount] = link;
	link->a->linkCount += 1;

	link->b->links = realloc(link->b->links, sizeof(Silkworm_Link*) * (link->b->linkCount + 1));
	link->b->links[link->b->linkCount] = link;
	link->b->linkCount += 1;

	return link;
}

void* Silkworm_ClothCreate(Silkworm_ClothCreateInfo* clothCreateInfo)
{
	int32_t i, j, k, m;
	Silkworm_NodeCreateInfo nodeCreateInfo;

	Silkworm_Cloth* cloth = malloc(sizeof(Silkworm_Cloth));

	cloth->horizontalNodeCount = ((uint32_t)clothCreateInfo->horizontalNodeCount / context->clothDensity) + 1;
	cloth->verticalNodeCount = ((uint32_t)clothCreateInfo->verticalNodeCount / context->clothDensity) + 1;
	cloth->nodes = malloc(sizeof(Silkworm_Node**) * cloth->horizontalNodeCount);

	uint64_t id;

	if (context->clothIndexStackCount > 0)
	{
		id = context->clothIndexStack[context->clothIndexStackCount - 1];
		context->clothIndexStackCount -= 1;

		context->cloths[id] = cloth;
	}
	else
	{
		id = context->clothCount;

		context->cloths = realloc(context->cloths, sizeof(Silkworm_Cloth*) * (context->clothCount + 1));
		context->cloths[id] = cloth;
		context->clothCount += 1;
	}

	cloth->id = id;

	for (i = 0; i < NUM_NODE_TRIANGLE_HASH_BUCKETS; i += 1)
	{
		cloth->nodeHash.buckets[i].elements = NULL;
		cloth->nodeHash.buckets[i].count = 0;
	}

	for (i = 0; i < NUM_LINK_TRIANGLE_HASH_BUCKETS; i += 1)
	{
		cloth->linkHash.buckets[i].elements = NULL;
		cloth->linkHash.buckets[i].count = 0;
	}

	for (i = 0; i < cloth->horizontalNodeCount; i += 1)
	{
		cloth->nodes[i] = malloc(sizeof(Silkworm_Node*) * cloth->verticalNodeCount);

		for (j = 0; j < cloth->verticalNodeCount; j += 1)
		{
			nodeCreateInfo.x = clothCreateInfo->x + i * context->clothDensity;
			nodeCreateInfo.y = clothCreateInfo->y + j * context->clothDensity;
			nodeCreateInfo.destroyable = true;
			nodeCreateInfo.friction = clothCreateInfo->friction;
			nodeCreateInfo.mass = clothCreateInfo->mass;
			nodeCreateInfo.pinned = false;
			nodeCreateInfo.pushFactor = 0;
			nodeCreateInfo.radius = 1;
			nodeCreateInfo.windFactor = clothCreateInfo->windFactor;

			Silkworm_Node* node = Silkworm_CreateNode(&nodeCreateInfo);

			cloth->nodes[i][j] = node;

			if (j == 0)
			{
				node->pinned = true;
			}

			node->destroyable = true;

			node->clothReference.clothId = cloth->id;
			node->clothReference.horizontalIndex = i;
			node->clothReference.verticalIndex = j;
		}
	}

	cloth->triangles = malloc(sizeof(Silkworm_Triangle*) * cloth->horizontalNodeCount * cloth->verticalNodeCount * 2);
	uint32_t triangleIndex = 0;

	for (i = 0; i < cloth->horizontalNodeCount; i += 1)
	{
		for (j = 0; j < cloth->verticalNodeCount; j += 1)
		{
			if (i + 1 < cloth->horizontalNodeCount && j + 1 < cloth->verticalNodeCount)
			{
				cloth->triangles[triangleIndex] = malloc(sizeof(Silkworm_Triangle));

				cloth->triangles[triangleIndex]->a = cloth->nodes[i][j];
				cloth->triangles[triangleIndex]->b = cloth->nodes[i + 1][j];
				cloth->triangles[triangleIndex]->c = cloth->nodes[i][j + 1];
				cloth->triangles[triangleIndex]->orientation = UpperLeft;

				cloth->triangles[triangleIndex]->aHorizontalIndex = i;
				cloth->triangles[triangleIndex]->aVerticalIndex = j;

				cloth->triangles[triangleIndex]->bHorizontalIndex = i + 1;
				cloth->triangles[triangleIndex]->bVerticalIndex = j;

				cloth->triangles[triangleIndex]->cHorizontalIndex = i;
				cloth->triangles[triangleIndex]->cVerticalIndex = j + 1;

				NodeTriangleHashTable_Insert(&cloth->nodeHash, cloth->nodes[i][j], triangleIndex);
				NodeTriangleHashTable_Insert(&cloth->nodeHash, cloth->nodes[i + 1][j], triangleIndex);
				NodeTriangleHashTable_Insert(&cloth->nodeHash, cloth->nodes[i][j + 1], triangleIndex);

				triangleIndex += 1;
			}

			if (i - 1 >= 0 && j - 1 >= 0)
			{
				cloth->triangles[triangleIndex] = malloc(sizeof(Silkworm_Triangle));

				cloth->triangles[triangleIndex]->a = cloth->nodes[i][j];
				cloth->triangles[triangleIndex]->b = cloth->nodes[i - 1][j];
				cloth->triangles[triangleIndex]->c = cloth->nodes[i][j - 1];
				cloth->triangles[triangleIndex]->orientation = BottomRight;

				cloth->triangles[triangleIndex]->aHorizontalIndex = i;
				cloth->triangles[triangleIndex]->aVerticalIndex = j;

				cloth->triangles[triangleIndex]->bHorizontalIndex = i - 1;
				cloth->triangles[triangleIndex]->bVerticalIndex = j;

				cloth->triangles[triangleIndex]->cHorizontalIndex = i;
				cloth->triangles[triangleIndex]->cVerticalIndex = j - 1;

				NodeTriangleHashTable_Insert(&cloth->nodeHash, cloth->nodes[i][j], triangleIndex);
				NodeTriangleHashTable_Insert(&cloth->nodeHash, cloth->nodes[i - 1][j], triangleIndex);
				NodeTriangleHashTable_Insert(&cloth->nodeHash, cloth->nodes[i][j - 1], triangleIndex);

				triangleIndex += 1;
			}
		}
	}

	cloth->triangleCount = triangleIndex;

	for (i = 0; i < cloth->horizontalNodeCount; i += 1)
	{
		for (j = 0; j < cloth->verticalNodeCount; j += 1)
		{
			if (i - 1 >= 0)
			{
				Silkworm_Link* link = Silkworm_CreateLink(cloth->nodes[i - 1][j], cloth->nodes[i][j], context->clothDensity, clothCreateInfo->tearThreshold);

				uint32_t indexArrayOneCount = 0;
				uint32_t* indexArrayOne = NodeTriangleHashTable_Fetch(&cloth->nodeHash, cloth->nodes[i - 1][j], &indexArrayOneCount);

				uint32_t indexArrayTwoCount = 0;
				uint32_t* indexArrayTwo = NodeTriangleHashTable_Fetch(&cloth->nodeHash, cloth->nodes[i][j], &indexArrayTwoCount);

				for (k = 0; k < indexArrayOneCount; k += 1)
				{
					uint32_t triangleIndex = indexArrayOne[k];
					for (m = 0; m < indexArrayTwoCount; m += 1)
					{
						if (indexArrayTwo[m] == triangleIndex)
						{
							LinkTriangleHashTable_Insert(&cloth->linkHash, link, triangleIndex);
						}
					}
				}
			}

			if (j - 1 >= 0)
			{
				Silkworm_Link* link = Silkworm_CreateLink(cloth->nodes[i][j - 1], cloth->nodes[i][j], context->clothDensity, clothCreateInfo->tearThreshold);

				uint32_t indexArrayOneCount = 0;
				uint32_t* indexArrayOne = NodeTriangleHashTable_Fetch(&cloth->nodeHash, cloth->nodes[i][j - 1], &indexArrayOneCount);

				uint32_t indexArrayTwoCount = 0;
				uint32_t* indexArrayTwo = NodeTriangleHashTable_Fetch(&cloth->nodeHash, cloth->nodes[i][j], &indexArrayTwoCount);

				for (k = 0; k < indexArrayOneCount; k += 1)
				{
					uint32_t triangleIndex = indexArrayOne[k];
					for (m = 0; m < indexArrayTwoCount; m += 1)
					{
						if (indexArrayTwo[m] == triangleIndex)
						{
							LinkTriangleHashTable_Insert(&cloth->linkHash, link, triangleIndex);
						}
					}
				}
			}
		}
	}

	return (void*) cloth;
}

/* FIXME: this is very unsafe */
double Silkworm_ClothHorizontalNodeCount(double clothId)
{
	return context->cloths[(uint32_t)clothId]->horizontalNodeCount;
}

double Silkworm_ClothVerticalNodeCount(double clothId)
{
	return context->cloths[(uint32_t)clothId]->verticalNodeCount;
}

int32_t Silkworm_ClothFillTriangleBuffer(void* clothPtr, void* vertexBufferPtr, float depth, float leftUV, float widthUV, float topUV, float heightUV)
{
	uint32_t i, vertexCount;
	Silkworm_Cloth* cloth = (Silkworm_Cloth*) clothPtr;
	Silkworm_Vertex* vertexBuffer = (Silkworm_Vertex*) vertexBufferPtr;

	vertexCount = 0;
	for (i = 0; i < cloth->triangleCount; i += 1)
	{
		if (cloth->triangles[i] != NULL)
		{
			if (cloth->triangles[i]->orientation == UpperLeft)
			{
				float left = leftUV + widthUV * ((float)cloth->triangles[i]->aHorizontalIndex / (cloth->horizontalNodeCount - 1));
				float right = leftUV + widthUV * ((float)cloth->triangles[i]->bHorizontalIndex / (cloth->horizontalNodeCount - 1));

				float top = topUV + heightUV * ((float)cloth->triangles[i]->aVerticalIndex / (cloth->verticalNodeCount - 1));
				float bottom = topUV + heightUV * ((float)cloth->triangles[i]->cVerticalIndex / (cloth->verticalNodeCount - 1));
					
				vertexBuffer[vertexCount].x = cloth->triangles[i]->a->position.x;
				vertexBuffer[vertexCount].y = cloth->triangles[i]->a->position.y;
				vertexBuffer[vertexCount].z = depth;
				vertexBuffer[vertexCount].u = left;
				vertexBuffer[vertexCount].v = top;

				vertexBuffer[vertexCount + 1].x = cloth->triangles[i]->b->position.x;
				vertexBuffer[vertexCount + 1].y = cloth->triangles[i]->b->position.y;
				vertexBuffer[vertexCount + 1].z = depth;
				vertexBuffer[vertexCount + 1].u = right;
				vertexBuffer[vertexCount + 1].v = top;

				vertexBuffer[vertexCount + 2].x = cloth->triangles[i]->c->position.x;
				vertexBuffer[vertexCount + 2].y = cloth->triangles[i]->c->position.y;
				vertexBuffer[vertexCount + 2].z = depth;
				vertexBuffer[vertexCount + 2].u = left;
				vertexBuffer[vertexCount + 2].v = bottom;

				vertexCount += 3;
			}
			else if (cloth->triangles[i]->orientation == BottomRight)
			{
				float left = leftUV + widthUV * ((float)cloth->triangles[i]->bHorizontalIndex / (cloth->horizontalNodeCount - 1));
				float right = leftUV + widthUV * ((float)cloth->triangles[i]->aHorizontalIndex / (cloth->horizontalNodeCount - 1));

				float top = topUV + heightUV * ((float)cloth->triangles[i]->cVerticalIndex / (cloth->verticalNodeCount - 1));
				float bottom = topUV + heightUV * ((float)cloth->triangles[i]->aVerticalIndex / (cloth->verticalNodeCount - 1));

				vertexBuffer[vertexCount].x = cloth->triangles[i]->a->position.x;
				vertexBuffer[vertexCount].y = cloth->triangles[i]->a->position.y;
				vertexBuffer[vertexCount].z = depth;
				vertexBuffer[vertexCount].u = right;
				vertexBuffer[vertexCount].v = bottom;

				vertexBuffer[vertexCount + 1].x = cloth->triangles[i]->b->position.x;
				vertexBuffer[vertexCount + 1].y = cloth->triangles[i]->b->position.y;
				vertexBuffer[vertexCount + 1].z = depth;
				vertexBuffer[vertexCount + 1].u = left;
				vertexBuffer[vertexCount + 1].v = bottom;

				vertexBuffer[vertexCount + 2].x = cloth->triangles[i]->c->position.x;
				vertexBuffer[vertexCount + 2].y = cloth->triangles[i]->c->position.y;
				vertexBuffer[vertexCount + 2].z = depth;
				vertexBuffer[vertexCount + 2].u = right;
				vertexBuffer[vertexCount + 2].v = top;

				vertexCount += 3;
			}
		}
	}

	return vertexCount;
}

void Silkworm_PushNodesInRadius(float x, float y, float radius, float xDirection, float yDirection)
{
	/* TODO: spatial hash implementation */

	uint32_t i;

	for (i = 0; i < context->nodeCount; i += 1)
	{
		Silkworm_Node* node = context->nodes[i];

		if (node != NULL && !node->pinned)
		{
			float xDistance = (float)fabs(x - node->position.x);
			float yDistance = (float)fabs(y - node->position.y);

			float squareDistance = xDistance * xDistance + yDistance * yDistance;

			if (squareDistance <= radius * radius)
			{
				node->position.x += xDirection * node->pushFactor;
				node->position.y += yDirection * node->pushFactor;
			}
		}
	}
}

void Silkworm_PinNodesInRadius(float x, float y, float radius)
{
	/* TODO: spatial hash implementation */

	uint32_t i;

	for (i = 0; i < context->nodeCount; i += 1)
	{
		Silkworm_Node* node = context->nodes[i];

		if (node != NULL)
		{
			float xDistance = (float)fabs((float)x - node->position.x);
			float yDistance = (float)fabs((float)y - node->position.y);

			float squareDistance = xDistance * xDistance + yDistance * yDistance;

			if (squareDistance <= radius * radius)
			{
				node->pinned = true;
			}
		}
	}
}

void Silkworm_UnpinNodesInRadius(float x, float y, float radius)
{
	/* TODO: spatial hash implementation */

	uint32_t i;

	for (i = 0; i < context->nodeCount; i += 1)
	{
		Silkworm_Node* node = context->nodes[i];

		if (node != NULL)
		{
			float xDistance = (float)fabs((float)x - node->position.x);
			float yDistance = (float)fabs((float)y - node->position.y);

			float squareDistance = xDistance * xDistance + yDistance * yDistance;

			if (squareDistance <= radius * radius)
			{
				node->pinned = false;
			}
		}
	}
}

void Silkworm_DestroyNodesInRadius(float x, float y, float radius)
{
	/* TODO: spatial hash implementation */

	uint32_t i;

	for (i = 0; i < context->nodeCount; i += 1)
	{
		Silkworm_Node* node = context->nodes[i];

		if (node != NULL && node->destroyable)
		{
			float xDistance = (float)fabs(x - node->position.x);
			float yDistance = (float)fabs(y - node->position.y);

			float squareDistance = xDistance * xDistance + yDistance * yDistance;

			if (squareDistance <= radius * radius)
			{
				Silkworm_DestroyNode(node);

				if (context->nodeDestructionDataCount >= context->nodeDestructionDataCapacity)
				{
					context->nodeDestructionDataCapacity *= 2;
					context->nodeDestructionData = realloc(context->nodeDestructionData, sizeof(Silkworm_Vector2) * context->nodeDestructionDataCapacity);

				}

				context->nodeDestructionData[context->nodeDestructionDataCount] = node->position;
				context->nodeDestructionDataCount += 1;
			}
		}
	}

	Silkworm_PerformDestroys();
}

void* Silkworm_FindClothInRadius(float x, float y, float radius)
{
	/* TODO: spatial hash implementation */

	uint32_t i;

	for (i = 0; i < context->nodeCount; i += 1)
	{
		Silkworm_Node* node = context->nodes[i];

		if (node != NULL)
		{
			float xDistance = (float)fabs(x - node->position.x);
			float yDistance = (float)fabs(y - node->position.y);

			float squareDistance = xDistance * xDistance + yDistance * yDistance;

			if (squareDistance <= radius * radius)
			{
				Silkworm_Cloth* cloth = LookupCloth(node->clothReference.clothId);

				if (cloth != NULL)
				{
					return cloth;
				}
			}
		}
	}

	return NULL;
}

void Silkworm_ClearAll()
{
	uint32_t i;

	for (i = 0; i < context->clothCount; i += 1)
	{
		if (context->cloths[i] != NULL)
		{
			Silkworm_ClothDestroy(context->cloths[i]);
		}
	}

	Silkworm_PerformDestroys();

	context->timeElapsed = 0;
}

void Silkworm_Finish()
{
	Silkworm_ClearAll();

	free(context->nodes);
	free(context->links);
	free(context->cloths);

	free(context->nodeIndexStack);
	free(context->linkIndexStack);
	free(context->clothIndexStack);

	free(context->nodeDestructionData);
	free(context->linkDestructionData);

	free(context);
	context = NULL;
}