#include "../../../shared_cpp/OrthographicRenderer.h"
#include "../../../shared_cpp/types.h"
#include "../../../shared_cpp/WebglContext.h"
#include "../../../shared_cpp/mathlib.h"
#include "../../../shared_cpp/MainLoop.h"
#include <cstdio>
#include <cmath>
#include <emscripten/html5.h>
#include <unistd.h>
#include <pthread.h>
#include <cmath>
#include <cfloat>

struct Rigidbody {
    Vector2 force = { 0, 0 };
    Vector2 velocity = { 0, 0 };
    Vector2 position = { 0, 0 };
    float32 rotationalVelocity  = 0.f;
    float32 rotation = 0.f;
    float32 mass = 1.f;
    float32 cofOfRestition = 1.f;
    float32 momentOfInertia = 0.f;

    void reset() {
        force = { 0, 0 };
        velocity = { 0, 0 };
        rotationalVelocity = 0.f;
        rotation = 0.f;
    }

    void applyForce(Vector2 f) {
        force += f;
    }

    void applyGravity(float32 deltaTimeSeconds) {
        velocity += (Vector2 { 0.f, -96.f } * deltaTimeSeconds);
    }

    void update(float32 deltaTimeSeconds) {
        applyGravity(deltaTimeSeconds);
        
        Vector2 acceleration = force / mass;
        velocity += (acceleration * deltaTimeSeconds);
        position += (velocity * deltaTimeSeconds);
        force = Vector2 { 0.f, 0.f };

        rotation += (rotationalVelocity * deltaTimeSeconds);
    }

    void setMomentOfInertia(float32 moi) {
        momentOfInertia = moi;
    }
};

struct IntersectionResult {
    bool intersect = false;
    Vector2 collisionNormal;
    Vector2 relativeVelocity;
    Vector2 firstPointOfApplication;
    Vector2 secondPointOfApplication;
};

struct Rectangle {
	OrthographicShape shape;
	Rigidbody body;
	Rigidbody previousBody;
	Vector4 color;
	float32 width = 0.f;
	float32 height = 0.f;

	void load(OrthographicRenderer* renderer, Vector4 inColor, float32 inWidth, float32 inHeight) {
		color = inColor.toNormalizedColor();
		width = inWidth;;
		height = inHeight;

		float32 halfWidth = width / 2.f;
		float32 halfHeight = height / 2.f;

	    OrthographicVertex vertices[6];
		vertices[0].position = Vector2 { -halfWidth, -halfHeight };
		vertices[1].position = Vector2 { -halfWidth, halfHeight };
		vertices[2].position = Vector2 { halfWidth, halfHeight };
		vertices[3].position = Vector2 { -halfWidth, -halfHeight };
		vertices[4].position = Vector2 { halfWidth, -halfHeight };
		vertices[5].position = Vector2 { halfWidth, halfHeight };
		
		for (int32 i = 0; i < 6; i++) {
			vertices[i].color = color;
		}

		shape.load(vertices, 6, renderer);
		body.reset();
		body.momentOfInertia = (1.f / 12.f) * body.mass * (width + height * height * height);
	}

