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

// Side note: It is Eastertime, so I chose this easter color palette. Enjoy: https://htmlcolors.com/palette/144/easter

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 = 0.7f;
    float32 momentOfInertia = 0.f;

    void reset() {
        force = { 0, 0 };
        velocity = { 0, 0 };
    }

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

    void applyGravity() {
        force += Vector2 { 0.f, -100.f };
    }

    void update(float32 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 Pill {
    OrthographicShape shape;
    Rigidbody body;
    float32 a = 0;
    float32 b = 0;

    Pill copy() {
        Pill retval;
        retval.shape = shape;
        retval.body = body;
        retval.a = a;
        retval.b = b;
        return retval;
    }

    void load(OrthographicRenderer* renderer, float32 numSegments, float32 width, float32 height) {
        // Note that a so-called "pill" is simply an ellipse.
        // Equation of an ellipse is:
        //
        //	x^2 / a^2 + y^2 / b^2 = 1
        //
        // or, in parametric form:
        //
        // x = a * cos(t), y = b * sin(t)
        //
        float32 angleIncrements = (2.f * PI) / numSegments;
        uint32 numVertices = static_cast<uint32>(numSegments * 3.f);
        OrthographicVertex* vertices = new OrthographicVertex[numVertices];

        a = width / 2.f;
        b = height / 2.f;

        Vector4 color = Vector4().fromColor(243,166,207, 255);

        for (uint32 vertexIndex = 0; vertexIndex < numVertices; vertexIndex += 3) {
            // Create a single "slice" of the ellipse (like a pizza)
            float32 currAngle = (vertexIndex / 3.f) * angleIncrements;
            float32 nextAngle = (vertexIndex / 3.f + 1.f) * angleIncrements;

            vertices[vertexIndex].position = Vector2 { 0.f, 0.f };
            vertices[vertexIndex].color = color;

            vertices[vertexIndex + 1].position = Vector2 { a * cosf(currAngle), b * sinf(currAngle) };
            vertices[vertexIndex + 1].color = color;

            vertices[vertexIndex + 2].position = Vector2 { a * cosf(nextAngle), b * sinf(nextAngle) };
            vertices[vertexIndex + 2].color = color;
        }

        shape.load(vertices, numVertices, renderer);
        body.reset();

        // https://byjus.com/jee/moment-of-inertia-of-ellipse/
        body.momentOfInertia = (body.mass * (a * a + b * b)) / 4.f;

        a = width / 2.f;
        b = height / 2.f;

        delete[] vertices;
    }

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

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

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

    float32 getArea() {
        return 0.f;
    }
};

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 = 100000.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();
    }

    bool isPointOnLine(Vector2 p) {
        // If the dot product is nearly zero, that means it is in the direction of the line
        if (ABS((end - start).dot(p - start)) <= 0.001) {

            // We're on the general line, now let's see if we're within the proper bounds:
            return p.x >= start.x && p.x <= end.x && p.x >= start.y && p.y <= start.y;
        }
        
        return false;
    }
};

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

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();
IntersectionResult getIntersection(Pill* pill, LineSegment* segment);
void resolveCollision(Rigidbody* first, Rigidbody* second, IntersectionResult* ir);

// Global Variables
WebglContext context;
OrthographicRenderer renderer;
Pill pill;
MainLoop mainLoop;
LineSegment segmentList[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);

    pill.body.position = Vector2 { context.width / 2.f, context.height / 2.f };
    pill.body.rotationalVelocity = 0.3f;
    pill.load(&renderer, 64, 100.f, 50.f);

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

    mainLoop.run(update);
}

float32 areaOfTriangle(Vector2 a, Vector2 b, Vector2 c) {
    // Refernce for this for the formula: https://www.onlinemath4all.com/area-of-triangle-using-determinant-formula.html
    return ABS(0.5 * (a.x * b.y - b.x * a.y + b.x * c.y - c.x * b.y + c.x * a.y - a.x * c.y));
}

