#include <Redacted3D/engine/collision.h>

VertexArray genIcosahedron(const Sphere* sphere) {
    const float phi = (1.0 + std::sqrt(5.0)) / 2.0;
    VertexArray result;
    std::vector<Vertex> vertices = {
            {-1,  phi,  0}, { 1,  phi,  0}, {-1, -phi,  0}, { 1, -phi,  0},
            { 0, -1,  phi}, { 0,  1,  phi}, { 0, -1, -phi}, { 0,  1, -phi},
            { phi,  0, -1}, { phi,  0,  1}, {-phi,  0, -1}, {-phi,  0,  1}
    };

    std::vector<unsigned int> indices = {
            0, 11, 5, 0, 5, 1, 0, 1, 7, 0, 7, 10, 0, 10, 11,
            1, 5, 9, 5, 11, 4, 11, 10, 2, 10, 7, 6, 7, 1, 8,
            3, 9, 4, 3, 4, 2, 3, 2, 6, 3, 6, 8, 3, 8, 9,
            4, 9, 5, 2, 4, 11, 6, 2, 10, 8, 6, 7, 9, 8, 1
    };
    result.vertices = vertices;
    result.indices = indices;
    return result;
}
AABB Collision::genMinimallyEnclosingAABB(const VertexArray* vA) {
    float minX = std::numeric_limits<float>::max();
    float minY = std::numeric_limits<float>::max();
    float minZ = std::numeric_limits<float>::max();
    float maxX = -std::numeric_limits<float>::max();
    float maxY = -std::numeric_limits<float>::max();
    float maxZ = -std::numeric_limits<float>::max();
    AABB result;
    for (const auto& vertex : vA->vertices) {
        if (vertex.x < minX)
            minX = vertex.x;
        if (vertex.y < minY)
            minY = vertex.y;
        if (vertex.z < minZ)
            minZ = vertex.z;

        if (vertex.x > maxX)
            maxX = vertex.x;
        if (vertex.y > maxY)
            maxY = vertex.y;
        if (vertex.z > maxZ)
            maxZ = vertex.z;
    }
    result.minPoint = {minX, minY, minZ};
    result.maxPoint = {maxX, maxY, maxZ};
    return result;
}

AABB Collision::genMinimallyEnclosingAABB(const VertexArray* vA, const Vector3& rotation) {
    return vA->getCachedOBB(rotation).MinimalEnclosingAABB();
}

Sphere Collision::genMinimallyEnclosingSphere(const VertexArray* vA) {
    //std::cout << vA->getCachedOBB(Vector3(0, 0, 0)).MinimalEnclosingSphere().Radius << std::endl;
    return Collision::genMinimallyEnclosingAABB(vA).MinimalEnclosingSphere();
    //return Collision::genMinimallyEnclosingAABB(vA)
}

OBB Collision::genMinimallyEnclosingOBB(const VertexArray* vA, const Vector3& rotation) {
    Vector3 centroid(0, 0, 0);
    Vector3 radii(0, 0, 0);
    Matrix3x3 rotationMatrix = Matrix3x3::FromQuat(Quaternion((EulerAngle) rotation));
    Vector3 axis0 = Vector3(rotationMatrix[0][0], rotationMatrix[1][0], rotationMatrix[2][0]);
    Vector3 axis1 = Vector3(rotationMatrix[0][1], rotationMatrix[1][1], rotationMatrix[2][1]);
    Vector3 axis2 = Vector3(rotationMatrix[0][2], rotationMatrix[1][2], rotationMatrix[2][2]);

    for (const auto& vertex : vA->vertices)
        centroid += vertex;
    centroid /= vA->vertices.size();

    for (const auto& vertex : vA->vertices) {
        Vector3 diff = vertex - centroid;
        radii.x = std::max(radii.x, std::abs(diff.x));
        radii.y = std::max(radii.y, std::abs(diff.y));
        radii.z = std::max(radii.z, std::abs(diff.z));
    }
    return {centroid, radii, axis0, axis1, axis2};
}

//TODO the OBB is fucked in some way.
VertexArray Collision::getDrawable(const Shape& collider) {

    if (const auto* s = dynamic_cast<const AABB*>(&collider)) {
        VertexArray result;
        GLuint indices[36] = {
                0, 1, 5,0, 5, 4,2, 3, 7,2, 7, 6,0, 1, 3,0, 3,
                2,4, 5, 7,4, 7,6,0,2, 6,0, 6, 4,1, 3, 7,
                1, 7, 5
        };

        result.vertices.push_back(s->minPoint);
        result.vertices.emplace_back(s->maxPoint.x, s->minPoint.y, s->minPoint.z);
        result.vertices.emplace_back(s->minPoint.x, s->maxPoint.y, s->minPoint.z);
        result.vertices.emplace_back(s->maxPoint.x, s->maxPoint.y, s->minPoint.z);
        result.vertices.emplace_back(s->minPoint.x, s->minPoint.y, s->maxPoint.z);
        result.vertices.emplace_back(s->maxPoint.x, s->minPoint.y, s->maxPoint.z);
        result.vertices.emplace_back(s->minPoint.x, s->maxPoint.y, s->maxPoint.z);
        result.vertices.push_back(s->maxPoint);

        for (unsigned int ind : indices)
            result.indices.push_back(ind);
        return result;
    }

    if (const auto* s = dynamic_cast<const OBB*>(&collider)) {
        VertexArray result;
        return result;
    }

    if (const auto* s = dynamic_cast<const Sphere*>(&collider))
    {
        VertexArray result = genIcosahedron(s);

        if (s->Radius != 1.0f) //Scale
        {
            for (auto& vertex : result.vertices)
            {
                // TODO: change to Normalized() with J3ML 2.2
                vertex = vertex.Normalized() * s->Radius;
            }
        }

        for (auto& v : result.vertices) //Translate
            v += s->Position;
        return result;
    }
    return {};
}