#include <windows.h>
#include <gl/gl.h>
#include <stdio.h>
#include <stdint.h>
#include <stddef.h>
#include <assert.h>

#include <glm\glm.hpp>
#include <glm\gtc\matrix_transform.hpp>
#include <glm\gtx\norm.hpp>
#include <glm\gtx\matrix_decompose.hpp>
#include <glm\gtx\euler_angles.hpp>
using namespace glm;

static const float Pi32 = glm::pi<float>();
static const float Tau32 = Pi32 * 2.0f;

typedef int32_t B32;

#define EDIT_MODE 0

#define SwapPtr(type, a, b) { void* tmp = a; a = b; b = (type*)tmp; }
#define ArrayCount(Array) (sizeof(Array) / sizeof((Array)[0]))

inline vec2 Cross(const vec2 &a, float s) {
    return vec2(s * a.y, -s * a.x);
}
inline vec2 Cross(float s, const vec2 &a) {
    return vec2(-s * a.y, s * a.x);
}
inline float Cross(const vec2 &a, const vec2 &b) {
    return a.x * b.y - a.y * b.x;
}
inline float Clamp(float min, float value, float max) {
    float result = value;
    if (result < min) {
        result = min;
    } else if (result > max) {
        result = max;
    }
    return(result);
}

struct RenderState {
    vec2 gameSize;
    ivec2 screenSize;
    ivec2 viewportSize;
    ivec2 viewportOffset;
    float renderScale;
    float cameraScale;
    vec2 cameraOffset;
};

struct ButtonState {
    int halfTransitionCount;
    B32 endedDown;
};
inline B32 WasDown(ButtonState state) {
    B32 result = ((state.halfTransitionCount > 1) || ((state.halfTransitionCount == 1) && (state.endedDown)));
    return(result);
}
inline B32 IsDown(ButtonState state) {
    B32 result = state.endedDown;
    return(result);
}

static const int MouseButton_Count = 5;
struct MouseState {
    vec2 pos;
    union {
        ButtonState buttons[MouseButton_Count];
        struct {
            ButtonState leftMouseButton;
            ButtonState middleMouseButton;
            ButtonState rightMouseButton;
            ButtonState xMouseButton1;
            ButtonState xMouseButton2;
        };
    };
};

static const int KeyboardButton_Count = 4;
struct KeyboardState {
    union {
        ButtonState buttons[KeyboardButton_Count];
        struct {
            ButtonState space;
            ButtonState f1;
            ButtonState q;
            ButtonState e;
        };
    };
};

struct InputState {
    MouseState mouse;
    KeyboardState keyboard;
};

inline vec2 Unproject(RenderState *renderState, int x, int y) {
    vec2 result = {};
    if (renderState->renderScale > 0) {
        result.x = (float)((x - renderState->viewportOffset.x) / (renderState->renderScale * renderState->cameraScale));
        result.y = (float)((y - renderState->viewportOffset.y) / (renderState->renderScale * renderState->cameraScale));
        result.x -= (renderState->gameSize.x * 0.5f / renderState->cameraScale) + renderState->cameraOffset.x;
        result.y -= (renderState->gameSize.y * 0.5f / renderState->cameraScale) + renderState->cameraOffset.y;
    }
    return (result);
}

static const float GameWidthInMeters = 10.0f;
static const float GameAspect = 16.0f / 9.0f;

struct Transform {
    vec2 translation;
    mat2 rotation;
};
inline vec2 operator *(const vec2 &v, const Transform &t) {
    vec2 result = t.rotation * v + t.translation;
    return(result);
}

enum ShapeType {
    ShapeType_Plane = 1,
    ShapeType_Circle = 2,
    ShapeType_Polygon = 3,
};
struct PlaneShape {
    float visualLength;
    // Normal is computed from rotation
    // Distance is computed from position
};
struct CircleShape {
    float radius;
};
static const int MaxPolygonVertexCount = 16;
struct PolygonShape {
    vec2 localVerts[MaxPolygonVertexCount];
    int numVerts;
};
struct Shape {
    ShapeType type;
    union {
        CircleShape circle;
        PlaneShape plane;
        PolygonShape poly;
    };
};

struct MassData {
    vec2 centroid;
    float mass;
    float inertia;
};

enum BodyType {
    Static = 0,
    Dynamic = 1,
};
struct Body {
    vec2 position;
    float rotation;
    vec2 velocity;
    float angularVelocity;
    vec2 force;
    float torque;
    vec2 centroid;
    BodyType type;
    Shape shape;
    float mass;
    float invMass;
    float inertia;
    float invInertia;
};

struct Contact {
    vec2 normal;
    vec2 pointOnA;
    vec2 relativeArmA;
    vec2 relativeArmB;
    float distance;
    float accumulatedNormalImpulse;
    float accumulatedTangentImpulse;
    float normalMassRatio;
    float tangentMassRatio;
};

struct Face {
    int index;
    vec2 points[2];
};

struct Arbiter {
    Contact contacts[2];
    int numContacts;
    Body *bodyA;
    Body *bodyB;
    float friction;
};

#define CONTACT_GENERATOR(name) int name(const Transform &transformA, Shape *shapeA, const Transform &transformB, Shape *shapeB, Contact *contacts)
typedef CONTACT_GENERATOR(generate_contacts);

static const int MaxBodyCount = 1024;
static const int MaxArbiterCount = MaxBodyCount * 2;
static const int SolverIterationCount = 4;

static const float InteractionRadius = 0.4f;

enum InteractionType {
    InteractionType_None = 0,
    InteractionType_MoveRotate = 1,
};

struct InteractionNone {
};

struct InteractionDragging {
    vec2 startPos;
};

struct InteractionState {
    Body *hotBody;
    Body *activeBody;
    InteractionType type;
    union {
        InteractionNone none;
        InteractionDragging dragging;
    };
};

struct World {
    vec2 boundaryExt;
    Body* bodies;
    int numBodies;
    vec2 gravity;
    Arbiter *arbiters;
    int numArbiters;
    bool stepEnabled;
    bool step;
    InteractionState interaction;
};

inline Body *AddBody(World *world, BodyType type, vec2 pos, float rotation = 0.0f) {
    assert(world->numBodies < MaxBodyCount);
    Body *result = &world->bodies[world->numBodies++];
    memset(result, 0, sizeof(Body));
    result->type = type;
    result->position = pos;
    result->rotation = rotation;
    return(result);
}

inline void ApplyMassData(Body *body, MassData *massData) {
    if (body->type == BodyType::Dynamic) {
        body->mass = massData->mass;
        body->inertia = massData->inertia;
        body->invMass = body->mass > 0 ? (1.0f / body->mass) : 0;
        body->invInertia = body->inertia > 0 ? (1.0f / body->inertia) : 0;
        body->centroid = massData->centroid;
    } else {
        body->mass = body->inertia = 0;
        body->invMass = body->invInertia = 0;
        body->centroid = vec2(0, 0);
    }
}

