#include <filein/module_loader.hpp>

#include <algo/misc.hpp>
#include <filein/json_loader.hpp>
#include <utils/math.hpp>

#include <array>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <map>
#include <ranges>

/* ------------------------------------- */
/* EXPORT */
/* ------------------------------------- */

void export_module(voxel_graph_ptr voxel_graph, const char* file_name, int id, const char* type_name)
{
    boost::json::value value;
    auto module_object = value.emplace_object();

    boost::json::array components_array;
    std::map<voxel_ptr, std::size_t> voxel_index;
    std::map<connector_ptr, std::size_t> connector_index;
    export_components(components_array, voxel_graph, id, voxel_index, connector_index);
    module_object.emplace("components", components_array);

    module_object.emplace("id", id);

    boost::json::array joints_array;
    if (std::string(type_name) == "pad")
        export_joints_pad(joints_array, voxel_graph, connector_index);
    else
        export_joints(joints_array, voxel_graph, voxel_index, connector_index);
    module_object.emplace("joints", joints_array);

    export_module_type(module_object, voxel_graph, type_name);

    save_configuration(module_object, "./data/rofi/modules", file_name);
}

void export_components(boost::json::array &components, voxel_graph_ptr &voxel_graph, int parent_id, 
                       std::map<voxel_ptr, std::size_t> &voxel_index, std::map<connector_ptr, std::size_t> &connector_index)
{
    for (const auto &connector : voxel_graph->getConnections())
    {
        if (export_component_connector(components, connector, parent_id))
            connector_index.emplace(connector, components.size() - 1);
    }
    for (const auto &voxel : voxel_graph->getVoxels())
    {
        if (std::dynamic_pointer_cast<rofi::ModuleLink>(voxel) || std::dynamic_pointer_cast<rofi::PadBoard>(voxel))
            continue;

        export_component_voxel(components, voxel, parent_id); 
        voxel_index.emplace(voxel, components.size() - 1);
    }
}

bool export_component_voxel(boost::json::array &components, const voxel_ptr &voxel, int parent_id)
{
    boost::json::object component_object;

    component_object.emplace("parent", parent_id);
    
    if (voxel->hasSharedRotation())
        component_object.emplace("type", "UM body");
    else
        component_object.emplace("type", "Cube body");

    components.emplace_back(component_object);

    return true;
}

bool export_component_connector(boost::json::array &components, const connector_ptr &connector, int parent_id)
{
    auto connector_true_type = connector->getType(true);
    auto connector_semantic_type = connector->getType(false);

    boost::json::value retval;
    ASSUMPTION(!retval.is_object());

    using namespace rofi;

    if (connector_true_type == connector_type::rotation && std::dynamic_pointer_cast<RotationConnector>(connector)->isHead())
    {
        boost::json::object component_object;
        component_object.emplace("parent", parent_id);
        component_object.emplace("type", "UM shoe");
        retval = components.emplace_back(component_object);
    }
    if (connector_true_type != connector_type::shared_rotation && connector_semantic_type != connector_type::empty)
    {
        boost::json::object component_object;
        component_object.emplace("parent", parent_id);
        component_object.emplace("type", "roficom");
        retval = components.emplace_back(component_object);
    }

    return retval.is_object();
}

void export_joints_pad(boost::json::array &joints, voxel_graph_ptr &voxel_graph, 
                   const std::map<connector_ptr, std::size_t> &connector_index)
{
    const auto &connections = voxel_graph->getConnections();
    connector_ptr fake_connector = std::make_shared<rofi::OpenConnector>(Node::create());
    auto [width, height] = get_pad_width_height_from_connector_count(connections);

    for ( int i = 0; i < width; i++ )
    {
        for ( int j = 0; j < height; j++ )
        {
            if ( i > 0 ) 
                export_joint_pad(joints, fake_connector, (i - 1) * height + j, i * height + j, glm::vec3(0.0f, 1.0f, 0.0f));

            if ( j > 0 ) 
                export_joint_pad(joints, fake_connector, i * height + j - 1, i * height + j, glm::vec3(0.0f, 0.0f, 1.0f));
        }
    }
}

void export_joint_pad(boost::json::array &joints, connector_ptr connector, std::size_t from_id, std::size_t to_id, const glm::vec3 translation)
{
    boost::json::object array_element;

    array_element.emplace("from", from_id);

    boost::json::object joint_object;

    export_joint_positions(joint_object, connector, rofi::connector_type::open);

    boost::json::array sourceToDestination_array = create_STD_matrix_pad(translation);
    joint_object.emplace("sourceToDestination", sourceToDestination_array);

    joint_object.emplace("type", "rigid");

    array_element.emplace("joint", joint_object);

    array_element.emplace("to", to_id);

    joints.emplace_back(array_element);
}

