#include <common/aabb.hpp>

AABB::AABB(std::vector<float> const &vertices)
{
    findLH(vertices);
    createFullAABB();
}

AABB::AABB(glm::vec3 low, glm::vec3 high)
{
    LH.first = low;
    LH.second = high;
    createFullAABB();
}

void AABB::findLH(std::vector<float> const &vertices)
{
    LH.first = glm::vec3(vertices[0], vertices[1], vertices[2]);
    LH.second = glm::vec3(vertices[0], vertices[1], vertices[2]);

    for (std::size_t i = 3; i < vertices.size() - 2; i += 3)
    {
        LH.first = glm::min(LH.first, 
                            glm::vec3(vertices[i], 
                                      vertices[i + 1], 
                                      vertices[i + 2]));
        LH.second = glm::max(LH.second, 
                            glm::vec3(vertices[i], 
                                      vertices[i + 1], 
                                      vertices[i + 2]));
    }
}

void AABB::createFullAABB()
{
    full_bbox[0] = LH.first;

    full_bbox[1] = glm::vec3(LH.second.x, LH.first.y, LH.first.z);
    full_bbox[2] = glm::vec3(LH.first.x, LH.second.y, LH.first.z);
    full_bbox[3] = glm::vec3(LH.first.x, LH.first.y, LH.second.z);

    full_bbox[4] = glm::vec3(LH.second.x, LH.second.y, LH.first.z);
    full_bbox[5] = glm::vec3(LH.second.x, LH.first.y, LH.second.z);
    full_bbox[6] = glm::vec3(LH.first.x, LH.second.y, LH.second.z);

    full_bbox[7] = LH.second;
}

std::vector<float> AABB::getVertices() const
{
    std::vector<float> vertices;
    vertices.reserve(24);
    for (auto &coord : full_bbox)
    {
        vertices.emplace_back(coord.x);
        vertices.emplace_back(coord.y);
        vertices.emplace_back(coord.z);
    }

    return vertices;
}

std::vector<unsigned int> AABB::getIndices() const
{
    return { 0, 1, 0, 2, 0, 3,
             7, 4, 7, 5, 7, 6,
             1, 4, 1, 5,
             2, 4, 2, 6,
             3, 5, 3, 6
            };
}

std::pair<glm::vec3, glm::vec3> AABB::getLHWorld(node_ptr node) const
{
    std::pair<glm::vec3, glm::vec3> LH_world;
    auto node_model_world = node->getModelMatWorld();

    LH_world.first = node_model_world * glm::vec4(LH.first, 1);
    LH_world.second = node_model_world * glm::vec4(LH.second, 1);

    return LH_world;
}

std::pair<glm::vec3, glm::vec3> AABB::getAALHWorld(node_ptr node) const
{
    std::pair<glm::vec3, glm::vec3> AALH_world;
    auto node_translation = node->getTranslationMatWorld();
    auto node_scale = node->getScaleMatWorld();

    AALH_world.first = node_translation * node_scale * glm::vec4(LH.first, 1);
    AALH_world.second = node_translation * node_scale * glm::vec4(LH.second, 1);

    return AALH_world;
}

std::array<glm::vec3, 8> AABB::getAABBWorld(node_ptr node) const
{
    std::array<glm::vec3, 8> AABB_world;

    auto node_trans = node->getTranslationMatWorld();
    auto node_scale = node->getScaleMatWorld();

    for (int i = 0; i < 8; ++i)
    {
        AABB_world[i] = node_trans * node_scale * glm::vec4(full_bbox[i], 1);
    }


    return AABB_world;
}