	void update(float32 dtSeconds) {
		previousBody = body;
		
		body.update(dtSeconds);
		shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);
	}

	void render(OrthographicRenderer* renderer) {
		shape.render(renderer);
	}

	void unload() {
		shape.unload();
	}

	void restorePreviousBody() {
		body = previousBody;
	}

    Vector2 getPoint(int index) {
        switch (index) {
            case 0: return shape.model * Vector2 { -width / 2.f, -height / 2.f };
            case 1: return shape.model * Vector2 { -width / 2.f, height / 2.f };
            case 2: return  shape.model * Vector2 { width / 2.f, height / 2.f };
            case 3: return  shape.model * Vector2 { width / 2.f, -height / 2.f };
            default: {
                printf("Unable to find point: index=%d", index);
                return Vector2();
            }
        }
    }

	// Note that these getters are needlessly verbose for demonstration's sake
	void getPoints(Vector2* pointList) {
		Vector2 botLeft = shape.model * Vector2 { -width / 2.f, -height / 2.f };
		Vector2 topLeft = shape.model * Vector2 { -width / 2.f, height / 2.f };
		Vector2 topRight = shape.model * Vector2 { width / 2.f, height / 2.f };
		Vector2 botRight = shape.model * Vector2 { width / 2.f, -height / 2.f };

		pointList[0] = botLeft;
		pointList[1] = topLeft;
		pointList[2] = topRight;
		pointList[3] = botRight;
	}

    void getEdges(Vector2* edgeList) {
		Vector2 pointsList[4];
		getPoints(pointsList);

		for (int i = 0; i < 4; i++) {
			if (i + 1 == 4) {
				edgeList[i] =  pointsList[i] - pointsList[0];
			} else {
				edgeList[i] =  pointsList[i] - pointsList[i + 1];
			}
		}
	}

	void getNormals(Vector2* normalList) {
		Vector2 edgeList[4];
		getEdges(edgeList);

		for (int i = 0; i < 4; i++) {
			normalList[i] = edgeList[i].getPerp().normalize();
		}
	}
};

EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);
EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData);

void load();
void update(float32 time, void* userData);
void unload();

WebglContext context;
OrthographicRenderer renderer;
MainLoop mainLoop;
Rectangle rectangleList[4];

int main() {
	context.init("#gl_canvas");
    emscripten_set_click_callback("#gl_canvas_play", NULL, false, onPlayClicked);
    emscripten_set_click_callback("#gl_canvas_stop", NULL, false, onStopClicked);
    return 0;
}

void load() {
    renderer.load(&context);

	rectangleList[0].load(&renderer, Vector4 { 235.f, 35.f, 35.f, 255.f }, 24.f, 32.f);
	rectangleList[0].body.position = Vector2 { context.width / 3.f, context.height / 3.f };
	rectangleList[0].body.velocity = Vector2 { 100.f, 250.f };
	rectangleList[0].body.rotation = 0.2f;
	
	rectangleList[1].load(&renderer, Vector4 { 35.f, 235.f, 35.f, 255.f }, 64.f, 96.f);
	rectangleList[1].body.position = Vector2 { context.width / 3.f, context.height * (2.f / 3.f) };
	rectangleList[1].body.rotation = 1.3f;
	
	rectangleList[2].load(&renderer, Vector4 { 235.f, 35.f, 235.f, 255.f }, 64.f, 32.f);
	rectangleList[2].body.position = Vector2 { context.width * (2.f / 3.f), context.height / 3.f };
	rectangleList[2].body.velocity = Vector2 { -100.f, 250.f };
	rectangleList[2].body.rotation = -0.5f;
	
	rectangleList[3].load(&renderer, Vector4 { 35.f, 235.f, 235.f, 255.f }, 8.f, 16.f);
	rectangleList[3].body.position = Vector2 { context.width * (2.f / 3.f), context.height * (2.f / 3.f) };
	rectangleList[3].body.rotation = -3.23f;

    mainLoop.run(update);
}

Vector2 getProjection(Vector2* vertices, Vector2 axis) {
	float32 min = axis.dot(vertices[0]);
	float32 max = min;

	for (int v = 1; v < 4; v++) {
		float32 d = axis.dot(vertices[v]);

		if (d < min) {
			min = d;
		} else if (d > max) {
			max = d;
		}
	}

	return Vector2 { min, max };
}

bool projectionsOverlap(Vector2 first, Vector2 second) {
	return first.x <= second.y && second.x <= first.y;
}

float32 getProjectionOverlap(Vector2 first, Vector2 second) {
	float32 firstOverlap = fabs(first.x - second.y);
	float32 secondOverlap = fabs(second.x - first.y);
	return firstOverlap > secondOverlap ? secondOverlap : firstOverlap;
}