IntersectionResult getIntersection(Pill* pill, LineSegment* segment) {
    IntersectionResult result;
    
    /**

    Formula for finding equation of pill:
    1) Using the parametrized equation of a line:
        x(t) = x1 + (x2 - x1) * t
        y(t) = y1 + (y2 - y1) * t
    2) Knowing the equation of a translated and rotated ellipse:

        Define x and y as follows:

        x = x_1 + l, where l is the translation in x
        y = y_1 + h, where h is the translation in y

        Therefore, the general equation of a rotated ellipse is:

        (x * cos(theta) + y * sin(theta)) / (a * a)
            + (y * cos(theta) - x * sin(theta)) / b * b = 1

        ( + l + x * cos(theta) + y * sin(theta)) / (a * a)
            + (y + h + y * cos(theta) + y * sin(theta)) / (a * a)

    3) Plug in the parametrized equation of our line for x_1 and y_2.

    4) If the determinant >= 0, we have an intersection, otherwise, nothing.

    **/
    

    /*float32 rotationAngleOfPill = pill->body.rotation;
    Vector2 translationOfPill = pill->body.position;
    
    float32 cosRotation = cosf(rotationAngleOfPill);
    float32 sinRotation = sinf(rotationAngleOfPill);
    float32 cosRotationSquared = cosRotation * cosRotation;
    float32 sinRotationSquared = sinRotation * sinRotation;
    float32 aSquared = pill->a * pill->a;
    float32 bSquared = pill->b * pill->b;

    float32 x = (segment->end.x - segment->start.x) - translationOfPill.x;
    float32 xSquared = x * x;
    float32 y = (segment->end.y - segment->start.y) - translationOfPill.y;
    float32 ySquared = y * y;

    float32 A_XPart = (segment->end.x - segment->start.x - translationOfPill.x) * cosRotationSquared;
    float32 A_YPart = (segment->end.y - segment->start.y - translationOfPill.y) * sinRotationSquared;
    float32 A = ((A_XPart * A_XPart) / aSquared) + ((A_YPart * A_YPart) / bSquared);

    float32 B_XPart = 2 * (segment->start.x - translationOfPill.x) * (segment->end.x - segment->start.x - translationOfPill.x);
    float32 B = 2 * cosRotation * sinRotation * (1.f / aSquared - 1.f / bSquared) * (x * y);
    float32 C = (sinRotationSquared / aSquared + cosRotationSquared / bSquared) * ySquared;*/

    Mat4x4 inverseModel = pill->shape.model.inverse();
    Vector2 start = inverseModel * segment->start;
    Vector2 end = inverseModel * segment->end;
    float32 A = pow((end.x - start.x), 2.f) / pow(pill->a, 2) + pow((end.y - start.y), 2.f) / pow(pill->b, 2);
    float32 B = ((2 * start.x) * (end.x - start.x)) / pow(pill->a, 2) + ((2 * start.y) * (end.y - start.y)) / pow(pill->b, 2);
    float32 C = pow(start.x, 2) / pow(pill->a, 2) + pow(start.y, 2) / pow(pill->b, 2) - 1.f;

    float32 determinant = B * B - 4 * A * C;
    if (determinant < 0.f) {    
        result.intersect = false;
        return result;
    }

    float32 t1 = -B + determinant / (2 * A);
    float32 t2 = -B - determinant / (2 * A);

    Vector2 pointAtT1 = pill->shape.model * Vector2 { segment->start.x + (t1 * (segment->end.x - segment->start.x)), segment->start.y + (t1 * (segment->end.y - segment->start.y)) };
    Vector2 pointAtT2 = { segment->start.x + (t2 * (segment->end.x - segment->start.x)), segment->start.y + (t2 * (segment->end.y - segment->start.y)) };

    result.intersect = true;
    result.relativeVelocity = pill->body.velocity - segment->body.velocity;;
    result.collisionNormal = (pointAtT1 - pill->body.position).normalize();
    result.firstPointOfApplication = pointAtT1 - pill->body.position;
    result.secondPointOfApplication = pointAtT1 - ((segment->end - segment->start) / 2.f);

    return result;
}

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 cofOfRestition = (first->cofOfRestition + second->cofOfRestition) / 2.f;
    float32 lNumerator = (relativeVelocity * -(1.0 + cofOfRestition)).dot(collisionNormal);
    float32 lLinearDenomPart = collisionNormal.dot(collisionNormal * (1 / first->mass + 1 / second->mass));
    float32 lRotationalDenomPart = powf(firstPerp.dot(collisionNormal), 2) / first->momentOfInertia
        + powf(secondPerp.dot(collisionNormal), 2) / second->momentOfInertia;

    float32 lImpulseMagnitude = lNumerator / (lLinearDenomPart);// + lRotationalDenomPart);

    first->velocity = first->velocity + (collisionNormal * (lImpulseMagnitude / first->mass));
    second->velocity = second->velocity - (collisionNormal * (lImpulseMagnitude / second->mass));

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

void update(float32 deltaTimeSeconds, void* userData) {

    // Input
    pill.body.applyGravity();

    // Update
    Pill copyPill = pill.copy();
    pill.update(deltaTimeSeconds);

    // Intersections
    for (int32 lineIdx = 0; lineIdx < 4; lineIdx++) {
        IntersectionResult ir = getIntersection(&pill, &segmentList[lineIdx]);
        if (ir.intersect) {

            // Find the exact moment that the intersection happens by rewinding the simulation until we're not intersecting
            IntersectionResult subIr = ir;
            float32 subdividedTimeSeconds = deltaTimeSeconds;
            do {
                ir = subIr;

                pill = copyPill.copy();
                subdividedTimeSeconds /= 2.f;
                pill.update(subdividedTimeSeconds);

                subIr = getIntersection(&pill, &segmentList[lineIdx]);
                if (subdividedTimeSeconds == 0.f) {
                    printf("Error: Should not be happening.\n");
                    break;
                }
            } while (subIr.intersect);

            printf("Found intersection at timestamp: %f\n", subdividedTimeSeconds);
            resolveCollision(&pill.body, &segmentList[lineIdx].body, &ir);
            pill.update(deltaTimeSeconds - subdividedTimeSeconds);
        }
    }

    // Render
    renderer.render();
    pill.shape.render(&renderer);

    for (int32 segmentIndex = 0; segmentIndex < 4; segmentIndex++) {
        segmentList[segmentIndex].render(&renderer);
    }
}

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