/* Silkworm - Verlet cloth physics in C
 *
 * Copyright (c) 2021 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 "Silkworm.h"

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

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* nodeIndicesToDestroy;
	uint32_t nodeIndicesToDestroyCount;

	uint64_t* linkIndicesToDestroy;
	uint32_t linkIndicesToDestroyCount;

	uint64_t* nodeIndexStack;
	uint32_t nodeIndexStackCount;

	uint64_t* linkIndexStack;
	uint32_t linkIndexStackCount;

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

	uint8_t* currentBufferAddress; /* GM doesnt let you pass more than 4 arguments with different types lol */
} 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->nodeIndicesToDestroy = NULL;
	context->nodeIndicesToDestroyCount = 0;

	context->linkIndicesToDestroy = NULL;
	context->linkIndicesToDestroyCount = 0;

	context->nodeIndexStack = NULL;
	context->nodeIndexStackCount = 0;

	context->linkIndexStack = NULL;
	context->linkIndexStackCount = 0;

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

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

void Silkworm_Update(double deltaTime)
{
	uint32_t i, j;
	Silkworm_Link *link;
	Silkworm_Node *node;

	float delta = (float)deltaTime;

	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)
				{
					context->linkIndicesToDestroy = realloc(context->linkIndicesToDestroy, sizeof(uint64_t) * (context->linkIndicesToDestroyCount + 1));
					context->linkIndicesToDestroy[context->linkIndicesToDestroyCount] = link->id;
					context->linkIndicesToDestroyCount += 1;
				}
				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->velocity.x = velocityX * delta;
				node->velocity.y = velocityY * delta;

				if (fabs(node->position.x) > context->xBound || fabs(node->position.x) > context->yBound)
				{
					context->nodeIndicesToDestroy = realloc(context->nodeIndicesToDestroy, sizeof(uint64_t) * (context->nodeIndicesToDestroyCount + 1));
					context->nodeIndicesToDestroy[context->nodeIndicesToDestroyCount] = node->id;
					context->nodeIndicesToDestroyCount += 1;
				}
			}
		}
	}

	for (j = 0; j < context->nodeIndicesToDestroyCount; j += 1)
	{
		uint64_t nodeIndex = context->nodeIndicesToDestroy[j];

		free(context->nodes[nodeIndex]);
		context->nodes[nodeIndex] = NULL;

		context->nodeIndexStack = realloc(context->nodeIndexStack, sizeof(uint64_t) * (context->nodeIndexStackCount + 1));
		context->nodeIndexStack[context->nodeIndexStackCount] = nodeIndex;
		context->nodeIndexStackCount += 1;
	}

	for (j = 0; j < context->linkIndicesToDestroyCount; j += 1)
	{
		uint64_t linkIndex = context->linkIndicesToDestroy[j];

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

		context->linkIndexStack = realloc(context->linkIndexStack, sizeof(uint64_t) * (context->linkIndexStackCount + 1));
		context->linkIndexStack[context->linkIndexStackCount] = linkIndex;
		context->linkIndexStackCount += 1;
	}

	context->nodeIndicesToDestroyCount = 0;
	context->linkIndicesToDestroyCount = 0;
}

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

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

