#include <algo/misc.hpp>

#include <rofi/connector.hpp>
#include <rofi/voxel.hpp>

bool check_grid_alignment(node_ptr node)
{
    auto pos = node->getPositionWorld();
    float epsilon = 0.0001f;
    return glm::all(glm::epsilonEqual(pos, glm::round(pos), epsilon));
}

node_ptr find_node_with_rofi_object(const std::vector<node_ptr> &nodes, objectbase_ptr object)
{
    using namespace rofi;
    auto voxel_to_find = std::dynamic_pointer_cast<Voxel>(object);
    auto connector_to_find = std::dynamic_pointer_cast<Connector>(object);

    for (auto &node : nodes)
    {
        const auto objects = node->getObjects();
        for (const auto &obj : objects)
        {
            if (voxel_to_find)
            {
                if (const auto voxel = std::dynamic_pointer_cast<Voxel>(obj);
                    voxel && voxel.get() == voxel_to_find.get())
                    return node;
            }
            else if (connector_to_find)
            {
                if (const auto connector = std::dynamic_pointer_cast<Connector>(obj);
                    connector && connector.get() == connector_to_find.get())
                    return node;
            }
        }

        if (auto node_from_child = find_node_with_rofi_object(node->getChildren(), object);
            node_from_child)
            return node_from_child;
    }

    return nullptr;
}

glm::quat get_connector_face_rotation(glm::vec3 local_direction)
{
    ASSUMPTION(glm::length(local_direction) == 1.0f);

    return glm::rotation(rofi::enum_to_vec(rofi::side::z_neg), local_direction);
}

glm::quat get_shared_voxel_face_rotation(glm::vec3 local_direction)
{
    ASSUMPTION(glm::length(local_direction) == 1.0f);
    return glm::rotation(rofi::enum_to_vec(rofi::side::z_neg), -local_direction);
}

bool face_each_other(node_ptr fst_node, node_ptr snd_node, glm::vec3 axis)
{
    ASSUMPTION(glm::length(axis) == 1.0f);

    auto abs_axis = glm::abs(axis);
    float epsilon = 0.001f;

    if (abs_axis == glm::vec3(1.0f, 0.0f, 0.0f))
        return glm::all(glm::epsilonEqual(fst_node->getRotationAxisXWorld(), -snd_node->getRotationAxisXWorld(), epsilon));
    if (abs_axis == glm::vec3(0.0f, 1.0f, 0.0f))
        return glm::all(glm::epsilonEqual(fst_node->getRotationAxisYWorld(), -snd_node->getRotationAxisYWorld(), epsilon));
    if (abs_axis == glm::vec3(0.0f, 0.0f, 1.0f))
        return glm::all(glm::epsilonEqual(fst_node->getRotationAxisZWorld(), -snd_node->getRotationAxisZWorld(), epsilon));

    return false;
}

bool AABB_intersect_axis(const std::pair<glm::vec3, glm::vec3> &fst_LH, const std::pair<glm::vec3, glm::vec3> &snd_LH, int axis)
{
    // isOverlapping1D(box1,box2) = xmax1 >= xmin2 and xmax2 >= xmin1
    return fst_LH.second[axis] >= snd_LH.first[axis] && snd_LH.second[axis] >= fst_LH.first[axis];
}

bool AABB_intersect(const AABB &fst_aabb, const AABB &snd_aabb, const node_ptr &fst_node, const node_ptr &snd_node)
{
    auto fst_LH = fst_aabb.getAALHWorld(fst_node);
    auto snd_LH = snd_aabb.getAALHWorld(snd_node);

    return AABB_intersect_axis(fst_LH, snd_LH, 0)
           && AABB_intersect_axis(fst_LH, snd_LH, 1)
           && AABB_intersect_axis(fst_LH, snd_LH, 2);
}

std::pair<std::size_t, std::size_t> get_pad_width_height_from_connector_count(const std::vector<connector_ptr> &connectors)
{
    float max_x_pos = 0;
    float max_y_pos = 0;
    for (const auto &connector : connectors)
    {
        const auto &con_node = connector->getNode();
        const auto x_pos = con_node->getPositionLocal().x;
        const auto y_pos = con_node->getPositionLocal().y;
        if (x_pos > max_x_pos)
            max_x_pos = x_pos;
        if (y_pos > max_y_pos)
            max_y_pos = y_pos;
    }

    return { static_cast<std::size_t>(max_x_pos) + 1, static_cast<std::size_t>(max_y_pos) + 1 };
}

const glm::mat4 get_magic_matrix()
{
    return glm::mat4(0, 1, 0, 0, // col X
                     0, 0, 1, 0, // col Y
                     1, 0, 0, 0, // col Z
                     0, 0, 0, 1  // col W
                     );
}

glm::vec3 convert_to_rofi_coordinates(const glm::vec3 &position)
{
    return glm::vec3(position.z, position.x, position.y);
}

glm::mat4 convert_to_rofi_coordinates(const glm::mat4 &matrix)
{
    return get_magic_matrix() * matrix;
}

glm::vec3 convert_to_rofi_export_coordinates(const glm::vec3 &position, const glm::vec3 &to_voxel_center)
{
    const auto voxel_position = position + to_voxel_center * 0.5f;
    return glm::vec3(voxel_position.z, voxel_position.x, voxel_position.y);
}

glm::mat4 convert_to_rofi_export_coordinates(const glm::mat4 &editor_matrix)
{
    // auto m = // glm::toMat4(glm::angleAxis(glm::radians(90.0f), glm::vec3(0.0f, 0.0f, -1.0f))) 
    //          glm::toMat4(glm::angleAxis(glm::radians(90.0f), glm::vec3(0.0f, -1.0f, 0.0f)))
    //        * glm::toMat4(glm::angleAxis(glm::radians(90.0f), glm::vec3(-1.0f, 0.0f, 0.0f)))
    //        * corrected_matrix;

    const auto A_neg_X_local_correction_matrix = glm::mat4(0, 0, 1, 0, // col X
                                                           0, -1, 0, 0, // col Y
                                                           1, 0, 0, 0, // col Z
                                                           0, 0, 0, 1 // col W
                                                           );

    const auto corrected_component_matrix = editor_matrix * A_neg_X_local_correction_matrix;
        
    auto m = get_magic_matrix() * corrected_component_matrix;

    m[3] = glm::vec4(0.0f, 0.0f, 0.0f, 1.0f);
    return m;
}

glm::vec3 convert_to_editor_coordinates(const glm::vec3 &rofi_position)
{
    return glm::vec3(rofi_position.y, rofi_position.z, rofi_position.x);
}

glm::mat4 convert_to_editor_import_coordinates(const glm::mat4 &rofi_matrix)
{
    const auto A_neg_X_local_correction_matrix = glm::mat4(0, 0, 1, 0, // col X
                                                           0, -1, 0, 0, // col Y
                                                           1, 0, 0, 0, // col Z
                                                           0, 0, 0, 1 // col W
                                                           );
        
    auto m = glm::inverse(get_magic_matrix()) * rofi_matrix * glm::inverse(A_neg_X_local_correction_matrix);

    m[3] = glm::vec4(0.0f, 0.0f, 0.0f, 1.0f);
    return m;
}