const float32 EPSILON = 1.f;
IntersectionResult getIntersection(Rectangle* first, Rectangle* second) {
	IntersectionResult ir;

	// For two rectangles to overlap, it means that at least one of the corners of one is inside of the other
	Vector2 firstNormals[4];
	first->getNormals(firstNormals);
	Vector2 firstPoints[4];
	first->getPoints(firstPoints);

	Vector2 secondNormals[4];
	second->getNormals(secondNormals);
	Vector2 secondPoints[4];
    second->getPoints(secondPoints);

	float32 minOverlap = FLT_MAX;
	Vector2 minOverlapAxis;
    bool minOverlapAxisIsFromFirstRectangle = true;
    

	for (int i = 0; i < 4; i++) {
		Vector2 normal = firstNormals[i];

	    Vector2 firstProj = getProjection(firstPoints, normal);
		Vector2 secondProj = getProjection(secondPoints, normal);

		if (!projectionsOverlap(firstProj, secondProj)) {
			return ir;
		}

		float32 overlap = getProjectionOverlap(firstProj, secondProj);
		if (overlap < minOverlap) {
			minOverlap = overlap;
			minOverlapAxis = normal;
            minOverlapAxisIsFromFirstRectangle = true;
		}
	}

	for (int i = 0; i < 4; i++) {
		Vector2 normal = secondNormals[i];

	    Vector2 firstProj = getProjection(firstPoints, normal);
		Vector2 secondProj = getProjection(secondPoints, normal);

		if (!projectionsOverlap(firstProj, secondProj)) {
			return ir;
		}

		float32 overlap = getProjectionOverlap(firstProj, secondProj);
		if (overlap < minOverlap) {
			minOverlap = overlap;
			minOverlapAxis = normal;
            minOverlapAxisIsFromFirstRectangle = false;
		}
	}

	ir.intersect = true;
	ir.relativeVelocity = first->body.velocity - second->body.velocity;
	ir.collisionNormal = minOverlapAxis;

    // Find the point of collision, this is kind of tricky, and it is just an approximation for now.
    // At this point, we know that we intersected along the minOverlapAxis, but we do not know where
    // that exactly happened. To remedy this will, we create two parallel lines: one at the top of the
    // normal area, and one at the bottom. For point on both of the Rectangles, we will check:
    // (1) if it is between these two planes
    // (2) if, for that rectangle, it is the closest point to the original normal vector
    // (3) or if it is equally distant from normal vector as another point (then this is a "flat" collision)
    //
    // The collision point MUST be between these two planes. We can then say the corner/face of the non-monoverlapAxis 
    // Rectangle is the collision point. This enables us to then solve for their respective points of application fairly
    // easily. If the collision "point" is an entire face, we make the collision point be the center point.
    //
    Vector2 topPlane;
    Vector2 bottomPlane;

    for (int p = 0; p < 4; p++) {
        Vector2 point = minOverlapAxisIsFromFirstRectangle ? first->getPoint(p) : second->getPoint(p);

    }

    ir.firstPointOfApplication = Vector2();
    ir.secondPointOfApplication = Vector2();

	return ir;
}