boost::json::array create_STD_matrix_pad(const glm::vec3 translation)
{
    boost::json::array sourceToDestination_array;

    auto translation_mat = glm::translate(glm::mat4(1.0f), translation);

    for (int i = 0; i < 4; ++i)
    {
        boost::json::array row_array;
        row_array.emplace_back(glm::row(translation_mat, i).x);
        row_array.emplace_back(glm::row(translation_mat, i).y);
        row_array.emplace_back(glm::row(translation_mat, i).z);
        row_array.emplace_back(glm::row(translation_mat, i).w);
        sourceToDestination_array.emplace_back(row_array);
    }
    
    return sourceToDestination_array;
}

void export_joints(boost::json::array &joints, voxel_graph_ptr &voxel_graph,
                   const std::map<voxel_ptr, std::size_t> &voxel_index, const std::map<connector_ptr, std::size_t> &connector_index)
{
    export_joints_rotation(joints, voxel_graph->getVoxels(), voxel_index, connector_index);
    export_joints_shared_rotation(joints, voxel_graph->getVoxels(), voxel_index);
    export_joints_fixed_open(joints, voxel_graph, voxel_index, connector_index);
}

void export_joints_rotation(boost::json::array &joints, const std::vector<voxel_ptr> &voxels,
                   const std::map<voxel_ptr, std::size_t> &voxel_index, const std::map<connector_ptr, std::size_t> &connector_index)
{
    for (const auto &voxel : voxels)
    {
        if (std::dynamic_pointer_cast<rofi::ModuleLink>(voxel))
            continue;

        // NOTE: This function has changed behaviour afterwards
        if (!voxel->hasRotationConnector())
            continue;
        
        boost::json::object array_element;

        array_element.emplace("from", voxel_index.find(voxel)->second);

        auto rot_con = voxel->getRotationConnector().lock();

        export_joint(array_element, rot_con, rofi::connector_type::rotation);

        auto to_index = connector_index.find(rot_con)->first->getType(false) == rofi::connector_type::empty 
                              ? connector_index.find(rot_con)->second
                              : connector_index.find(rot_con)->second - 1;

        array_element.emplace("to", to_index);

        joints.emplace_back(array_element);
    }
}

void export_joints_shared_rotation(boost::json::array &joints, const std::vector<voxel_ptr> &voxels,
                   const std::map<voxel_ptr, std::size_t> &voxel_index)
{
    std::set<voxel_ptr> seen_voxels;

    for (const auto &voxel : voxels)
    {
        if (std::dynamic_pointer_cast<rofi::ModuleLink>(voxel))
            continue;

        if (seen_voxels.contains(voxel) || !voxel->hasSharedRotation())
            continue;

        boost::json::object array_element;
       
        array_element.emplace("from", voxel_index.find(voxel)->second);

        auto shared_con = voxel->getSharedConnector().lock();

        export_joint(array_element, shared_con, rofi::connector_type::shared_rotation);

        auto connected_voxels = shared_con->getConnection();
        auto second_voxel = connected_voxels.first.lock() == voxel 
                            ? connected_voxels.second.lock() 
                            : connected_voxels.first.lock();

        array_element.emplace("to", voxel_index.find(second_voxel)->second);

        seen_voxels.emplace(voxel);
        seen_voxels.emplace(second_voxel);
        joints.emplace_back(array_element);
    }
}

void export_joints_fixed_open(boost::json::array &joints, voxel_graph_ptr &voxel_graph,
                   const std::map<voxel_ptr, std::size_t> &voxel_index, const std::map<connector_ptr, std::size_t> &connector_index)
{
    /* Joints on Rotation Connector(UM shoe) */
    for (const auto &connector : voxel_graph->getConnections())
    {
        auto rot_con = std::dynamic_pointer_cast<rofi::RotationConnector>(connector);
        if (!rot_con || !rot_con->isHead())
            continue;
        
        // From here, the 'connector' variable refers to the head connector, which represents a rotation shoe
        const auto &bound_connector_pair = rot_con->getBoundConnectors();
        const std::vector<connector_ptr> semantic_connectors = {connector, bound_connector_pair.first.lock(), bound_connector_pair.second.lock()};

        for (const auto &semantic_connector : semantic_connectors)
        {
            auto semantic_type = semantic_connector->getType(false);
            if(semantic_type == rofi::connector_type::empty)
                continue;

            boost::json::object array_element;

            auto from_index = connector_index.find(connector)->first->getType(false) == rofi::connector_type::empty 
                              ? connector_index.find(connector)->second
                              : connector_index.find(connector)->second - 1;

            array_element.emplace("from", from_index);

            export_joint(array_element, semantic_connector, semantic_connector->getType(false));

            array_element.emplace("to", connector_index.find(semantic_connector)->second);

            joints.emplace_back(array_element);
        }
    }

    /* Joints on Cube-Voxels */
    for (const auto &connector : voxel_graph->getConnections())
    {
        auto connector_true_type = connector->getType(true);
        if (connector_true_type != rofi::connector_type::fixed && connector_true_type != rofi::connector_type::open)
            continue;
        if (connector->isEmpty())
            continue;

        std::vector<voxel_ptr> connected_voxels;
        if (!connector->getConnection().first.expired())
            connected_voxels.emplace_back(connector->getConnection().first.lock());
        if (!connector->getConnection().second.expired())
            connected_voxels.emplace_back(connector->getConnection().second.lock());

        for (const auto &voxel : connected_voxels)
        {
            if (std::dynamic_pointer_cast<rofi::ModuleLink>(voxel))
                continue;

            boost::json::object array_element;

            array_element.emplace("from", voxel_index.find(voxel)->second);

            export_joint(array_element, connector, connector_true_type);

            array_element.emplace("to", connector_index.find(connector)->second);

            joints.emplace_back(array_element);
        }
    }
}