inline void DefinePlane(Body *body, float visualLength = 10.0f) {
    memset(&body->shape, 0, sizeof(Shape));
    body->shape.type = ShapeType::ShapeType_Plane;
    body->shape.plane.visualLength = visualLength;
    body->mass = body->invMass = body->inertia = body->invInertia = 0;
}

inline MassData ComputeCircleMassData(CircleShape *circle, float density) {
    MassData result = {};
    result.mass = Pi32 * circle->radius * circle->radius * density;
    result.centroid = vec2(0);
    result.inertia = result.mass * (0.5f * circle->radius * circle->radius + glm::length2(result.centroid));
    return(result);
}
inline void DefineCircle(Body *body, float radius, float density = 1.0f) {
    memset(&body->shape, 0, sizeof(Shape));
    body->shape.type = ShapeType::ShapeType_Circle;
    body->shape.circle.radius = radius;
    MassData massData = ComputeCircleMassData(&body->shape.circle, density);
    ApplyMassData(body, &massData);
}

static vec2 GetPolygonCentroid(int numVerts, vec2 *verts) {
    const float inv3 = 1.0f / 3.0f;
    vec2 result = vec2(0);
    float area = 0.0f;
    for (int vertIndex = 0; vertIndex < numVerts; vertIndex++) {
        vec2 p0 = verts[vertIndex];
        vec2 p1 = verts[(vertIndex + 1) % numVerts];
        float d = (p0.x * p1.y) - (p1.x * p0.y);
        area += d * 0.5f;
        result += (p0 + p1) * d * inv3;
    }
    if (area > 0) {
        result *= (1.0f / area);
    }
    return(result);
}
static MassData ComputePolygonMassData(PolygonShape *polygon, float density) {
    float area = 0.0f;
    const float inv3 = 1.0f / 3.0f;
    float I = 0.0f;
    for (int vertIndex = 0; vertIndex < polygon->numVerts; vertIndex++) {
        vec2 p0 = polygon->localVerts[vertIndex];
        vec2 p1 = polygon->localVerts[(vertIndex + 1) % polygon->numVerts];
        float d = (p0.x * p1.y) - (p1.x * p0.y);
        float triangleArea = d * 0.5f;
        area += triangleArea;
        float ex1 = p0.x, ey1 = p0.y;
        float ex2 = p1.x, ey2 = p1.y;
        float intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
        float inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
        I += (0.25f * inv3 * d) * (intx2 + inty2);
    }
    MassData result = {};
    result.mass = area * density;
    result.centroid = GetPolygonCentroid(polygon->numVerts, &polygon->localVerts[0]);
    result.inertia = I * density + (result.mass * glm::length2(result.centroid));
    return(result);
}
inline void DefineBox(Body *body, vec2 radius, float density = 1.0f) {
    memset(&body->shape, 0, sizeof(Shape));
    body->shape.type = ShapeType::ShapeType_Polygon;
    body->shape.poly.numVerts = 4;
    body->shape.poly.localVerts[0] = vec2(radius.x, radius.y);
    body->shape.poly.localVerts[1] = vec2(-radius.x, radius.y);
    body->shape.poly.localVerts[2] = vec2(-radius.x, -radius.y);
    body->shape.poly.localVerts[3] = vec2(radius.x, -radius.y);
    MassData massData = ComputePolygonMassData(&body->shape.poly, density);
    ApplyMassData(body, &massData);
}

// -----------------------------------------
// INITIALIZATION
// -----------------------------------------

static void GameInit(World *world) {
    memset(world, 0, sizeof(World));
    world->stepEnabled = true;
    world->bodies = (Body *)malloc(sizeof(Body) * MaxBodyCount);
    world->arbiters = (Arbiter *)malloc(sizeof(Arbiter) * MaxArbiterCount);
    world->boundaryExt = vec2(GameWidthInMeters, GameWidthInMeters / GameAspect) * 0.5f;
    world->gravity = vec2(0, -10);

    DefinePlane(AddBody(world, BodyType::Static, vec2(0, -2), glm::radians(90.0f)));
    //DefinePlane(AddBody(world, BodyType::Static, vec2(-world->boundaryExt.x, 0), glm::radians(0.0f)));
    //DefinePlane(AddBody(world, BodyType::Static, vec2(world->boundaryExt.x, 0), glm::radians(180.0f)));
    //DefineBox(AddBody(world, BodyType::Static, vec2(0, -2)), vec2(1, 1));

    float radius = 0.5f;
    float spaceBetween = 0.1f;
    const int numDynamicBodies = 2;
    float sumDistance = radius * 2 * numDynamicBodies + (spaceBetween * numDynamicBodies - 1);
    for (int i = 0; i < numDynamicBodies; ++i) {
        float x = 0;
        float y = 0 + i * radius * 2 + i * spaceBetween;
        //DefineCircle(AddBody(world, BodyType::Dynamic, vec2(x, y)), 0.5f);
        DefineBox(AddBody(world, BodyType::Dynamic, vec2(x, y)), vec2(radius));
    }
}

// -----------------------------------------
// UPDATE
// -----------------------------------------

static vec2 GetSupportPoint(vec2 normal, int vertexCount, vec2 *vertices) {
    vec2 result = vertices[0];
    float distance = glm::dot(result, normal);
    for (int vertexIndex = 1; vertexIndex < vertexCount; ++vertexIndex) {
        vec2 v = vertices[vertexIndex];
        float p = glm::dot(v, normal);
        if (p > distance) {
            distance = p;
            result = v;
        }
    }
    return (result);
}

static Face GetFace(int vertexCount, vec2 *verts, vec2 normal) {
    int firstIndex = 0;
    float firstDistance = glm::dot(verts[0], normal);
    for (int vertexIndex = 1; vertexIndex < vertexCount; vertexIndex++) {
        vec2 v = verts[vertexIndex];
        float p = glm::dot(v, normal);
        if (p > firstDistance) {
            firstDistance = p;
            firstIndex = vertexIndex;
        }
    }

    int secondIndex = -1;
    float secondDistance = 0;
    for (int vertexIndex = 0; vertexIndex < vertexCount; vertexIndex++) {
        if (vertexIndex != firstIndex) {
            vec2 v = verts[vertexIndex];
            float p = glm::dot(v, normal);
            if (secondIndex == -1 || p > secondDistance) {
                secondDistance = p;
                secondIndex = vertexIndex;
            }
        }
    }
    assert(secondIndex > -1);
    Face result = {};
    result.index = firstIndex;
    result.points[0] = verts[firstIndex];
    result.points[1] = verts[secondIndex];
    return (result);
}

struct ClipResult {
    vec2 points[2];
};
static ClipResult Clip(vec2 normal, vec2 ref1, vec2 ref2, vec2 inc1, vec2 inc2) {
    vec2 tangent = Cross(normal, 1.0f);
    vec2 segmentDistance = inc2 - inc1;
    vec2 segmentToLine0 = ref1 - inc1;
    vec2 segmentToLine1 = ref2 - inc1;
    float d0 = glm::dot(segmentToLine0, tangent);
    float d1 = glm::dot(segmentToLine1, tangent);
    float f0 = d0 / glm::dot(segmentDistance, tangent);
    float f1 = d1 / glm::dot(segmentDistance, tangent);
    ClipResult result = {};
    result.points[0] = inc1 + segmentDistance * Clamp(0, f0, 1);
    result.points[1] = inc1 + segmentDistance * Clamp(0, f1, 1);
    return (result);
}

