#include <cmath>
#include <vector>

// Basic 3D vector structure
struct Vector3 {
    float x, y, z;
    Vector3() : x(0), y(0), z(0) {}
    Vector3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {}
};

// Overloaded operators for vector arithmetic
inline Vector3 operator+(const Vector3& a, const Vector3& b) {
    return Vector3(a.x + b.x, a.y + b.y, a.z + b.z);
}
inline Vector3 operator-(const Vector3& a, const Vector3& b) {
    return Vector3(a.x - b.x, a.y - b.y, a.z - b.z);
}
inline Vector3 operator*(const Vector3& v, float s) {
    return Vector3(v.x * s, v.y * s, v.z * s);
}
inline Vector3 operator*(float s, const Vector3& v) {
    return v * s;
}
inline Vector3 operator/(const Vector3& v, float s) {
    return Vector3(v.x / s, v.y / s, v.z / s);
}

inline float dot(const Vector3& a, const Vector3& b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
}
inline Vector3 cross(const Vector3& a, const Vector3& b) {
    return Vector3(a.y * b.z - a.z * b.y,
                   a.z * b.x - a.x * b.z,
                   a.x * b.y - a.y * b.x);
}
inline float length(const Vector3& v) {
    return sqrt(dot(v, v));
}
inline Vector3 normalize(const Vector3& v) {
    float len = length(v);
    return (len > 0.f) ? v / len : Vector3(0, 0, 0);
}

// Triangle structure defined by three vertices.
struct Triangle {
    Vector3 p0, p1, p2;
};

// Helper function: Computes the closest point on a triangle to a given point 'p'.
// This function uses barycentric coordinate tests to determine whether the point
// lies in one of the vertex regions, edge regions, or the interior face region.
Vector3 ClosestPointOnTriangle(const Vector3& p, const Triangle& tri) {
    // Rename triangle vertices for clarity.
    const Vector3& A = tri.p0;
    const Vector3& B = tri.p1;
    const Vector3& C = tri.p2;

    // Compute vectors for two edges sharing vertex A.
    Vector3 AB = B - A;
    Vector3 AC = C - A;
    // Vector from vertex A to point p.
    Vector3 AP = p - A;

    // Compute dot products to measure how far p projects onto the triangle edges.
    float d1 = dot(AB, AP); // p's projection on AB
    float d2 = dot(AC, AP); // p's projection on AC

    // Check if p is in the vertex region outside A.
    if (d1 <= 0.f && d2 <= 0.f)
        return A;

    // Check if p is in the vertex region outside B.
    Vector3 BP = p - B;
    float d3 = dot(AB, BP); // p's projection on AB from B's perspective
    float d4 = dot(AC, BP); // p's projection on AC (relative to A)
    if (d3 >= 0.f && d4 <= d3)
        return B;

    // Check if p is in the edge region of AB.
    float vc = d1 * d4 - d3 * d2;
    if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f)
        return A + AB * (d1 / (d1 - d3));

    // Check if p is in the vertex region outside C.
    Vector3 CP = p - C;
    float d5 = dot(AB, CP); // p's projection on AB (relative to C)
    float d6 = dot(AC, CP); // p's projection on AC from C's perspective
    if (d6 >= 0.f && d5 <= d6)
        return C;

    // Check if p is in the edge region of AC.
    float vb = d5 * d2 - d1 * d6;
    if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f)
        return A + AC * (d2 / (d2 - d6));

    // Check if p is in the edge region of BC.
    float va = d3 * d6 - d5 * d4;
    if (va <= 0.f && (d4 - d3) >= 0.f && (d5 - d6) >= 0.f)
        return B + (C - B) * ((d4 - d3) / ((d4 - d3) + (d5 - d6)));

    // p is inside the face region. Compute barycentric coordinates (v, w)
    // so that p can be expressed as: A + AB*v + AC*w.
    float denom = 1.f / (va + vb + vc);
    float v = vb * denom;
    float w = vc * denom;
    return A + AB * v + AC * w;
}

// Resolves collision between a sphere (center 'pos', radius 'radius')
// and a set of triangles. Moves 'pos' out of any collisions by computing
// the minimal displacement needed to separate the sphere from the intersecting triangles.
Vector3 ResolveCollision(Vector3 pos, float radius, std::vector<Triangle>& triangles) {
    bool collisionFound = true;
    const int maxIterations = 10;
    int iterations = 0;

    // Loop until no collisions are found or the maximum number of iterations is reached.
    while (collisionFound && iterations < maxIterations) {
        collisionFound = false;
        // Iterate over each triangle in the scene.
        for (const auto& tri : triangles) {
            // Find the closest point on the triangle to the sphere center.
            Vector3 closestPoint = ClosestPointOnTriangle(pos, tri);
            // Compute the vector from this closest point to the sphere center.
            Vector3 difference = pos - closestPoint;
            float distanceSquared = dot(difference, difference);
            // If the squared distance is less than the squared radius, a collision occurs.
            if (distanceSquared < radius * radius) {
                float distance = sqrt(distanceSquared);
                float penetrationDepth = radius - distance;
                Vector3 pushDirection;
                // If the sphere center is not exactly at the collision point, normalize the difference vector.
                if (distance > 0.0001f)
                    pushDirection = difference / distance;
                else {
                    // If the sphere center is exactly on the triangle, use the triangle's normal.
                    Vector3 triangleNormal = normalize(cross(tri.p1 - tri.p0, tri.p2 - tri.p0));
                    // Use world up (0,0,1) if the triangle normal is degenerate.
                    pushDirection = (length(triangleNormal) > 0.0001f) ? triangleNormal : Vector3(0, 0, 1);
                }
                // Move the sphere center by the penetration depth along the push direction.
                pos = pos + pushDirection * penetrationDepth;
                collisionFound = true;
            }
        }
        iterations++;
    }
    return pos;
}