double Silkworm_CreateNode(double xPosition, double yPosition, double mass, double friction, double radius, double pushFactor)
{
	Silkworm_Node* node = malloc(sizeof(Silkworm_Node));
	uint64_t id;

	if (context->nodeIndexStackCount > 0)
	{
		id = context->nodeIndexStack[context->nodeIndexStackCount - 1];
		context->nodeIndexStack = realloc(context->nodeIndexStack, sizeof(uint64_t) * (context->nodeIndexStackCount - 1));
		context->nodeIndexStackCount -= 1;

		context->nodes[id] = node;
	}
	else
	{
		id = context->nodeCount;

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

	node->id = id;
	node->position.x = (float)xPosition;
	node->position.y = (float)yPosition;
	node->previousPosition.x = (float)xPosition;
	node->previousPosition.y = (float)yPosition;
	node->velocity.x = 0;
	node->velocity.y = 0;
	node->acceleration.x = 0;
	node->acceleration.y = 0;
	node->mass = (float)mass;
	node->friction = (float)friction;
	node->radius = (float)radius;
	node->pushFactor = (float)pushFactor;
	node->pinned = false;
	node->destroyable = false;

	return (double)node->id;
}

void Silkworm_NodeSetVelocity(double nodeId, double xVelocity, double yVelocity)
{
	LookupNode(nodeId)->velocity.x = (float)xVelocity;
	LookupNode(nodeId)->velocity.y = (float)yVelocity;
}

void Silkworm_NodeSetAcceleration(double nodeId, double xAcceleration, double yAcceleration)
{
	LookupNode(nodeId)->acceleration.x = (float)xAcceleration;
	LookupNode(nodeId)->acceleration.y = (float)yAcceleration;
}

void Silkworm_NodeSetDestroyable(double nodeId)
{
	LookupNode(nodeId)->destroyable = true;
}

void Silkworm_ClothNodePin(double clothId, double i, double j)
{
	Silkworm_Cloth* cloth = LookupCloth(clothId);
	context->nodes[cloth->nodeIndices[(uint64_t)i][(uint64_t)j]]->pinned = true;
}

void Silkworm_ClothNodeUnpin(double clothId, double i, double j)
{
	Silkworm_Cloth* cloth = LookupCloth(clothId);
	context->nodes[cloth->nodeIndices[(uint64_t)i][(uint64_t)j]]->pinned = false;
}

double Silkworm_CreateLink(double aId, double bId, double distance, double tearThreshold)
{
	Silkworm_Node *nodeA = LookupNode(aId);
	Silkworm_Node *nodeB = LookupNode(bId);

	uint64_t id;

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

	if (context->linkIndexStackCount > 0)
	{
		id = context->linkIndexStack[context->linkIndexStackCount - 1];
		context->linkIndexStack = realloc(context->linkIndexStack, sizeof(uint64_t) * (context->linkIndexStackCount - 1));
		context->linkIndexStackCount -= 1;

		context->links[id] = link;
	}
	else 
	{
		id = context->linkCount;

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

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

	return (double)link->id;
}

double Silkworm_CreateCloth(double xPosition, double yPosition, double horizontalNodeCount, double verticalNodeCount, double mass, double friction, double windFactor, double tearThreshold)
{
	int32_t i, j;

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

	cloth->windFactor = (float)windFactor;
	cloth->horizontalNodeCount = (uint32_t) horizontalNodeCount;
	cloth->verticalNodeCount = (uint32_t) verticalNodeCount;
	cloth->nodeIndices = malloc(sizeof(uint64_t*) * cloth->horizontalNodeCount);

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

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

		for (j = 0; j < verticalNodeCount; j += 1)
		{
			uint64_t nodeId = (uint64_t) Silkworm_CreateNode(xPosition + i * context->clothDensity, yPosition + j * context->clothDensity, mass, friction, 1, 0.5);

			cloth->nodeIndices[i][j] = nodeId;

			if (j == 0)
			{
				Silkworm_ClothNodePin((double)cloth->id, (double)i, (double)j);
			}

			Silkworm_NodeSetDestroyable((double)nodeId);
		}
	}

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

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

				cloth->triangles[triangleIndex]->a = context->nodes[cloth->nodeIndices[i][j]];
				cloth->triangles[triangleIndex]->b = context->nodes[cloth->nodeIndices[i + 1][j]];
				cloth->triangles[triangleIndex]->c = context->nodes[cloth->nodeIndices[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;

				triangleIndex += 1;
			}

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

				cloth->triangles[triangleIndex]->a = context->nodes[cloth->nodeIndices[i][j]];
				cloth->triangles[triangleIndex]->b = context->nodes[cloth->nodeIndices[i - 1][j]];
				cloth->triangles[triangleIndex]->c = context->nodes[cloth->nodeIndices[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;

				triangleIndex += 1;
			}
		}
	}

	cloth->triangleCount = triangleIndex;

	for (i = 0; i < horizontalNodeCount; i += 1)
	{
		for (j = 0; j < verticalNodeCount; j += 1)
		{
			if (i - 1 >= 0)
			{
				Silkworm_CreateLink((double)cloth->nodeIndices[i - 1][j], (double)cloth->nodeIndices[i][j], context->clothDensity, tearThreshold);
			}

			if (j - 1 >= 0)
			{
				Silkworm_CreateLink((double)cloth->nodeIndices[i][j - 1], (double)cloth->nodeIndices[i][j], context->clothDensity, tearThreshold);
			}
		}
	}

	return (double)cloth->id;
}

void Silkworm_SetTriangleBuffer(const char* bufferId)
{
	context->currentBufferAddress = (uint8_t*)bufferId;
}

/* pattern is x, y position then color + alpha then UV position */
double Silkworm_ClothFillTriangleBuffer(double clothId, double leftUV, double widthUV, double topUV, double heightUV)
{
	uint32_t i, triangleCount;
	uint8_t* bufferAddress = context->currentBufferAddress;
	Silkworm_Cloth* cloth = LookupCloth(clothId);

	Silkworm_Color color;
	color.r = 255;
	color.g = 255;
	color.b = 255;
	color.a = 255;

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

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

				memcpy(bufferAddress, &cloth->triangles[i]->a->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &left, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &top, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &cloth->triangles[i]->b->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &right, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &top, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &cloth->triangles[i]->c->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &left, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &bottom, sizeof(float));
				bufferAddress += sizeof(float);

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

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

				memcpy(bufferAddress, &cloth->triangles[i]->a->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &right, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &bottom, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &cloth->triangles[i]->b->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &left, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &bottom, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &cloth->triangles[i]->c->position, sizeof(Silkworm_Vector2));
				bufferAddress += sizeof(Silkworm_Vector2);

				memcpy(bufferAddress, &color, sizeof(Silkworm_Color));
				bufferAddress += sizeof(Silkworm_Color);

				memcpy(bufferAddress, &right, sizeof(float));
				bufferAddress += sizeof(float);

				memcpy(bufferAddress, &top, sizeof(float));
				bufferAddress += sizeof(float);

				triangleCount += 1;
			}
		}
	}

	return (double)triangleCount;
}