struct SATResult {
    float distance;
    vec2 normal;
};
static SATResult QuerySAT(const Transform &transformA, const Transform &transformB, int numVertsA, vec2 *localVertsA, int numVertsB, vec2 *localVertsB) {
    SATResult result = {};

    mat2 rotAToB = transformA.rotation * glm::transpose(transformB.rotation);

    B32 first = true;
    for (int vertIndexA = 0; vertIndexA < numVertsA; vertIndexA++) {
        // Create normal for A
        vec2 v0 = localVertsA[vertIndexA];
        vec2 v1 = localVertsA[(vertIndexA + 1) % numVertsA];
        vec2 t = glm::normalize(v1 - v0);
        vec2 localNormalA = Cross(t, 1.0f);

        // Bring normal A into its own space
        vec2 normalForA = transformA.rotation * localNormalA;
        // Bring normal A into the space of B
        vec2 normalForB = rotAToB * localNormalA;

        // Get support point for B
        vec2 supportPointB = GetSupportPoint(-normalForB, numVertsB, localVertsB);

        // Get closest distance
        vec2 pA = v0 * transformA;
        vec2 pB = supportPointB * transformB;
        vec2 pAB = pB - pA;
        float proj = glm::dot(pAB, normalForA);

        // Record axis of minimum penetration
        if (first || proj > result.distance) {
            first = false;
            result.normal = localNormalA;
            result.distance = proj;
        }
    }
    return(result);
}

static vec2 GetClosestPointOnLineSegment(vec2 point, vec2 a, vec2 b, float *region) {
    vec2 lineAB = b - a;
    vec2 pointToLine = point - a;
    *region = glm::dot(pointToLine, lineAB) / glm::length2(lineAB);
    float percentage = Clamp(0, *region, 1);
    vec2 result = a + lineAB * percentage;
    return (result);
}

struct ManifoldInput {
    B32 flip;
    vec2 localNormalA;
    Transform transformA;
    Transform transformB;
    mat2 rotAtoB;
    vec2 *localVertsA;
    vec2 *localVertsB;
    int numVertsA;
    int numVertsB;
};
inline ManifoldInput MakeInputManifold(B32 flip, vec2 localNormalA, Transform transformA, Transform transformB, int numVertsA, vec2 *localVertsA, int numVertsB, vec2 *localVertsB) {
    ManifoldInput result = {};
    result.flip = flip;
    result.localNormalA = localNormalA;
    result.transformA = transformA;
    result.transformB = transformB;
    result.localVertsA = localVertsA;
    result.localVertsB = localVertsB;
    result.numVertsA = numVertsA;
    result.numVertsB = numVertsB;
    result.rotAtoB = transformA.rotation * glm::transpose(transformB.rotation);
    return (result);
}

struct ManifoldOutput {
    vec2 points[2];
    float distances[2];
    int features[2];
    vec2 normal;
    int count;
};

static CONTACT_GENERATOR(ContactGeneratorPlaneCircle) {
    int result = 0;

    PlaneShape *plane = &shapeA->plane;
    CircleShape *circle = &shapeB->circle;

    vec2 posA = transformA.translation;
    vec2 posB = transformB.translation;
    vec2 normal = transformA.rotation[0];

    vec2 distanceToPlane = posA - posB;
    float projDistance = glm::dot(distanceToPlane, normal);
    float projRadius = -circle->radius;
    float d = projRadius - projDistance;
    vec2 pointOnA = posB + normal * projDistance;

    Contact *contact = &contacts[result++];
    *contact = {};
    contact->normal = normal;
    contact->distance = d;
    contact->pointOnA = pointOnA;

    return(result);
}

static CONTACT_GENERATOR(ContactGeneratorCircleCircle) {
    int result = 0;

    CircleShape *circleA = &shapeA->circle;
    CircleShape *circleB = &shapeB->circle;

    vec2 posA = transformA.translation;
    vec2 posB = transformB.translation;

    // Get normal from distance between circles
    vec2 distanceBetween = posB - posA;
    vec2 normal;
    if (glm::length2(distanceBetween) > 0) {
        normal = glm::normalize(distanceBetween);
    } else {
        normal = vec2(1, 0);
    }

    // Get distance and get closest point
    float projDistance = glm::dot(distanceBetween, normal);
    float bothRadius = circleA->radius + circleB->radius;
    float d = -(bothRadius - projDistance);
    vec2 pointOnA = posA + normal * circleA->radius;

    Contact *contact = &contacts[result++];
    *contact = {};
    contact->normal = normal;
    contact->distance = d;
    contact->pointOnA = pointOnA;
    return(result);
}

static CONTACT_GENERATOR(ContactGeneratorPlanePolygon) {
    int result = 0;

    PlaneShape *plane = &shapeA->plane;
    PolygonShape *poly = &shapeB->poly;

    vec2 posA = transformA.translation;
    vec2 normalA = transformA.rotation[0];
    mat2 rotAToB = glm::transpose(transformB.rotation);
    vec2 normalB = rotAToB * normalA;

    vec2 pointsOnA[2];
    float distanceToA[2];
    int clipTypeOnB[2];
    int numPoints = 0;

    Face faceB = GetFace(poly->numVerts, &poly->localVerts[0], -normalB);
    vec2 supportPoint1 = faceB.points[0] * transformB;
    vec2 planePoint = posA;
    vec2 distanceToPlane = supportPoint1 - planePoint;
    float d = glm::dot(distanceToPlane, normalA);
    if (d > 0) {
        pointsOnA[0] = supportPoint1 + normalA * -d;
        distanceToA[0] = d;
        numPoints = 1;
        clipTypeOnB[0] = 0;
    } else {
        vec2 supportPoint2 = faceB.points[1] * transformB;
        vec2 distance0 = supportPoint1 - planePoint;
        float d0 = glm::dot(distance0, normalA);
        if (d0 <= 0) {
            int idx = numPoints++;
            pointsOnA[idx] = supportPoint1 + normalA * -d0;
            distanceToA[idx] = d0;
            clipTypeOnB[idx] = 1;
        }

        vec2 distance1 = supportPoint2 - planePoint;
        float d1 = glm::dot(distance1, normalA);
        if (d0 <= 0) {
            int idx = numPoints++;
            pointsOnA[idx] = supportPoint2 + normalA * -d1;
            distanceToA[idx] = d1;
            clipTypeOnB[idx] = 2;
        }
    }

    for (int pointIndex = 0; pointIndex < numPoints; pointIndex++) {
        Contact *contact = &contacts[result++];
        *contact = {};
        contact->distance = distanceToA[pointIndex];
        contact->normal = normalA;
        contact->pointOnA = pointsOnA[pointIndex];
    }

    return(result);
}