void export_joint(boost::json::object &array_element, const connector_ptr &connector, rofi::connector_type expected_type)
{
    boost::json::object joint_object;

    // ROTATION / SHARED ROTATION
        // axis

    // ROTATION         // SHARED ROTATION
        // limits           // modulo

    // ALL
        // positions

    // ROTATION / SHARED ROTATION       // THE REST
        // postMatrix, preMatrrix           // sourceToDestination

    // ALL
        //type

    export_joint_axis(joint_object, connector, expected_type);

    export_joint_limits(joint_object, connector, expected_type);

    export_joint_modulo(joint_object, connector, expected_type);

    export_joint_positions(joint_object, connector, expected_type);

    export_joint_transform_matrix(joint_object, connector, expected_type);

    export_joint_type(joint_object, connector, expected_type);

    array_element.emplace("joint", joint_object);
}

void export_joint_axis(boost::json::object &joint, const connector_ptr &connector, rofi::connector_type expected_type)
{
    using namespace rofi;

    if (expected_type != connector_type::rotation && expected_type != connector_type::shared_rotation)
        return;

    glm::vec3 rotation_axis;

    if (auto rot_con = std::dynamic_pointer_cast<RotationConnector>(connector);
        rot_con && rot_con->isHead())
        rotation_axis = rot_con->getRotationAxis();
    else if (auto shared_con = std::dynamic_pointer_cast<SharedRotationConnector>(connector))
        rotation_axis = shared_con->getRotationAxis();
    else
        ASSUMPTION(false);

    boost::json::array axis_array;
    axis_array.emplace_back(rotation_axis.x);
    axis_array.emplace_back(rotation_axis.y);
    axis_array.emplace_back(rotation_axis.z);
    axis_array.emplace_back(0.0f);

    joint.emplace("axis", axis_array);
}

void export_joint_limits(boost::json::object &joint, const connector_ptr &connector, rofi::connector_type expected_type)
{
    using namespace rofi;
    if (expected_type != connector_type::rotation)
        return;

    auto rot_con = std::dynamic_pointer_cast<RotationConnector>(connector);
    ASSUMPTION(rot_con);

    if (!rot_con->isHead())
        return;

    boost::json::object limits_object;
    
    limits_object.emplace("max", rot_con->getPosRotationLimit());
    limits_object.emplace("min", rot_con->getNegRotationLimit());

    joint.emplace("limits", limits_object);
}

void export_joint_modulo(boost::json::object &joint, const connector_ptr &connector, rofi::connector_type expected_type)
{
    using namespace rofi;
    if (expected_type != connector_type::shared_rotation)
        return;
    auto shared_con = std::dynamic_pointer_cast<SharedRotationConnector>(connector);
    ASSUMPTION(shared_con);

    joint.emplace("modulo", shared_con->getModulo());
}

void export_joint_positions(boost::json::object &joint, const connector_ptr &connector, rofi::connector_type expected_type)
{
    boost::json::array positions_array;

    using namespace rofi;
    auto rot_con = std::dynamic_pointer_cast<RotationConnector>(connector);
    switch(expected_type)
    {
        case connector_type::fixed:
            break;
        case connector_type::open:
            break;
        case connector_type::rotation:
            if (!rot_con)
                ASSUMPTION(false);
            if (rot_con->isHead())
                positions_array.emplace_back(0.0f);
            else
                return;
            break;
        case connector_type::shared_rotation:
            positions_array.emplace_back(0.0f);
            break;
        default:
            ASSUMPTION(false);
            break;
    }

    joint.emplace("positions", positions_array);
}

