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

struct Edge {
    Vector2 normal;
    Vector2 start;
    Vector2 end;
};

struct SATResult {
    Edge* minOverlapEdge = NULL;        // Edge that caused the intersection (on the 'first' shape)
    Vector2 overlapPoint;               // Point that caused the intersection (on the 'second' shape)
    float32 minOverlap = FLT_MAX;       // Smallest projection overlap
};

struct ProjectionResult {
    Vector2 minVertex;
    Vector2 maxVertex;
    Vector2 projection;
};

// Given the vertices in the shape, we find the min and max vertices that project
// onto the axis. We also save which vertices were which so that we can resolve
// the collision later.
ProjectionResult getProjection(Vector2* vertices, int numVertices, Vector2 axis) {
    ProjectionResult pr;
    float32 min = axis.dot(vertices[0]);
    float32 max = min;

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

        if (d < min) {
            pr.minVertex = vertices[v];
            min = d;
        } else if (d > max) {
            pr.maxVertex = vertices[v];
            max = d;
        }
    }
    
    pr.projection = Vector2 { min, max };
    return pr;
}

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

float32 getProjectionOverlap(Vector2 first, Vector2 second) {
    float32 e = MIN(first.y, second.y);
    float32 f = MAX(first.x, second.x);
    return e - f;
}

bool runSatForShapesEdges(SATResult* result, ConvexPolygon* first, ConvexPolygon* second) {
    for (int i = 0; i < first->numVertices; i++) {
        Vector2 normal = first->edges[i].normal;

        ProjectionResult firstProj = getProjection(first->transformedVertices, first->numVertices, normal);
        ProjectionResult secondProj = getProjection(second->transformedVertices, second->numVertices, normal);

        if (!projectionsOverlap(firstProj.projection, secondProj.projection)) {
            return false;
        }

        float32 overlap = getProjectionOverlap(firstProj.projection, secondProj.projection);
        if (overlap < result->minOverlap) {
            result->minOverlap = overlap;
            result->minOverlapEdge = &first->edges[i];
            
            // The overlapPoint will be the point on the other shape that penetrated the edge.
            // If we caught the intersection reasonably early, it should be the point on 'second'
            // that is nearest to the points on 'first'.
            float32 min1min2 = (firstProj.minVertex - secondProj.minVertex).length();
            float32 min1max2 = (firstProj.minVertex - secondProj.maxVertex).length();
            float32 max1max2 = (firstProj.maxVertex - secondProj.maxVertex).length();
            float32 max1min2 = (firstProj.maxVertex - secondProj.minVertex).length();
            
            float32 closest = MIN(min1min2, MIN(min1max2, MIN(max1max2, max1min2)));
            if (closest == min1min2 || closest == max1min2) {
                result->overlapPoint = secondProj.minVertex;
            } else {
                result->overlapPoint = secondProj.maxVertex;
            }
        }
    }

    return true;
}

const float32 EPSILON = 1.f;
IntersectionResult getIntersection(ConvexPolygon* first, ConvexPolygon* second) {
    IntersectionResult ir;
    SATResult sat;
    
    if (!runSatForShapesEdges(&sat, first, second)) {
        return ir;
    }

    if (!runSatForShapesEdges(&sat, second, first)) {
        return ir;
    }

    ir.intersect = true;
    ir.relativeVelocity = first->body.velocity - second->body.velocity;
    ir.collisionNormal = sat.minOverlapEdge->normal;
    ir.firstPointOfApplication = sat.overlapPoint - first->body.position;
    ir.secondPointOfApplication = sat.overlapPoint - second->body.position;;

    return ir;
}