static CONTACT_GENERATOR(ContactGeneratorPolygonPolygon) {
    int result = 0;

    PolygonShape *polyA = &shapeA->poly;
    PolygonShape *polyB = &shapeB->poly;

    vec2 posA = transformA.translation;

    // Get vertices for A and B
    vec2 *localVertsA = polyA->localVerts;
    vec2 *localVertsB = polyB->localVerts;
    int numVertsA = polyA->numVerts;
    int numVertsB = polyB->numVerts;

    // Query SAT for A -> B or B -> A
    SATResult resultA = QuerySAT(transformA, transformB, numVertsA, localVertsA, numVertsB, localVertsB);
    SATResult resultB = QuerySAT(transformB, transformA, numVertsB, localVertsB, numVertsA, localVertsA);

    // Create input manifold and give A more weight than B
    const float kRelTol = 0.99f;
    const float kAbsTol = 0.001f;
    ManifoldInput input;
    if (resultA.distance > kRelTol * resultB.distance + kAbsTol) {
        input = MakeInputManifold(false, resultA.normal, transformA, transformB, numVertsA, localVertsA, numVertsB, localVertsB);
    } else {
        input = MakeInputManifold(true, resultB.normal, transformB, transformA, numVertsB, localVertsB, numVertsA, localVertsA);
    }

    ManifoldOutput output = {};

    // Get face vertices for A and B
    Face faceA = GetFace(input.numVertsA, input.localVertsA, input.localNormalA);
    Face faceB = GetFace(input.numVertsB, input.localVertsB, -(input.rotAtoB * input.localNormalA));

    // Transform face vertices for A and B
    vec2 sA1 = faceA.points[0] * input.transformA;
    vec2 sA2 = faceA.points[1] * input.transformA;
    vec2 sB1 = faceB.points[0] * input.transformB;
    vec2 sB2 = faceB.points[1] * input.transformB;

    // Transform local normal A and set as final normal
    output.normal = input.transformA.rotation * input.localNormalA;

    if (resultA.distance > 0 || resultB.distance > 0) {
        // We are separated

        // Get closest points from B to A and from A to B
        float regionA, regionB;
        vec2 closestOnA = GetClosestPointOnLineSegment(sB1, sA1, sA2, &regionA);
        vec2 closestOnB = GetClosestPointOnLineSegment(closestOnA, sB1, sB2, &regionB);

        // Get the separation distance
        float distance = glm::dot(sB1 - closestOnA, output.normal);

        // Reconstruct output normal and distance based on distance for closest points
        if ((regionA < 0 || regionA > 1) || (regionB < 0 || regionB > 1)) {
            vec2 closestDistance = closestOnB - closestOnA;
            output.normal = glm::normalize(closestDistance);
            distance = glm::dot(closestDistance, output.normal);
        }

        // Write out points and distances
        int idx = output.count++;
        output.distances[idx] = distance;
        output.points[idx] = closestOnA;
    } else {
        // We are penetrating or touching

        // Clip face line segments (Incident front face against reference side planes)
        //              Keep points only when behind the reference front face
        //              Clip points are on B - need to project back to A
        ClipResult clipPoints = Clip(output.normal, sA1, sA2, sB1, sB2);
        vec2 clipDistance0 = clipPoints.points[0] - sA1;
        float distance0 = glm::dot(clipDistance0, output.normal);
        if (distance0 <= 0) {
            int idx = output.count++;
            output.points[idx] = clipPoints.points[0] + output.normal * -distance0;
            output.distances[idx] = distance0;
            output.features[idx] = 1;
        }
        vec2 clipDistance1 = clipPoints.points[1] - sA1;
        float distance1 = glm::dot(clipDistance1, output.normal);
        if (distance1 <= 0) {
            int idx = output.count++;
            output.points[idx] = clipPoints.points[1] + output.normal * -distance1;
            output.distances[idx] = distance1;
            output.features[idx] = 2;
        }
    }

    // Move points from A to B and invert normal (When B > A)
    if (input.flip) {
        for (int pointIndex = 0; pointIndex < output.count; pointIndex++) {
            output.points[pointIndex] += output.normal * output.distances[pointIndex];
        }
        output.normal *= -1.0f;
    }

    // Create final contact points
    for (int i = 0; i < output.count; i++) {
        Contact *contact = &contacts[result++];
        *contact = {};
        contact->distance = output.distances[i];
        contact->normal = output.normal;
        contact->pointOnA = output.points[i];
    }

    return(result);
}
inline Transform MakeTransform(vec2 pos, float angle) {
    Transform result;
    result.translation = pos;
    result.rotation = glm::orientate2(angle);
    return(result);
}
static void ApplyImpulse(Body *body, const vec2 &impulse, const vec2 &point) {
    body->velocity += impulse * body->invMass;
    body->angularVelocity += Cross(point, impulse) * body->invInertia;
}
static void GameUpdate(float dt, World *world, InputState *input) {
    // Update interaction
    Body *newHotBody = 0;
    for (int bodyIndex = 0; bodyIndex < world->numBodies; ++bodyIndex) {
        Body *body = world->bodies + bodyIndex;
        vec2 deltaPos = input->mouse.pos - body->position;
        if (glm::length2(deltaPos) <= InteractionRadius * InteractionRadius) {
            newHotBody = body;
        }
    }
    world->interaction.hotBody = newHotBody;
    if (IsDown(input->mouse.leftMouseButton)) {
        if (world->interaction.hotBody && !world->interaction.activeBody) {
            world->interaction.activeBody = world->interaction.hotBody;
            world->interaction.type = InteractionType::InteractionType_MoveRotate;
            world->interaction.dragging.startPos = input->mouse.pos;
        }
    } else {
        if (world->interaction.type != InteractionType::InteractionType_None) {
            world->interaction.activeBody = 0;
            world->interaction.type = InteractionType::InteractionType_None;
        }
    }

    if (world->interaction.type != InteractionType::InteractionType_None) {
        assert(world->interaction.activeBody);
        switch (world->interaction.type) {
        case InteractionType::InteractionType_MoveRotate:
        {
            vec2 deltaPos = input->mouse.pos - world->interaction.dragging.startPos;
#if EDIT_MODE
            world->interaction.activeBody->position += deltaPos;
#else
            world->interaction.activeBody->velocity += deltaPos * 10.0f;
#endif
            world->interaction.dragging.startPos = input->mouse.pos;
            float rotAmount = Tau32 / 100.0f;
            char buffer[255];
            if (WasDown(input->keyboard.q)) {
                world->interaction.activeBody->rotation += rotAmount;
                sprintf_s(buffer, ArrayCount(buffer), "Rotation to right: %f\n", world->interaction.activeBody->rotation);
                OutputDebugString(buffer);
            } else if (WasDown(input->keyboard.e)) {
                world->interaction.activeBody->rotation -= rotAmount;
                sprintf_s(buffer, ArrayCount(buffer), "Rotation to left: %f\n", world->interaction.activeBody->rotation);
                OutputDebugString(buffer);
            }
        }; break;
        }
    }

#if !EDIT_MODE
    if (WasDown(input->keyboard.f1)) {
        world->stepEnabled = !world->stepEnabled;
        world->step = false;
    } else if (WasDown(input->keyboard.space)) {
        if (world->stepEnabled) {
            world->step = true;
        }
    }

    // Exit out when single step enabled and no step is requested
    if (world->stepEnabled) {
        if (!world->step) return;
        world->step = false;
    }

    // Integrate acceleration
    for (int bodyIndex = 0; bodyIndex < world->numBodies; ++bodyIndex) {
        Body *body = world->bodies + bodyIndex;
        if (body->type == BodyType::Dynamic) {
            vec2 acceleration = world->gravity + (body->force * body->mass);
            body->velocity += acceleration * dt;
            body->angularVelocity += body->torque * body->invInertia;
        }
    }
#endif

    // Create arbiters (Body pairs)
    world->numArbiters = 0;
    for (int bodyIndexA = 0; bodyIndexA < world->numBodies; ++bodyIndexA) {
        Body *bodyA = world->bodies + bodyIndexA;
        for (int bodyIndexB = bodyIndexA + 1; bodyIndexB < world->numBodies; ++bodyIndexB) {
            Body *bodyB = world->bodies + bodyIndexB;
            if ((bodyA->type == BodyType::Dynamic) || (bodyB->type == BodyType::Dynamic)) {
                assert(world->numArbiters < MaxArbiterCount);
                Arbiter *arbiter = &world->arbiters[world->numArbiters++];
                *arbiter = {};
                arbiter->bodyA = bodyA;
                arbiter->bodyB = bodyB;
                // TODO: Fixed friction for now
                arbiter->friction = 0.4f;
            }
        }
    }

    // Create contacts
    for (int arbiterIndex = 0; arbiterIndex < world->numArbiters; ++arbiterIndex) {
        Arbiter *arbiter = world->arbiters + arbiterIndex;
        Shape* shapeA = &arbiter->bodyA->shape;
        Shape* shapeB = &arbiter->bodyB->shape;
        Body *bodyA = arbiter->bodyA;
        Body *bodyB = arbiter->bodyB;

        bool swapped = false;
        if (shapeA->type > shapeB->type) {
            shapeA = &arbiter->bodyB->shape;
            shapeB = &arbiter->bodyA->shape;
            bodyA = arbiter->bodyB;
            bodyB = arbiter->bodyA;
            swapped = true;
        }

        // Build transform
        Transform transformA = MakeTransform(bodyA->position, bodyA->rotation);
        Transform transformB = MakeTransform(bodyB->position, bodyB->rotation);

        // TODO: Implement a jump table to find a proper contact generation function call
        arbiter->numContacts = 0;
        if (shapeA->type == ShapeType::ShapeType_Plane && shapeB->type == ShapeType::ShapeType_Circle) {
            arbiter->numContacts = ContactGeneratorPlaneCircle(transformA, shapeA, transformB, shapeB, arbiter->contacts);
        } else if (shapeA->type == ShapeType::ShapeType_Plane && shapeB->type == ShapeType::ShapeType_Polygon) {
            arbiter->numContacts = ContactGeneratorPlanePolygon(transformA, shapeA, transformB, shapeB, arbiter->contacts);
        } else if (shapeA->type == ShapeType::ShapeType_Circle && shapeB->type == ShapeType::ShapeType_Circle) {
            arbiter->numContacts = ContactGeneratorCircleCircle(transformA, shapeA, transformB, shapeB, arbiter->contacts);
        } else if (shapeA->type == ShapeType::ShapeType_Polygon && shapeB->type == ShapeType::ShapeType_Polygon) {
            arbiter->numContacts = ContactGeneratorPolygonPolygon(transformA, shapeA, transformB, shapeB, arbiter->contacts);
        }

        if (swapped) {
            for (int contactIndex = 0; contactIndex < arbiter->numContacts; ++contactIndex) {
                Contact *contact = arbiter->contacts + contactIndex;
                // Move contact point to the other body
                contact->pointOnA = contact->pointOnA + contact->normal * contact->distance;
                // The normal must be flipped as well
                contact->normal *= -1.0f;
            }
        }

        // Initialize contacts
        for (int contactIndex = 0; contactIndex < arbiter->numContacts; ++contactIndex) {
            Contact *contact = arbiter->contacts + contactIndex;

            contact->accumulatedNormalImpulse = contact->accumulatedTangentImpulse = 0;

            // Get center of mass (World space)
            vec2 centerOfMassA = bodyA->position + bodyA->centroid;
            vec2 centerOfMassB = bodyB->position + bodyB->centroid;

            // Get contact points (World space)
            vec2 contactPointOnA = contact->pointOnA;
            vec2 contactPointOnB = contactPointOnA + contact->normal * contact->distance;

            // Get contact arms (Relative space)
            contact->relativeArmA = contactPointOnA - centerOfMassA;
            contact->relativeArmB = contactPointOnB - centerOfMassB;

            // Get normal rotation on the z-plane
            float rnA = Cross(contact->relativeArmA, contact->normal);
            float rnB = Cross(contact->relativeArmB, contact->normal);

            // Build normal mass ratio
            float normalTensor = bodyA->invMass + bodyB->invMass + bodyA->invInertia * rnA * rnA + bodyB->invInertia * rnB * rnB;
            contact->normalMassRatio = normalTensor > 0 ? 1.0f / normalTensor : 0;

            // Get tangent rotation in the z-plane
            vec2 tangent = Cross(contact->normal, 1.0f);
            float rtA = Cross(contact->relativeArmA, tangent);
            float rtB = Cross(contact->relativeArmB, tangent);

            // Build tangent matrix
            float tangentTensor = bodyA->invMass + bodyB->invMass + bodyA->invInertia * rtA * rtA + bodyB->invInertia * rtB * rtB;
            contact->tangentMassRatio = tangentTensor > 0 ? 1.0f / tangentTensor : 0;
        }
    }

#if !EDIT_MODE
    // Solve contacts
    for (int iterationIndex = 0; iterationIndex < SolverIterationCount; ++iterationIndex) {
        for (int arbiterIndex = 0; arbiterIndex < world->numArbiters; ++arbiterIndex) {
            Arbiter *arbiter = world->arbiters + arbiterIndex;
            Body *bodyA = arbiter->bodyA;
            Body *bodyB = arbiter->bodyB;
            for (int contactIndex = 0; contactIndex < arbiter->numContacts; ++contactIndex) {
                Contact *contact = arbiter->contacts + contactIndex;
                vec2 normal = contact->normal;

                // Relative velocity on contact = vB + cross(wB, rB) - vA - cross(wA, rA);
                vec2 vAB = bodyB->velocity + Cross(bodyB->angularVelocity, contact->relativeArmB) - bodyA->velocity - Cross(bodyA->angularVelocity, contact->relativeArmA);

                // Normal impulse
                float projNormalVel = glm::dot(vAB, normal);
                float remove = projNormalVel + contact->distance / dt;
                float normalImpulse = remove * contact->normalMassRatio;
                float newImpulse = glm::min(contact->accumulatedNormalImpulse + normalImpulse, 0.0f);
                float impulseChange = newImpulse - contact->accumulatedNormalImpulse;
                contact->accumulatedNormalImpulse = newImpulse;
                ApplyImpulse(bodyA, normal * impulseChange, contact->relativeArmA);
                ApplyImpulse(bodyB, -normal * impulseChange, contact->relativeArmB);

                // Tangent impulse
                vec2 tangent = Cross(normal, 1.0f);
                float projTangentVel = glm::dot(vAB, tangent);
                float maxFriction = arbiter->friction * contact->accumulatedNormalImpulse;
                float tangentImpulse = projTangentVel * contact->tangentMassRatio;
                float newTangentImpulse = Clamp(maxFriction, contact->accumulatedTangentImpulse + tangentImpulse, -maxFriction);
                float tangentChange = newTangentImpulse - contact->accumulatedTangentImpulse;
                contact->accumulatedTangentImpulse = newTangentImpulse;
                ApplyImpulse(bodyA, tangent * tangentChange, contact->relativeArmA);
                ApplyImpulse(bodyB, -tangent * tangentChange, contact->relativeArmB);
            }
        }
    }

    // Integrate velocity and clear forces
    for (int bodyIndex = 0; bodyIndex < world->numBodies; ++bodyIndex) {
        Body *body = world->bodies + bodyIndex;
        if (body->type == BodyType::Dynamic) {
            body->position += body->velocity * dt;
            body->rotation += body->angularVelocity * dt;
        }
        body->force = vec2(0, 0);
        body->torque = 0;
    }
#endif

    // Clear forces
    for (int bodyIndex = 0; bodyIndex < world->numBodies; ++bodyIndex) {
        Body *body = world->bodies + bodyIndex;
        body->force = vec2(0, 0);
        body->torque = 0;
    }
}

