#include "../../../shared_cpp/Renderer2d.h"
#include "../../../shared_cpp/types.h"
#include "../../../shared_cpp/mathlib.h"
#include <cstdio>
#include <math.h>

Vector2 GRAVITY_ACCELERATION = Vector2(0, -100.f);

struct PointMassUpdateData {
	int32 index = 0;
	Vector2 localPosition;  // Position is in world coordinates
	Vector2 worldPosition;  // Position is in world coordinates
	Vector2 acceleration;
	Vector2 velocity;
	Vector2 force;

	bool isHovered = false;

	PointMassUpdateData* neighbors[4];
};

struct SoftbodyRectangle {
	// User defined
	float32 width = 200;
	float32 height = 200;
	int32 springDensity = 16;
	float32 k = 8000.f; // in N /m
	float32 c = 30.f;
	float32 jointMassKg = 1.f;
	float32 floorPosition = 200;

	// Calculated before runtime
    Vector2 springDimensions;

	// Runtime data
	PointMassUpdateData* updateData = NULL;
	float32 rotationalVelocity = 0.f;
	float32 rotation = 0.f;

	// Render data
	Mesh2d mesh;
	Mesh2d pointsMesh;
	Mesh2d floorMesh;
	Vertex2d* vertices = NULL;
	Vertex2d* pointsVertices = NULL;

	void load(Renderer2d* renderer) {
		auto worldPosition = Vector2(800.f / 2 - width / 2, 400.f);
		springDimensions = Vector2(width / springDensity, height / springDensity);
	    int32 numVertices = springDensity * springDensity; // Each subdivision is a square.
		int32 numIndices = 6 * ((springDensity - 1) * (springDensity - 1));
	    vertices = new Vertex2d[numVertices];
		updateData = new PointMassUpdateData[numVertices];
		pointsVertices = new Vertex2d[numVertices];
		auto indices = new GLuint[numIndices];

		// -- Load a square with the desired density
		int32 vIdx = 0;
		int32 iIdx = 0;
		float32 inverseDensity = 1.f / springDensity;
		float32 halfInv = inverseDensity / 2.f;
		float32 rotation = PI / 8.f;
		for (int32 y = 0; y < springDensity; y++) {  // Rows
			for (int32 x = 0; x < springDensity; x++) { // Columns
			    Vector2 vpos = Vector2(
					x * inverseDensity - halfInv,
					y * inverseDensity - halfInv
				);
				vpos = vpos.rotate(rotation);
				// Get the position in model coordinates
				vpos.x = vpos.x * width;
				vpos.y = vpos.y * height;
				updateData[vIdx].localPosition = vpos;
				// Get the position in world coorodinates
				vpos.x = vpos.x + worldPosition.x;
				vpos.y = vpos.y + worldPosition.y;
				vertices[vIdx] = { vpos, Vector4(1, 0, 0, 1) };
				updateData[vIdx].index = vIdx;
				updateData[vIdx].worldPosition = vpos;
				updateData[vIdx].force = Vector2(0, 0);
				updateData[vIdx].velocity = Vector2(0, 0);
				updateData[vIdx].acceleration = Vector2(0, 0);

				if (x != springDensity - 1) updateData[vIdx].neighbors[0] = &updateData[vIdx + 1]; // Right
				else                        updateData[vIdx].neighbors[0] = NULL;
				if (y != springDensity - 1) updateData[vIdx].neighbors[1] = &updateData[vIdx + springDensity]; // Bottom
				else                        updateData[vIdx].neighbors[1] = NULL;
				if (x != 0)                 updateData[vIdx].neighbors[2] = &updateData[vIdx - 1]; // Left
				else                        updateData[vIdx].neighbors[2] = NULL;
				if (y != 0)                 updateData[vIdx].neighbors[3] = &updateData[vIdx - springDensity]; // Top
				else                        updateData[vIdx].neighbors[3] = NULL;
				

				if (y != springDensity - 1 && x != springDensity - 1) {
					indices[iIdx++] = vIdx;
					indices[iIdx++] = vIdx + 1;
					indices[iIdx++] = vIdx + springDensity;
					indices[iIdx++] = vIdx + springDensity;
					indices[iIdx++] = vIdx + springDensity + 1;
					indices[iIdx++] = vIdx + 1;
				}

				pointsVertices[vIdx].position = vpos;
				pointsVertices[vIdx].color = Vector4(0, 0, 0, 1);
				
				vIdx++;
			}
		}

		mesh.load(vertices, numVertices, indices, numIndices, renderer, GL_DYNAMIC_DRAW);
		pointsMesh.load(pointsVertices, numVertices, renderer, GL_DYNAMIC_DRAW);
		delete [] indices;

		// -- Load the floor line;
		Vector2 floorDimensions = Vector2(renderer->context->width, 8);
		Vector4 floorColor = Vector4(0.5, 0.5, 0.5, 1);
	    Vertex2d floorVertices[6];
	    floorVertices[0] = { Vector4(0, floorPosition, 0, 1), floorColor };
		floorVertices[1] = { Vector4(floorDimensions.x, floorPosition, 0, 1), floorColor };
		floorVertices[2] = { Vector4(0, floorPosition - floorDimensions.y, 0, 1), floorColor };
		floorVertices[3] = { Vector4(0, floorPosition - floorDimensions.y, 0, 1), floorColor };
		floorVertices[4] = { Vector4(floorDimensions.x, floorPosition - floorDimensions.y, 0, 1), floorColor };
		floorVertices[5] = { Vector4(floorDimensions.x, floorPosition, 0, 1), floorColor };
		floorMesh.load(floorVertices, 6, renderer);
	}