void export_joint_transform_matrix(boost::json::object &joint, const connector_ptr &connector, rofi::connector_type expected_type)
{
    switch(expected_type)
    {
        using namespace rofi;
        case connector_type::fixed:
        case connector_type::open:
        {
            bool is_identity = true;
            boost::json::array sourceToDestination_array = create_STD_matrix(connector, is_identity);
            if (is_identity)
                joint.emplace("sourceToDestination", "identity");
            else
                joint.emplace("sourceToDestination", sourceToDestination_array);
            break;
        }
        case connector_type::rotation:
            joint.emplace("postMatrix", "identity");
            joint.emplace("preMatrix", "identity");
            break;
        case connector_type::shared_rotation:
        {
            boost::json::array postMatrix_array = create_shared_rotation_postMatrix(connector);
            joint.emplace("postMatrix", postMatrix_array);
            joint.emplace("preMatrix", "identity");
            break;
        }
        default:
            ASSUMPTION(false);
            break;
    }
}

boost::json::array create_STD_matrix(const connector_ptr &connector, bool &is_identity)
{
    using namespace rofi;
    boost::json::array sourceToDestination_array;
    auto connected_at_side = connector->getVoxelSide().first != side::undefined ? connector->getVoxelSide().first : connector->getVoxelSide().second;

    glm::quat rotation;
    switch(connected_at_side)
    {
        using enum side;
        case x_pos: // in reality x_neg, switched due to opposite shoe y-axis in the editor
            rotation = connector->getType(true) == connector_type::rotation 
                                                   ? glm::quat(1.0f, glm::vec3(0.0f)) 
                                                   : glm::angleAxis(glm::radians(180.0f), enum_to_vec(y_pos));
            break;
        case x_neg: // in reality x_pos, switched due to opposite shoe y-axis in the editor
            rotation = connector->getType(true) == connector_type::rotation 
                       ? glm::angleAxis(glm::radians(180.0f), enum_to_vec(y_pos)) 
                       : glm::quat(1.0f, glm::vec3(0.0f));
            break;
        case z_neg:
            rotation = glm::angleAxis(glm::radians(180.0f), enum_to_vec(z_pos)) * glm::angleAxis(glm::radians(90.0f), enum_to_vec(y_neg));
            break;
        case z_pos:
            rotation = glm::angleAxis(glm::radians(90.0f), enum_to_vec(y_pos));
            break;
        case y_neg:
            rotation = glm::angleAxis(glm::radians(90.0f), enum_to_vec(y_pos)) * glm::angleAxis(glm::radians(90.0f), enum_to_vec(z_pos));
            break;
        case y_pos:
            rotation = glm::angleAxis(glm::radians(90.0f), enum_to_vec(y_pos)) * glm::angleAxis(glm::radians(90.0f), enum_to_vec(z_neg));
            break;
    }

    auto rotation_mat = glm::toMat4(rotation);
    float epsilon = 0.00001f;

    for (int i = 0; i < 4; ++i)
    {
        auto id_column = glm::vec4(0.0f);
        id_column[i] = 1.0f;
        if (!glm::all(glm::epsilonEqual(rotation_mat[i], id_column, epsilon)))
            is_identity = false;

    }

    for (int i = 0; i < 4; ++i)
    {
        boost::json::array row_array;
        row_array.emplace_back(glm::row(rotation_mat, i).x);
        row_array.emplace_back(glm::row(rotation_mat, i).y);
        row_array.emplace_back(glm::row(rotation_mat, i).z);
        row_array.emplace_back(glm::row(rotation_mat, i).w);
        sourceToDestination_array.emplace_back(row_array);
    }
    
    return sourceToDestination_array;
}

boost::json::array create_shared_rotation_postMatrix(const connector_ptr &connector)
{
    using namespace rofi;
    boost::json::array postMatrix_array;
    auto rotation_mat = glm::toMat4(glm::rotation(enum_to_vec(side::z_neg), enum_to_vec(side::z_pos)));
    auto translation_mat = glm::translate(glm::mat4(1.0f), enum_to_vec(side::z_pos));
    auto postMatrix = translation_mat * rotation_mat;

    for (int i = 0; i < 4; ++i)
    {
        boost::json::array row_array;
        row_array.emplace_back(glm::row(postMatrix, i).x);
        row_array.emplace_back(glm::row(postMatrix, i).y);
        row_array.emplace_back(glm::row(postMatrix, i).z);
        row_array.emplace_back(glm::row(postMatrix, i).w);
        postMatrix_array.emplace_back(row_array);
    }
    
    return postMatrix_array;
}