// -----------------------------------------
// RENDERING
// -----------------------------------------

static void DrawCircle(float radius, int numSegments = 32, bool filled = false) {
    float segmentLength = Tau32 / numSegments;
    glBegin(filled ? GL_POLYGON : GL_LINE_LOOP);
    for (int segmentIndex = 0; segmentIndex < numSegments; ++segmentIndex) {
        float angle = segmentIndex * segmentLength;
        float xpos = glm::cos(angle) * radius;
        float ypos = glm::sin(angle) * radius;
        glVertex2f(xpos, ypos);
    }
    glEnd();
}
static void DrawRotationAxis() {
    float axisLen = 0.25f;
    glBegin(GL_LINES);
    glColor3f(0, 1, 0);
    glVertex2f(axisLen, 0);
    glVertex2f(0, 0);
    glColor3f(1, 0, 0);
    glVertex2f(0, axisLen);
    glVertex2f(0, 0);
    glEnd();
}
static void DrawPatternLine(vec2 start, vec2 end, float patternWidth = 0.05f) {
    vec2 line = end - start;
    float lineDistance = glm::length(start - end);
    vec2 normal = glm::normalize(line);
    int numParts = (int)(lineDistance / patternWidth);
    glBegin(GL_LINES);
    float d = patternWidth * 2;
    glVertex2f(start.x, start.y);
    glVertex2f(start.x + normal.x * patternWidth, start.y + normal.y * patternWidth);
    for (int i = 1; i < numParts / 2; ++i) {
        vec2 a = start + normal * d;
        vec2 b = a + normal * patternWidth;
        glVertex2f(a.x, a.y);
        glVertex2f(b.x, b.y);
        d += patternWidth * 2;
    }
    glVertex2f(end.x, end.y);
    glVertex2f(end.x - normal.x * patternWidth, end.y - normal.y * patternWidth);
    glEnd();
}