void resolveCollision(Rigidbody* first, Rigidbody* second, IntersectionResult* ir) {
    Vector2 relativeVelocity = ir->relativeVelocity;
    Vector2 collisionNormal  = ir->collisionNormal;
    Vector2 firstPerp        = ir->firstPointOfApplication.getPerp();
    Vector2 secondPerp       = ir->secondPointOfApplication.getPerp();
	float32 firstPerpNorm    = firstPerp.dot(collisionNormal);
	float32 sndPerpNorm      = secondPerp.dot(collisionNormal);

    float32 cofOfRestition = (first->cofOfRestition + second->cofOfRestition) / 2.f;
    float32 numerator = (relativeVelocity * (-1 * (1.f + cofOfRestition))).dot(collisionNormal);
    float32 linearDenomPart = collisionNormal.dot(collisionNormal * (1.f / first->mass + 1.f / second->mass));
	float32 rotationalDenomPart = (firstPerpNorm * firstPerpNorm) / first->momentOfInertia + (sndPerpNorm * sndPerpNorm) / second->momentOfInertia;

    float32 impulseMagnitude = numerator / (linearDenomPart + rotationalDenomPart);
    first->velocity = first->velocity + (collisionNormal * (impulseMagnitude / first->mass));
	second->velocity = second->velocity - (collisionNormal * (impulseMagnitude / second->mass));

    first->rotationalVelocity = first->rotationalVelocity + firstPerp.dot(collisionNormal * impulseMagnitude) / first->momentOfInertia;
    second->rotationalVelocity = second->rotationalVelocity - secondPerp.dot(collisionNormal * impulseMagnitude) / second->momentOfInertia;
}

void update(float32 deltaTimeSeconds, void* userData) {
	// Update
	for (int r = 0; r < 4; r++) {
		rectangleList[r].update(deltaTimeSeconds);
	}

	// Check collisions with other rectangles
	for (int i = 0; i < 4; i++) {
		Rectangle* first = &rectangleList[i];
		for (int j = i + 1; j < 4; j++) {
			Rectangle* second = &rectangleList[j];
			
			IntersectionResult ir = getIntersection(first, second);
			if (!ir.intersect) {
				continue;
			}

			// Handle collison here
			IntersectionResult irCopy = ir;
			float32 copyDt = deltaTimeSeconds;
		
			do {
				first->restorePreviousBody();
				second->restorePreviousBody();
				
				ir = irCopy;
				copyDt = copyDt /= 2.f;

			    first->update(copyDt);
				second->update(copyDt);
				
				irCopy = getIntersection(first, second);

				if (copyDt <= 0.f) {
					printf("Error: Should not be happening.\n");
					break;
				}

			} while (irCopy.intersect);

			printf("Found intersection at timestamp: %f\n", copyDt);

			resolveCollision(&first->body, &second->body, &ir);
			float32 frameTimeRemaining = deltaTimeSeconds - copyDt;

			first->update(frameTimeRemaining);
			second->update(frameTimeRemaining);
		}
	}

    // Check collisions with walls
    for (int r = 0; r < 4; r++) {
        Rectangle* rect = &rectangleList[r];
        if (rect->body.position.x <= 0.f) {
            rect->body.velocity = rect->body.velocity - Vector2 { 1.f, 0.f } * (2 * (rect->body.velocity.dot(Vector2 { 1.f, 0.f })));
        }
        if (rect->body.position.y <= 0.f) {
            rect->body.velocity = rect->body.velocity - Vector2 { 0.f, 1.f } * (2 * (rect->body.velocity.dot(Vector2 { 0.f, 1.f })));
        } 
        if (rect->body.position.x >= 640.f) {
            rect->body.velocity = rect->body.velocity - Vector2 { -1.f, 0.f } * (2 * (rect->body.velocity.dot(Vector2{ -1.f, 0.f })));
        }
        if (rect->body.position.y >= 480.f) {
            rect->body.velocity = rect->body.velocity - Vector2 { 0.f, -1.f } * (2 * (rect->body.velocity.dot(Vector2 { 0.f, -1.f }))) ;
        }
    }
	
	// Renderer
	renderer.render();
	for (int r = 0; r < 4; r++) {
		rectangleList[r].render(&renderer);
	}
}

void unload() {
    mainLoop.stop();
    renderer.unload();
	for (int r = 0; r < 4; r++) {
		rectangleList[r].unload();
	}
}

//
// Interactions with DOM handled below
//
EM_BOOL onPlayClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData) {
    printf("Play clicked\n");
    
    load();
    return true;
}

EM_BOOL onStopClicked(int eventType, const EmscriptenMouseEvent* mouseEvent, void* userData) {
    printf("Stop clicked\n");
    unload();
    return true;
}