void export_joint_type(boost::json::object &joint, const connector_ptr &connector, rofi::connector_type expected_type)
{
    std::string type;

    using namespace rofi;
    auto rot_con = std::dynamic_pointer_cast<RotationConnector>(connector);
    switch(expected_type)
    {
        case connector_type::fixed:
            type = "rigid";
            break;
        case connector_type::open:
            type = "rigid";
            break;
        case connector_type::rotation:
            if (!rot_con)
                ASSUMPTION(false);
            if (rot_con->isHead())
                type = "rotational";
            else
                return;
            break;
        case connector_type::shared_rotation:
            type = "modRotational";
            break;
        default:
            ASSUMPTION(false);
            break;
    }

    joint.emplace("type", type);
}

bool check_universal_module_voxel_graph(voxel_graph_ptr &voxel_graph)
{
    if (voxel_graph->getConnections().size() != 7)
        return false;

    std::size_t open_con_count = 0;
    for (const auto &connector : voxel_graph->getConnections())
        if (connector->getType(false) == rofi::connector_type::open)
            ++open_con_count;

    return open_con_count == 6;
}

void export_module_type(boost::json::object &module_object, voxel_graph_ptr &voxel_graph, const char* type_name)
{
    std::string type;
    std::string type_name_s = std::string(type_name);

    if (type_name_s == "cube")
    {
        if (voxel_graph->getConnections().size() != 6)
            type = "unknown";
        else
            type = "cube";
    }
    else if (type_name_s == "universal")
    {
        if (!check_universal_module_voxel_graph(voxel_graph))
            type = "unknown";
        else
            type = "universal";
    }
    else if (type_name_s == "pad")
        type = "pad";
    else
        type = "unknown";

    module_object.emplace("type", type);
}

/* ------------------------------------- */
/* IMPORT */
/* ------------------------------------- */

std::vector<module_ptr> import_saved_configurations_module()
{
    const std::filesystem::path modules_directory = "./data/rofi/modules";
    std::vector<module_ptr> imported_configurations;

    for (auto const& dir_entry : std::filesystem::directory_iterator{modules_directory})
    {
        if (dir_entry.path().filename().string() == "." || dir_entry.path().filename().string() == "..")
            continue;

        const auto module = import_module(dir_entry);
        if (module)
            imported_configurations.emplace_back(module);
    }

    return imported_configurations;
}

module_ptr import_module(const std::filesystem::path file_path)
{
    const auto json_object = read_file(file_path).as_object();    

    boost::json::array components_array;
    boost::json::array joints_array;
    boost::json::value id_value;
    boost::json::string type_string;
    std::map<objectbase_ptr, uint64_t> component_ids = std::map<objectbase_ptr, uint64_t>{};
    get_module_data(json_object, components_array, id_value, joints_array, type_string);

    if (type_string == "unknown")
        return nullptr;

    if (type_string == "cube")
        return std::make_shared<rofi::Module>(import_passive_cube(components_array, joints_array, component_ids), 
                                              std::move(file_path.stem().string()), std::move(std::string(type_string)), std::move(component_ids));
    if (type_string == "universal")
        return std::make_shared<rofi::Module>(import_universal(components_array, joints_array, component_ids), 
                                              std::move(file_path.stem().string()), std::move(std::string(type_string)), std::move(component_ids));
    if (type_string == "pad")
        return std::make_shared<rofi::Module>(import_pad(components_array, joints_array, component_ids), 
                                              std::move(file_path.stem().string()), std::move(std::string(type_string)), std::move(component_ids));
    // if (type_string == "unknown")
    //     return std::make_shared<rofi::Module>(import_unknown(components_array, joints_array, component_ids), 
                                                //  std::move(file_path.string()), std::move(std::string(type_string)), std::move(component_ids));

    ASSUMPTION(false);
    return std::make_shared<rofi::Module>(std::make_shared<rofi::VoxelGraph>(), "Not", "Reachable", std::move(component_ids));
}

void get_module_data(const boost::json::object &json_object, boost::json::array &components_array, boost::json::value &id_value,
                       boost::json::array &joints_array, boost::json::string &type_string)
{
    const boost::json::object module_object = json_object.contains("modules") 
                                              ? json_object.at("modules").as_array().front().as_object()
                                              : json_object;

    components_array = module_object.at("components").as_array();
    id_value = module_object.at("id");
    joints_array = module_object.at("joints").as_array();
    type_string = module_object.at("type").as_string();
}