static void DrawBody(const mat4 &vp, Body *body) {
    Transform transform = MakeTransform(body->position, body->rotation);
    mat4 rot = mat4(transform.rotation);
    mat4 model = glm::translate(mat4(), vec3(transform.translation, 0)) * rot;
    mat4 mvp = vp * model;

    glLoadMatrixf(&mvp[0][0]);
    switch (body->shape.type) {
    case ShapeType::ShapeType_Plane:
    {
        PlaneShape *plane = &body->shape.plane;
        glColor3f(1, 1, 1);
        glBegin(GL_LINES);
        glVertex2f(0, plane->visualLength * 0.5f);
        glVertex2f(0, -plane->visualLength * 0.5f);
        glEnd();
    }; break;

    case ShapeType::ShapeType_Circle:
    {
        CircleShape *circle = &body->shape.circle;
        glColor3f(1, 1, 1);
        DrawCircle(circle->radius);
    }; break;

    case ShapeType::ShapeType_Polygon:
    {
        PolygonShape *poly = &body->shape.poly;
        glColor3f(1, 1, 1);
        glBegin(GL_LINE_LOOP);
        for (int vertexIndex = 0; vertexIndex < poly->numVerts; ++vertexIndex) {
            glVertex2f(poly->localVerts[vertexIndex].x, poly->localVerts[vertexIndex].y);
        }
        glEnd();
    }; break;
    }

    // Draw axis translated with mass centroid
    mat4 axisModel = glm::translate(mvp, vec3(body->centroid, 0));
    glLoadMatrixf(&axisModel[0][0]);
    DrawRotationAxis();
}
static void DrawContact(const mat4 &vp, Contact *contact) {
    mat4 contactModel = glm::translate(vp, vec3(contact->pointOnA, 0));
    glLoadMatrixf(&contactModel[0][0]);
    glColor3f(1, 0, 0);
    DrawCircle(0.05f, 16, true);
    glColor3f(1, 1, 0);
    DrawPatternLine(vec2(0, 0), vec2(0, 0) + contact->normal * contact->distance);

    vec2 pointOnB = contact->pointOnA + contact->normal * contact->distance;
    contactModel = glm::translate(vp, vec3(pointOnB, 0));
    glLoadMatrixf(&contactModel[0][0]);
    glColor3f(1, 0, 1);
    DrawCircle(0.05f, 16, true);
}
static void GameRender(World *world, RenderState *renderState, InputState *input) {
    mat4 proj = glm::ortho(-world->boundaryExt.x, world->boundaryExt.x, -world->boundaryExt.y, world->boundaryExt.y, 0.0f, 100.0f);
    mat4 view = glm::scale(glm::translate(mat4(), vec3(renderState->cameraOffset, 0)), vec3(renderState->cameraScale));
    mat4 vp = proj * view;

    glViewport(renderState->viewportOffset.x, renderState->viewportOffset.y, renderState->viewportSize.x, renderState->viewportSize.y);
    glMatrixMode(GL_MODELVIEW);
    glLoadMatrixf(&vp[0][0]);

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // Draw world boundaries
    glColor3f(0, 0, 1);
    glBegin(GL_LINE_LOOP);
    glVertex2f(world->boundaryExt.x, world->boundaryExt.y);
    glVertex2f(-world->boundaryExt.x, world->boundaryExt.y);
    glVertex2f(-world->boundaryExt.x, -world->boundaryExt.y);
    glVertex2f(world->boundaryExt.x, -world->boundaryExt.y);
    glEnd();

    for (int bodyIndex = 0; bodyIndex < world->numBodies; ++bodyIndex) {
        Body *body = world->bodies + bodyIndex;
        DrawBody(vp, body);
    }

    // Draw interaction body
    if (world->interaction.activeBody) {
        mat4 mvp = glm::translate(vp, vec3(world->interaction.activeBody->position, 0));
        glLoadMatrixf(&mvp[0][0]);
        glColor3f(1.0f, 0.0f, 0.0f);
        DrawCircle(InteractionRadius, 16);
    } else if (world->interaction.hotBody) {
        mat4 mvp = glm::translate(vp, vec3(world->interaction.hotBody->position, 0));
        glLoadMatrixf(&mvp[0][0]);
        glColor3f(0.0f, 1.0f, 0.0f);
        DrawCircle(InteractionRadius, 16);
    }

    // Draw contacts
    for (int arbiterIndex = 0; arbiterIndex < world->numArbiters; ++arbiterIndex) {
        Arbiter *arbiter = world->arbiters + arbiterIndex;
        for (int contactIndex = 0; contactIndex < arbiter->numContacts; ++contactIndex) {
            Contact *contact = arbiter->contacts + contactIndex;
            DrawContact(vp, contact);
        }
    }

    // Draw mouse pos
    mat4 mouseMat = glm::translate(vp, vec3(input->mouse.pos, 0));
    glLoadMatrixf(&mouseMat[0][0]);
    glColor3f(0.4f, 0.6f, 0.7f);
    DrawCircle(0.1f, 16);
}