	Vector2 getForceBetweenPointMasses(PointMassUpdateData* first, PointMassUpdateData* second) {
		const float32 EPSILON = 0.1f;
		auto relativeVelocity = second->velocity - first->velocity;
		auto restLength = (second->localPosition - first->localPosition).length();
		auto relativePosition = second->worldPosition - first->worldPosition;
		auto currentLength = relativePosition.length();
		auto positionDir = relativePosition.normalize();
		auto velDotProduct = positionDir.dot(relativeVelocity);
		auto lengthDifference = (currentLength - restLength);
		if (ABS(lengthDifference) < EPSILON) {
			lengthDifference = 0;
		}
	    float32 springForce = k * lengthDifference;
        float32 dampingForce = c * velDotProduct;
        float32 totalForce = springForce + dampingForce;
        return positionDir * totalForce;
	}

	void update(float32 dtSeconds) {
		// -- Apply all forces
	    for (int32 v = 0; v < pointsMesh.numVertices; v++) {
			auto pointMass = &updateData[v];
			pointMass->force += GRAVITY_ACCELERATION * jointMassKg;

			// -- Add the forces from it's neighbors. Note that we only do the first two
			// neighbors, which are the right and bottom neighbors.
			for (int32 n = 0; n < 4; n++) {
				auto neighbor = pointMass->neighbors[n];
				if (neighbor == NULL) continue;

				auto forceBetween = getForceBetweenPointMasses(pointMass, neighbor);
				pointMass->force = pointMass->force + forceBetween;
			}
		}

		// -- Euler integrate and update the local position of each vertex.
		for (int32 v = 0; v < pointsMesh.numVertices; v++) {
			auto pointMass = &updateData[v];
			
			// -- Euler integration to find the current velocity and position
			pointMass->acceleration = pointMass->force / jointMassKg;
			pointMass->velocity = pointMass->velocity + (pointMass->acceleration * dtSeconds);
			pointMass->worldPosition = pointMass->worldPosition + (pointMass->velocity * dtSeconds);

			vertices[v].position = pointMass->worldPosition;
			pointsVertices[v].position = pointMass->worldPosition;
			pointMass->force = Vector2(); // Reset the force for the next update
		}

		// -- Collision detection
		for (int32 v = 0; v < pointsMesh.numVertices; v++) {
			auto pointMass = &updateData[v];
			auto prevPos = pointMass->worldPosition;

			// -- Floor
			particleFloorCollision(pointMass, prevPos, dtSeconds);

			// 
			const float32 COLLISION_DISTANCE = 0.3f;
			// auto localVector = pointMass->worldPosition - worldPosition;
			// auto displacement = (pointMass->worldPosition - worldPosition) - pointMass->localPosition;
			// if (displacement.length() >= 20.f) {
			// 	// auto positionNormal = (pointMass->restingPosition - pointMass->currentPosition).normalize();
			// 	// pointMass->currentPosition = pointMass->restingPosition - positionNormal * COLLISION_DISTANCE;
			// 	// float32 dotProduct = pointMass->velocity.dot(positionNormal);
			// 	// pointMass->velocity = pointMass->velocity - positionNormal * (2 * dotProduct);
			// }
			for (int32 n = 0; n < 4; n++) {
				auto neighbor = pointMass->neighbors[n];
				if (neighbor == NULL) continue;

				if ((neighbor->worldPosition - pointMass->worldPosition).length() < COLLISION_DISTANCE) {
					auto positionNormal = (neighbor->worldPosition - pointMass->worldPosition).normalize();
					pointMass->worldPosition = neighbor->worldPosition - positionNormal * COLLISION_DISTANCE;
					float32 dotProduct = pointMass->velocity.dot(positionNormal);
					pointMass->velocity = pointMass->velocity - positionNormal * (2 * dotProduct);
				}
			}
		}


		// -- Update vertices
		mesh.updateVertices(vertices);
		pointsMesh.updateVertices(pointsVertices);
	}

    void particleFloorCollision(PointMassUpdateData* ud, Vector2 prevPos, float32 dtSeconds) {
	    // We assume that the floor is always horizontal for this simulation
	    auto dotProduct = ud->velocity.dot(Vector2(0, 1));
		// if (dotProduct >= 0) {
		// 	return; // Not moving in the same direction
		// }

		if (ud->worldPosition.y - floorPosition < 0.1f) {
		    ud->worldPosition.y = floorPosition - 0.1f;;
		    ud->velocity = (ud->velocity - Vector2(0, 1) * (2 * dotProduct)) * 0.5f;
		}
	}

	void render(Renderer2d* renderer) {
		mesh.render(renderer);
		pointsMesh.render(renderer, GL_POINTS);
		floorMesh.render(renderer);
	}

	void unload() {
		mesh.unload();
		pointsMesh.unload();
		delete [] vertices;
		delete [] pointsVertices;
	}
};