voxel_graph_ptr import_passive_cube(const boost::json::array &components_array, const boost::json::array &joints_array, std::map<objectbase_ptr, uint64_t> &component_ids)
{
    auto vg = std::make_shared<rofi::VoxelGraph>();

    auto body_ids = get_component_ids(components_array, "Cube body");
    auto roficom_ids = get_component_ids(components_array, "roficom");

    // import the cube
    auto body = vg->addVoxel(Node::create());
    component_ids.emplace(body, *body_ids.begin());

    auto from_to_std = get_rigid_joints(joints_array, body_ids, roficom_ids);
    auto id_roficom_pairs = import_rigid_joints(vg, from_to_std, {{*body_ids.begin(), body}});
    for (auto &id_roficom : id_roficom_pairs)
        component_ids.emplace(id_roficom.second, id_roficom.first);

    return vg;
    
}

voxel_graph_ptr import_universal(const boost::json::array &components_array, const boost::json::array &joints_array, std::map<objectbase_ptr, uint64_t> &component_ids)
{
    auto vg = std::make_shared<rofi::VoxelGraph>();

    auto body_ids = get_component_ids(components_array, "UM body");
    auto shoe_ids = get_component_ids(components_array, "UM shoe");
    auto roficom_ids = get_component_ids(components_array, "roficom");

    auto id_body_pairs = import_um_bodies(vg, body_ids);
    for (auto &id_body : id_body_pairs)
        component_ids.emplace(id_body.second, id_body.first);

    auto from_to_min_max = get_rotation_joints(joints_array, body_ids);
    auto id_shoe_pairs = import_um_shoes(vg, from_to_min_max, id_body_pairs);
    for (auto &id_shoe : id_shoe_pairs)
        component_ids.emplace(id_shoe.second, id_shoe.first);

    auto from_to_std = get_rigid_joints(joints_array, shoe_ids, roficom_ids);
    auto id_roficom_pairs = import_rigid_joints(vg, from_to_std, id_shoe_pairs);
    for (auto &id_roficom : id_roficom_pairs)
        component_ids.emplace(id_roficom.second, id_roficom.first);

    return vg;
}

voxel_graph_ptr import_pad(const boost::json::array &components_array, const boost::json::array &joints_array, std::map<objectbase_ptr, uint64_t> &component_ids)
{
    using namespace rofi;
    auto vg = std::make_shared<VoxelGraph>();

    auto roficom_ids = get_component_ids(components_array, "roficom");

    std::set<uint64_t> pad_board_ids;
    for (std::size_t i = 0; i < roficom_ids.size(); ++i)
        pad_board_ids.emplace_hint(pad_board_ids.end(), i);

    
    auto from_to_std = get_rigid_joints(joints_array, pad_board_ids, roficom_ids);

    const auto [width, height] = get_pad_width_height_from_joints(roficom_ids.size(), from_to_std);

    for ( int x = 0; x < width; ++x ) 
    {
        for ( int y = 0; y < height; ++y ) 
        {
            float x_pos = static_cast<float>(x);
            float y_pos = static_cast<float>(y);

            auto open_con_node = Node::create(glm::vec3(x_pos, y_pos, -0.5f), get_connector_face_rotation(glm::vec3(0.0f, 0.0f, -1.0f)));
            open_con_node->rotateRotationQuat(glm::rotation(open_con_node->getRotationAxisXLocal(), glm::vec3(0.0f, 1.0f, 0.0f)));
            auto open_connector = vg->addConnector(open_con_node, connector_type::open);

            auto pad_board_node = Node::create(glm::vec3(x_pos, y_pos, 0.0f), glm::angleAxis(glm::radians(90.0f), glm::vec3(-1.0f, 0.0f, 0.0f)));
            auto pad_board = vg->addPadBoard(pad_board_node);

            vg->join(open_connector, pad_board, enum_to_vec(side::y_pos));
        }
    }

    return vg;
}

std::set<uint64_t> get_component_ids(const boost::json::array &components_array, const std::string &&component_type)
{
    std::set<uint64_t> ids;
    for (const auto &component_value : components_array)
    for (std::size_t id = 0; id < components_array.size(); ++id)
    {
        if (components_array.at(id).as_object().at("type").as_string() == component_type)
            ids.emplace_hint(ids.end(), id);
    }

    return ids;
}

std::vector<std::tuple<uint64_t, uint64_t, glm::mat4>> get_rigid_joints(const boost::json::array &joints_array, const std::set<uint64_t> &body_ids, const std::set<uint64_t> &roficom_ids)
{
    std::vector<std::tuple<uint64_t, uint64_t, glm::mat4>> from_to_std;
    for (const auto & joint : joints_array)
    {
        auto joints_element_object = joint.as_object();
        auto joint_object = joints_element_object.at("joint").as_object();

        if (joint_object.at("type").as_string() != "rigid")
            continue;

        auto from_id = joints_element_object.at("from").as_int64();
        if (body_ids.contains(from_id))
        {
            auto to_id = joints_element_object.at("to").as_int64();
            auto mat = get_transformation_matrix(joint_object.at("sourceToDestination"));
            from_to_std.emplace_back(from_id, to_id, mat);
        }
    }

    return from_to_std;
}