// -----------------------------------------
// Win32 Platform
// -----------------------------------------

#define WIN32_CLASSNAME "PhysicsWin32"
#define DEFAULT_SCREEN_WIDTH 1280
#define DEFAULT_SCREEN_HEIGHT 720

// WGL definitions
#define PFNWGLSWAPINTERVALPROC(name) BOOL name(int value)
typedef PFNWGLSWAPINTERVALPROC(wgl_swap_interval);

// Internal variables
static bool globalRunning;
static uint64_t globalPerfCounterFrequency;
static wgl_swap_interval *wglSwapIntervalEXT;

inline LARGE_INTEGER Win32GetWallClock() {
    LARGE_INTEGER result;
    QueryPerformanceCounter(&result);
    return(result);
}
inline float Win32GetSecondsElapsed(LARGE_INTEGER start, LARGE_INTEGER end) {
    float result = (float)(end.QuadPart - start.QuadPart) / (float)globalPerfCounterFrequency;
    return(result);
}

LRESULT CALLBACK Win32MessageProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam) {
    switch (msg) {
    case WM_DESTROY:
    case WM_CLOSE:
        globalRunning = 0;
        break;
    default:
        return DefWindowProc(hwnd, msg, wParam, lParam);
    }
    return 0;
}
static bool Win32SetPixelFormat(HDC deviceContext) {
    // Define pixel format
    PIXELFORMATDESCRIPTOR pfd = {};
    pfd.nSize = sizeof(PIXELFORMATDESCRIPTOR);
    pfd.nVersion = 1;
    pfd.dwFlags = PFD_DOUBLEBUFFER | PFD_SUPPORT_OPENGL | PFD_DRAW_TO_WINDOW;
    pfd.iPixelType = PFD_TYPE_RGBA;
    pfd.cColorBits = 32;
    pfd.cDepthBits = 24;
    pfd.cAlphaBits = 8;
    pfd.iLayerType = PFD_MAIN_PLANE;

    // Find pixel format
    int nPixelFormat = ChoosePixelFormat(deviceContext, &pfd);
    if (nPixelFormat == 0) {
        return false;
    }

    // Set pixel format
    if (!SetPixelFormat(deviceContext, nPixelFormat, &pfd)) {
        return false;
    }

    return true;
}
static void Win32LoadWGLExtensions(HINSTANCE instance) {
    // Create a dummy window and context to load extensions
    WNDCLASSA windowClass = {};
    windowClass.lpfnWndProc = DefWindowProcA;
    windowClass.hInstance = instance;
    windowClass.lpszClassName = "PhysicsWGLLoader";
    if (RegisterClassA(&windowClass)) {
        HWND window = CreateWindowExA(0, windowClass.lpszClassName, "Physics WGL Loader", 0, CW_USEDEFAULT, CW_USEDEFAULT, CW_USEDEFAULT, CW_USEDEFAULT, 0, 0, windowClass.hInstance, 0);
        HDC WindowDC = GetDC(window);
        if (Win32SetPixelFormat(WindowDC)) {
            HGLRC OpenGLRC = wglCreateContext(WindowDC);
            if (wglMakeCurrent(WindowDC, OpenGLRC)) {
                wglSwapIntervalEXT = (wgl_swap_interval *)wglGetProcAddress("wglSwapIntervalEXT");
                wglMakeCurrent(0, 0);
            }
            wglDeleteContext(OpenGLRC);
        }
        ReleaseDC(window, WindowDC);
        DestroyWindow(window);
        UnregisterClassA(windowClass.lpszClassName, instance);
    }
}
static bool Win32CreateOpenGL(HINSTANCE instance, HDC deviceContext, HGLRC *renderingContext) {
    // Load opengl extensions
    Win32LoadWGLExtensions(instance);

    // Set pixel format so we can use opengl in our window
    if (!Win32SetPixelFormat(deviceContext)) {
        return false;
    }

    // Create a opengl rendering context
    HGLRC rc = wglCreateContext(deviceContext);
    if (!rc) {
        return false;
    }

    // Activate the opengl rendering context
    if (!wglMakeCurrent(deviceContext, rc)) {
        wglDeleteContext(rc);
        return false;
    }

    // Enable vsync if possible
    if (wglSwapIntervalEXT) {
        wglSwapIntervalEXT(1);
    }

    *renderingContext = rc;

    return 1;
}
static void Win32DestroyOpenGL(HGLRC *renderingContext) {
    if (renderingContext) {
        // Disable opengl rendering context
        wglMakeCurrent(NULL, NULL);
        // Delete opengl rendering context
        wglDeleteContext(*renderingContext);
        *renderingContext = 0;
    }
}
static void Win32ProcessKeyboardMessage(ButtonState *newState, B32 isDown) {
    if (newState->endedDown != isDown) {
        newState->endedDown = isDown;
        ++newState->halfTransitionCount;
    }
}
static void Win32ProcessMessages(KeyboardState *keyboardState) {
    MSG msg;
    while (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE) != 0) {
        switch (msg.message) {
        case WM_QUIT:
        case WM_CLOSE:
        {
            globalRunning = false;
        }; break;
        case WM_SYSKEYDOWN:
        case WM_SYSKEYUP:
        case WM_KEYDOWN:
        case WM_KEYUP:
        {
            uint64_t keyCode = msg.wParam;
            B32 altKeyWasDown = (msg.lParam & (1 << 29));
            B32 wasDown = ((msg.lParam & (1 << 30)) != 0);
            B32 isDown = ((msg.lParam & (1 << 31)) == 0);
            if (wasDown != isDown) {
                switch (keyCode) {
                case VK_SPACE:
                    Win32ProcessKeyboardMessage(&keyboardState->space, isDown);
                    break;
                case VK_F1:
                    Win32ProcessKeyboardMessage(&keyboardState->f1, isDown);
                    break;
                case (int)'Q':
                    Win32ProcessKeyboardMessage(&keyboardState->q, isDown);
                    break;
                case (int)'E':
                    Win32ProcessKeyboardMessage(&keyboardState->e, isDown);
                    break;
                }

                if (isDown) {
                    if (keyCode == VK_F4 && altKeyWasDown) {
                        globalRunning = false;
                    }
                    if (keyCode == VK_RETURN && altKeyWasDown) {
                        if (msg.hwnd) {
                            // TODO: Toggle fullscreen on Return+Alt
                        }
                    }
                }
            }
        }; break;
        default:
        {
            TranslateMessage(&msg);
            DispatchMessage(&msg);
        }; break;
        }
    }
}

