#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>

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;
	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) {
		body.update(dtSeconds);
		shape.model = Mat4x4().translateByVec2(body.position).rotate2D(body.rotation);
	}

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

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

struct LineSegment {
    OrthographicShape shape;
    Rigidbody body;
    Vector2 start;
    Vector2 end;
    float32 length;
    Vector2 normal;
    OrthographicVertex vertices[2];

    void load(OrthographicRenderer* renderer, Vector4 color, Vector2 inStart, Vector2 inEnd) {
        start = inStart;
        end = inEnd;
        length = (start - end).length();
        vertices[0].position = start;
        vertices[0].color = color;
        vertices[1].position = end;
        vertices[1].color = color;
        normal = (end - start).getPerp().normalize();
        shape.load(vertices, 2, renderer);

        body.reset();
        body.mass = 1000000000.f;
        body.cofOfRestition = 1.f;
        body.rotationalVelocity = 0;
        body.velocity = Vector2();
        body.momentOfInertia = body.mass * (length / 2.f);
    }

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

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

    Vector2 getPointOnLine(float32 t) {
        return {
            start.x + (end.x - start.x) * t,
            start.y + (end.y - start.y) * t,
        };
    }

    Vector2 getNormal() {
        return (end - start).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;
LineSegment segmentList[4];
Rectangle rectangle;

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);

	segmentList[0].load(&renderer, Vector4().fromColor(191, 251, 146, 255.f), Vector2 { 50.f, 0.f }, Vector2 { 50.f, static_cast<float>(context.height) });
    segmentList[1].load(&renderer, Vector4().fromColor(159, 224, 210, 255.f), Vector2 { context.width - 50.f, 0.f }, Vector2 { context.width - 50.f, static_cast<float>(context.height) });
    segmentList[2].load(&renderer, Vector4().fromColor(248, 255, 156, 255.f), Vector2 { 50.f, 50.f }, Vector2 { context.width - 50.f, 150.f });
    segmentList[3].load(&renderer, Vector4().fromColor(205, 178, 214, 255.f), Vector2 { 50.f, 150.f }, Vector2 { context.width - 50.f, 50.f });

	rectangle.load(&renderer, Vector4 { 230.f, 182.f, 35.f, 255.f }, 64.f, 32.f);
	rectangle.body.position = Vector2 { context.width / 3.f, context.height / 2.f };

    mainLoop.run(update);
}

float32 getDistancePointToLine(Vector2 point, LineSegment* segment) {
	float32 xLength = segment->end.x - segment->start.x;
	float32 yLength = segment->end.y - segment->start.y;
	return fabs(xLength * (segment->start.y - point.y) - (segment->start.x - point.x) * yLength)
		/ sqrtf((xLength * xLength) + (yLength * yLength));
}

const float32 EPSILON = 1.f;
IntersectionResult getIntersection(Rectangle* rectangle, LineSegment* segment) {
	IntersectionResult ir;
	
	float32 halfWidth = rectangle->width / 2.f;
	float32 halfHeight = rectangle->height / 2.f;
	
	Vector2 bottomLeft = rectangle->shape.model * Vector2 { -halfWidth, -halfHeight };
	Vector2 topLeft = rectangle->shape.model * Vector2 { -halfWidth, halfHeight };
	Vector2 bottomRight = rectangle->shape.model * Vector2 { halfWidth, -halfHeight };
	Vector2 topRight = rectangle->shape.model * Vector2 { halfWidth, halfHeight };

	Vector2 collisionPoint;

	if (getDistancePointToLine(bottomLeft, segment) <= EPSILON) {
		ir.intersect = true;
		collisionPoint = bottomLeft;
	} else if (getDistancePointToLine(topLeft, segment) <= EPSILON) {
		ir.intersect = true;
		collisionPoint = topLeft;
	} else if (getDistancePointToLine(bottomRight, segment) <= EPSILON) {
		ir.intersect = true;
		collisionPoint = bottomRight;
	} else if (getDistancePointToLine(topRight, segment) <= EPSILON) {
		ir.intersect = true;
		collisionPoint = topRight;
	} else {
		ir.intersect = false;
		return ir;
	}

	ir.collisionNormal = segment->getNormal();
	ir.relativeVelocity = rectangle->body.velocity - segment->body.velocity;
	ir.firstPointOfApplication = (collisionPoint - rectangle->body.position);
	ir.secondPointOfApplication = collisionPoint - segment->getPointOnLine(0.5f);

	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
	Rectangle rectCopy = rectangle;
	rectangle.update(deltaTimeSeconds);

	for (int32 segmentIndex = 0; segmentIndex < 4; segmentIndex++) {
		IntersectionResult ir = getIntersection(&rectangle, &segmentList[segmentIndex]);
		if (!ir.intersect) {
		    continue;
		}

		// Handle collison here
		IntersectionResult irCopy = ir;
		float32 copyDt = deltaTimeSeconds;
		
		do {
			ir = irCopy;
			rectangle = rectCopy;
			copyDt = copyDt /= 2.f;

			rectangle.update(copyDt);
			irCopy = getIntersection(&rectangle, &segmentList[segmentIndex]);

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

		} while (irCopy.intersect);

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

		resolveCollision(&rectangle.body, &segmentList[segmentIndex].body, &ir);
		float32 frameTimeRemaining = deltaTimeSeconds - copyDt;
		
		update(frameTimeRemaining, userData);
		return;
    }
	
	// Renderer
	renderer.render();
	rectangle.render(&renderer);
	for (int32 segmentIndex = 0; segmentIndex < 4; segmentIndex++) {
        segmentList[segmentIndex].render(&renderer);
    }
}

void unload() {
    mainLoop.stop();
    renderer.unload();
	for (int32 segmentIndex = 0; segmentIndex < 4; segmentIndex++) {
        segmentList[segmentIndex].unload();
    }
	rectangle.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;
}