std::map<uint64_t, objectbase_ptr> import_rigid_joints(voxel_graph_ptr vg, 
                                                       const std::vector<std::tuple<uint64_t, uint64_t, glm::mat4>> &from_to_std, 
                                                       const std::map<uint64_t, objectbase_ptr> &from_components)
{
    using namespace rofi;

    std::map<uint64_t, objectbase_ptr> id_roficom_pairs;

    for (const auto [from, to, std] : from_to_std)
    {
        if (!from_components.contains(from))
            continue;

        const auto at_voxel_side = std_to_side(std);

        if (auto voxel = std::dynamic_pointer_cast<Voxel>(from_components.at(from))) // Voxel ID
        {
            const auto voxel_side_direction = enum_to_vec(at_voxel_side);
            auto node = Node::create(voxel_side_direction * 0.5f, get_connector_face_rotation(voxel_side_direction));

            // adjust cube North
            if (at_voxel_side == side::x_neg || at_voxel_side == side::x_pos || at_voxel_side == side::z_pos)
                node->rotateRotationQuat(glm::angleAxis(glm::radians(180.0f), node->getRotationAxisZLocal()));

            auto connector = vg->addConnector(node, connector_type::open);
            id_roficom_pairs.emplace(to, connector);
            vg->join(connector, std::dynamic_pointer_cast<Voxel>(from_components.at(from)), voxel_side_direction);
        }
        else if (auto rot_con = std::dynamic_pointer_cast<RotationConnector>(from_components.at(from))) // Shoe ID
        {
            connector_ptr con;
            if (at_voxel_side == side::z_neg)
                con = rot_con;
            else if (at_voxel_side == side::x_pos)
                con = rot_con->getBoundConnectors().first.lock();
            else if (at_voxel_side == side::x_neg)
                con = rot_con->getBoundConnectors().second.lock();
            else
                ASSUMPTION(false);
            
            std::dynamic_pointer_cast<RotationConnector>(con)->setSemanticType(connector_type::open);
            id_roficom_pairs.emplace(to, con);
        }
        else
            ASSUMPTION(false);
    }

    return id_roficom_pairs;
}

rofi::side std_to_side(const glm::mat4 &std_matrix)
{
    using enum rofi::side;

    const std::array<glm::mat4, 6> matrices =
    {
        glm::mat4(1.0f), // x_neg
        glm::toMat4(glm::angleAxis(glm::radians(180.0f), enum_to_vec(y_pos))), // x_pos
        glm::toMat4(glm::angleAxis(glm::radians(180.0f), enum_to_vec(z_pos)) * glm::angleAxis(glm::radians(90.0f), enum_to_vec(y_neg))), // z_neg
        glm::toMat4(glm::angleAxis(glm::radians(90.0f), enum_to_vec(y_pos))), // z_pos
        glm::toMat4(glm::angleAxis(glm::radians(90.0f), enum_to_vec(y_pos)) * glm::angleAxis(glm::radians(90.0f), enum_to_vec(z_pos))), // y_neg
        glm::toMat4(glm::angleAxis(glm::radians(90.0f), enum_to_vec(y_pos)) * glm::angleAxis(glm::radians(90.0f), enum_to_vec(z_neg))) // y_pos
    }; 

    std::array<rofi::side, 6> sides = { x_neg, x_pos, z_neg, z_pos, y_neg, y_pos };
    float epsilon = 0.00001f;

    for (int i = 0; i < 6; ++i)
    {
        const auto &side_mat = matrices[i];
        bool equal = true;

        for (int col = 0; col < 4; ++col)
        {
            if (!glm::all(glm::epsilonEqual(std_matrix[col], side_mat[col], epsilon)))
                equal = false;
        }
        if (equal)
            return sides[i];
    }

    ASSUMPTION(false);
    return undefined;
}