int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR pCmdLine, int nCmdShow) {
    // Get performance counter frequency
    LARGE_INTEGER perfCounterFreqResult;
    QueryPerformanceFrequency(&perfCounterFreqResult);
    globalPerfCounterFrequency = perfCounterFreqResult.QuadPart;

    // Register window class
    WNDCLASSEX wcex = {};
    wcex.cbSize = sizeof(WNDCLASSEX);
    wcex.style = CS_HREDRAW | CS_VREDRAW | CS_OWNDC;
    wcex.lpfnWndProc = Win32MessageProc;
    wcex.hInstance = hInstance;
    wcex.lpszClassName = WIN32_CLASSNAME;
    wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
    wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);
    wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
    if (!RegisterClassEx(&wcex)) {
        return -1;
    }

    // Create window
    HWND windowHandle = CreateWindowEx(WS_EX_OVERLAPPEDWINDOW, WIN32_CLASSNAME, "Physics Win32", WS_OVERLAPPEDWINDOW, CW_USEDEFAULT, CW_USEDEFAULT, DEFAULT_SCREEN_WIDTH, DEFAULT_SCREEN_HEIGHT, NULL, NULL, hInstance, NULL);
    if (windowHandle == NULL) {
        return -1;
    }

    // Get device context
    HDC deviceContext = GetDC(windowHandle);
    if (!deviceContext) {
        return -1;
    }

    // Create and initialize opengl
    HGLRC renderingContext = 0;
    if (!Win32CreateOpenGL(hInstance, deviceContext, &renderingContext)) {
        return -1;
    }

    // Get monitor refresh rate in hz
    int monitorRefreshHz = 60;
    int refreshRate = GetDeviceCaps(deviceContext, VREFRESH);
    if (refreshRate > 1) {
        monitorRefreshHz = refreshRate;
    }
    float gameUpdateHz = (float)monitorRefreshHz;
    float targetSecondsPerFrame = 1.0f / gameUpdateHz;

    // Initialize states
    RenderState renderState = {};
    renderState.cameraScale = 0.75f;
    renderState.cameraOffset = vec2(0, 0);

    World world = {};
    GameInit(&world);

    InputState input[2] = {};
    InputState *newInput = &input[0];
    InputState *oldInput = &input[1];

    ShowWindow(windowHandle, nCmdShow);
    UpdateWindow(windowHandle);

    // Initialize timing infos
    LARGE_INTEGER lastCounter = Win32GetWallClock();

    globalRunning = true;
    while (globalRunning) {
        // Preserve keyboard states from previous frame
        KeyboardState *oldKeyboardState = &oldInput->keyboard;
        KeyboardState *newKeyboardState = &newInput->keyboard;
        *newKeyboardState = {};
        for (int buttonIndex = 0; buttonIndex < KeyboardButton_Count; ++buttonIndex) {
            newKeyboardState->buttons[buttonIndex].endedDown = newKeyboardState->buttons[buttonIndex].endedDown;
        }

        // Process messages
        Win32ProcessMessages(newKeyboardState);

        // Update viewport for current window size
        {
            RECT windowSize;
            GetClientRect(windowHandle, &windowSize);
            renderState.gameSize = vec2(world.boundaryExt.x * 2, world.boundaryExt.y * 2);
            renderState.screenSize.x = (windowSize.right - windowSize.left) + 1;
            renderState.screenSize.y = (windowSize.bottom - windowSize.top) + 1;
            renderState.renderScale = (float)renderState.screenSize.x / renderState.gameSize.x;
            renderState.viewportOffset = ivec2(0, 0);
            renderState.viewportSize.x = renderState.screenSize.x;
            renderState.viewportSize.y = (int)(renderState.screenSize.x / GameAspect);
            if (renderState.viewportSize.y > renderState.screenSize.y) {
                renderState.viewportSize.y = renderState.screenSize.y;
                renderState.viewportSize.x = (int)(renderState.viewportSize.y * GameAspect);
                renderState.renderScale = (float)renderState.viewportSize.x / renderState.gameSize.x;
            }
            renderState.viewportOffset.x = (renderState.screenSize.x - renderState.viewportSize.x) / 2;
            renderState.viewportOffset.y = (renderState.screenSize.y - renderState.viewportSize.y) / 2;
        }

        // Mouse movement and buttons
        {
            POINT mousePos;
            GetCursorPos(&mousePos);
            ScreenToClient(windowHandle, &mousePos);
            int mouseX = mousePos.x;
            int mouseY = (renderState.screenSize.y - 1) - mousePos.y;
            newInput->mouse.pos = Unproject(&renderState, mouseX, mouseY);
            DWORD WinButtonID[MouseButton_Count] =
            {
                VK_LBUTTON,
                VK_MBUTTON,
                VK_RBUTTON,
                VK_XBUTTON1,
                VK_XBUTTON2,
            };
            for (int buttonIndex = 0; buttonIndex < MouseButton_Count; ++buttonIndex) {
                newInput->mouse.buttons[buttonIndex] = oldInput->mouse.buttons[buttonIndex];
                newInput->mouse.buttons[buttonIndex].halfTransitionCount = 0;
                Win32ProcessKeyboardMessage(&newInput->mouse.buttons[buttonIndex], GetKeyState(WinButtonID[buttonIndex]) & (1 << 15));
            }
        }

        // Update and render
        GameUpdate(targetSecondsPerFrame, &world, newInput);
        GameRender(&world, &renderState, newInput);

        // Present frame
        SwapBuffers(deviceContext);

        // Update timing infos
        LARGE_INTEGER endCounter = Win32GetWallClock();
        float secondsPerFrame = Win32GetSecondsElapsed(lastCounter, endCounter);
        lastCounter = endCounter;

        // Swap game input for next frame
        SwapPtr(InputState, newInput, oldInput);
    }

    // Destroy all opengl stuff
    Win32DestroyOpenGL(&renderingContext);

    // Destroy the window
    ReleaseDC(windowHandle, deviceContext);
    DestroyWindow(windowHandle);
    UnregisterClass(wcex.lpszClassName, wcex.hInstance);

    return 0;
}