const std::map<uint64_t, objectbase_ptr> import_um_bodies(voxel_graph_ptr vg, const std::set<uint64_t> &body_ids)
{
    using namespace rofi;

    auto fst_voxel_node = Node::create(glm::vec3(0.0f, 0.0f, 0.0f), get_shared_voxel_face_rotation(enum_to_vec(side::x_pos)));
    auto snd_voxel_node = Node::create(glm::vec3(1.0f, 0.0f, 0.0f), get_shared_voxel_face_rotation(enum_to_vec(side::x_neg)));
    auto shared_con_node = Node::create(glm::vec3(0.5f, 0.0f, 0.0f), get_connector_face_rotation(enum_to_vec(side::x_pos)));

    auto fst_voxel = vg->addVoxel(fst_voxel_node);
    auto snd_voxel = vg->addVoxel(snd_voxel_node);

    // Possible to read axis and modulo values from file also, as of now, they are always the same though
    auto shared_con = vg->addConnector(shared_con_node, connector_type::shared_rotation, enum_to_vec(side::z_pos));

    vg->join(shared_con, fst_voxel, enum_to_vec(side::z_pos));
    vg->join(shared_con, snd_voxel, enum_to_vec(side::z_pos));

    ASSUMPTION(body_ids.size() == 2);

    return {{*body_ids.begin(), fst_voxel}, {*body_ids.begin() + 1, snd_voxel}};
}

const std::map<uint64_t, objectbase_ptr> import_um_shoes(voxel_graph_ptr vg,
                                                         const std::vector<std::tuple<uint64_t, uint64_t, float, float>> &from_to_min_max, 
                                                         const std::map<uint64_t, objectbase_ptr> &id_body_pairs)
{
    using namespace rofi;
    std::map<uint64_t, objectbase_ptr> id_shoe_pairs;
    for (const auto &[from, to, min, max] : from_to_min_max)
    {
        if (!id_body_pairs.contains(from))
            continue;

        auto body = std::dynamic_pointer_cast<Voxel>(id_body_pairs.at(from));
        node_ptr shoe_node;
        if (body->getNode()->getPositionWorld() == glm::vec3(0.0f))
            shoe_node = Node::create(glm::vec3(-0.5f, 0.0f, 0.0f), get_connector_face_rotation(enum_to_vec(side::x_neg)));
        else if (body->getNode()->getPositionWorld() == glm::vec3(1.0f, 0.0f, 0.0f))
            shoe_node = Node::create(glm::vec3(1.5f, 0.0f, 0.0f), get_connector_face_rotation(enum_to_vec(side::x_pos)));

        auto connector = vg->addConnector(shoe_node, connector_type::rotation, enum_to_vec(side::x_pos), enum_to_vec(side::z_neg));
        auto rot_con = std::dynamic_pointer_cast<RotationConnector>(connector);
        rot_con->setNegRotationLimit(min);
        rot_con->setPosRotationLimit(max);

        vg->join(connector, body, enum_to_vec(side::z_neg));
        id_shoe_pairs.emplace(to, connector);
    }
    return id_shoe_pairs;
}

std::vector<std::tuple<uint64_t, uint64_t, float, float>> get_rotation_joints(const boost::json::array &joints_array, const std::set<uint64_t> &body_ids)
{
    std::vector<std::tuple<uint64_t, uint64_t, float, float>> from_to_min_max;
    for (const auto & joint : joints_array)
    {
        auto joints_element_object = joint.as_object();
        auto joint_object = joints_element_object.at("joint").as_object();

        if (joint_object.at("type").as_string() != "rotational")
            continue;

        auto from_id = joints_element_object.at("from").as_int64();
        if (body_ids.contains(from_id))
        {
            auto to_id = joints_element_object.at("to").as_int64();
            auto min = get_limit(joint_object.at("limits").as_object().at("min"));
            auto max = get_limit(joint_object.at("limits").as_object().at("max"));
            from_to_min_max.emplace_back(from_id, to_id, min, max);
        }
    }

    return from_to_min_max;
}

float get_limit(const boost::json::value &limit_value)
{
    if (limit_value.is_double())
        return static_cast<float>(limit_value.as_double());
    else if (limit_value.is_int64())
        return static_cast<float>(limit_value.as_int64());
    else
        ASSUMPTION(false);

    return 0.0f;
}

std::pair<uint64_t, uint64_t> get_pad_width_height_from_joints(const std::size_t roficom_count, const std::vector<std::tuple<uint64_t, uint64_t, glm::mat4>> &from_to_std)
{
    if (roficom_count == 1)
        return {1, 1};

    if (roficom_count > from_to_std.size())
    {
       return std::get<2>(from_to_std.back())[3].z == 1.0f 
              ? std::pair<uint64_t, uint64_t>{roficom_count, 1} 
              : std::pair<uint64_t, uint64_t>{ 1, roficom_count };
    }

    uint64_t max_to_from_0 = 0;

    for (const auto &[from, to, std] : from_to_std)
    {
        if (from > 0)
            continue;

        if (to > max_to_from_0)
            max_to_from_0 = to;
    }
    uint64_t height = max_to_from_0;
    const auto &[from, to, std] = from_to_std.back();
    uint64_t width = (to + 1) / height;

    return {width, height};
}