#include <filein/rofiworld_loader.hpp>

#include <algo/misc.hpp>
#include <filein/json_loader.hpp>
#include <filein/module_loader.hpp>

#include <queue>

void export_rofiworld(const rofiworld_ptr rofiworld, const std::vector<objectbase_ptr> &selected_spaceJoints, const char* file_name)
{
    boost::json::value value;
    auto rofiworld_object = value.emplace_object();
    
    boost::json::array modules_array;
    export_modules(modules_array, rofiworld->getModulesMap());
    rofiworld_object.emplace("modules", modules_array);


    boost::json::array spaceJoints_array;
    export_spaceJoints(spaceJoints_array, rofiworld, selected_spaceJoints);
    rofiworld_object.emplace("spaceJoints", spaceJoints_array);

    boost::json::array moduleJoints_array;
    export_moduleJoints(moduleJoints_array, rofiworld);
    rofiworld_object.emplace("moduleJoints", moduleJoints_array);

    save_configuration(rofiworld_object, "./data/rofi/rofiworlds", file_name);
}

void export_modules(boost::json::array &modules_array, const std::map<module_ptr, uint64_t> &modules_map)
{
    for (const auto &[module, id] : modules_map)
    {
        const auto &module_type = module->getType();

        if (module_type == "cube")
            export_module_cube(modules_array, module, id);
        if (module_type == "universal")
            export_module_universal(modules_array, module, id);
        if (module_type == "pad")
            export_module_pad(modules_array, module, id);
        // if (module_type == "unknown")
        //     export_module_unknown(modules_array, module, id); // TO DO
    }
}

void export_module_cube(boost::json::array &modules_array, const module_ptr module, const uint64_t &id)
{
    boost::json::object module_object;

    module_object.emplace("id", id);
    module_object.emplace("type", "cube");

    modules_array.emplace_back(module_object);
}
void export_module_universal(boost::json::array &modules_array, const module_ptr module, const uint64_t &id)
{
    boost::json::object module_object;

    export_alpha_beta(module_object, module);

    export_gamma(module_object, module);

    module_object.emplace("id", id);
    module_object.emplace("type", "universal");

    modules_array.emplace_back(module_object);
}

void export_alpha_beta(boost::json::object &module_object, const module_ptr module)
{
    using namespace rofi;
    float alpha = 0;
    float beta = 0;

    for (const auto&[connector, dock] : module->getConnectorDocks())
    {
        if (dock == "A-Z")
            alpha = std::dynamic_pointer_cast<RotationConnector>(connector)->getCurrentRotation();
        else if (dock == "B-Z")
            beta = std::dynamic_pointer_cast<RotationConnector>(connector)->getCurrentRotation();
    }

    module_object.emplace("alpha", alpha);
    module_object.emplace("beta", beta);
}

void export_gamma(boost::json::object &module_object, const module_ptr module)
{
    const auto fst_orientation = module->getParts()->getVoxels().front()->getNode()->getRotationAxisYWorld();
    const auto snd_orientation = module->getParts()->getVoxels().back()->getNode()->getRotationAxisYWorld();
    const auto ref_axis = module->getParts()->getVoxels().front()->getNode()->getRotationAxisZWorld();
    auto gamma = glm::degrees(glm::orientedAngle(fst_orientation, snd_orientation, ref_axis));
    module_object.emplace("gamma", gamma);
}

void export_module_pad(boost::json::array &modules_array, const module_ptr module, const uint64_t &id)
{
    boost::json::object module_object;

    const auto [width, height] = get_pad_width_height_from_connector_count(module->getParts()->getConnections());

    module_object.emplace("width", width);
    module_object.emplace("height", height);

    module_object.emplace("id", id);
    module_object.emplace("type", "pad");

    modules_array.emplace_back(module_object);
}

void export_module_unknown(boost::json::array &modules_array, const module_ptr module, const uint64_t &id)
{
    // TO DO
}

void export_spaceJoints(boost::json::array &spaceJoints_array, const rofiworld_ptr rofiworld, const std::vector<objectbase_ptr> &selected_space_joints)
{
    using namespace rofi;

    for (const auto &component : selected_space_joints)
    {
        const auto connector = std::dynamic_pointer_cast<Connector>(component);
        ASSUMPTION(connector);
        auto connector_node = connector->getNode();
        const auto module = rofiworld->getModuleWithNode(connector_node);
        auto connector_id = module->getComponentID(connector);
        const auto module_type = module->getType();
        export_spaceJoint(spaceJoints_array, rofiworld, connector_node, connector_id, module_type);
        
    }
}

void export_spaceJoint(boost::json::array &spaceJoints_array, const rofiworld_ptr rofiworld, const node_ptr node, const uint64_t component_id, const std::string &module_type)
{
    boost::json::object spaceJoint_object;

    export_spaceJoint_point(spaceJoint_object, node);
    export_spaceJoint_joint(spaceJoint_object, node);
    export_spaceJoint_to(spaceJoint_object, rofiworld, rofiworld->getModulesMap().at(rofiworld->getModuleWithNode(node)), component_id);

    spaceJoints_array.emplace_back(spaceJoint_object);
}

void export_spaceJoint_point(boost::json::object &spaceJoint_object, const node_ptr node)
{
    boost::json::array point_array;
    const auto position = convert_to_rofi_export_coordinates(node->getPositionWorld(), node->getRotationAxisZWorld());

    for (int i = 0; i < 3; ++i)
        point_array.emplace_back(position[i]);

    spaceJoint_object.emplace("point", point_array);
}

void export_spaceJoint_joint(boost::json::object &spaceJoint_object, const node_ptr node)
{
    boost::json::object joint_object;

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

    export_spaceJoint_joint_matrix(joint_object, node);

    boost::json::array positions_array;
    joint_object.emplace("positions", positions_array);

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

void export_spaceJoint_joint_matrix(boost::json::object &joint_object, const node_ptr node)
{
    auto rotation_mat = convert_to_rofi_export_coordinates(node->getRotationMatWorld());
    float epsilon = 0.00001f;
    bool is_identity = true;

    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;
    }
    if (is_identity)
    {
        joint_object.emplace("sourceToDestination", "identity");
        return;
    }

    boost::json::array sourceToDestination_array;

    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);
    }

    joint_object.emplace("sourceToDestination", sourceToDestination_array);
}

void export_spaceJoint_to(boost::json::object &spaceJoint_object, const rofiworld_ptr rofiworld, const uint64_t module_id, uint64_t component_id)
{
    boost::json::object to_object;

    to_object.emplace("component", component_id);
    to_object.emplace("id", module_id);

    spaceJoint_object.emplace("to", to_object);
}

void export_moduleJoints(boost::json::array &moduleJoints_array, const rofiworld_ptr rofiworld)
{
    using namespace rofi;
    for (const auto &voxel : rofiworld->getVoxelGraph()->getVoxels())
    {
        const auto module_link = std::dynamic_pointer_cast<ModuleLink>(voxel);
        if (!module_link)
            continue;

        export_moduleJoint(moduleJoints_array, rofiworld, module_link);
    }
}

void export_moduleJoint(boost::json::array &moduleJoints_array, const rofiworld_ptr rofiworld, const std::shared_ptr<rofi::ModuleLink> module_link)
{
    boost::json::object moduleJoint_object;

    const auto from_con = module_link->getInterModuleConnectors().first.lock();
    const auto to_con = module_link->getInterModuleConnectors().second.lock();

    const auto from_module = rofiworld->getModuleWithNode(from_con->getNode());
    const auto to_module = rofiworld->getModuleWithNode(to_con->getNode());

    export_moduleJoint_fromto(moduleJoint_object, from_module, rofiworld->getModulesMap().at(from_module), from_con, "from");
    export_moduleJoint_fromto(moduleJoint_object, to_module, rofiworld->getModulesMap().at(to_module), to_con, "to");

    export_moduleJoint_orientation(moduleJoint_object, from_con, to_con);

    moduleJoints_array.emplace_back(moduleJoint_object);
}

void export_moduleJoint_fromto(boost::json::object &moduleJoint_object, const module_ptr module, const uint64_t module_id, const connector_ptr connector, const std::string &&where)
{
    boost::json::object from_to_object;

    from_to_object.emplace("id", module_id);

    if (module->getType() == "universal")
        from_to_object.emplace("connector", module->getConnectorDock(connector));
    else
        from_to_object.emplace("connector", module->getComponentID(connector));

    moduleJoint_object.emplace(where, from_to_object);
}

void export_moduleJoint_orientation(boost::json::object &moduleJoint_object, const connector_ptr from_con, const connector_ptr to_con)
{
    using namespace rofi;
    using enum cardinal;

    std::string orientation;

    const auto mutual_orientation = Connector::get_mutual_orientation(from_con, to_con);

    switch(mutual_orientation)
    {
        case North: 
            orientation = "North";
            break;
        case East:  
            orientation = "East";
            break;
        case South:  
            orientation = "South";
            break;
        case West:  
            orientation = "West";
            break;
        default:
            ASSUMPTION(false);
    }

    moduleJoint_object.emplace("orientation", orientation);
}

std::vector<rofiworld_ptr> import_saved_configurations_rofiworld()
{
    const std::filesystem::path rofiworlds_directory = "./data/rofi/rofiworlds";
    std::vector<rofiworld_ptr> imported_configurations;

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

        const auto rofiworld = import_rofiworld(dir_entry);
        if (rofiworld)
            imported_configurations.emplace_back(rofiworld);
    }

    return imported_configurations;
}

rofiworld_ptr import_rofiworld(const std::filesystem::path file_path)
{
    if (!std::filesystem::exists(file_path))
    {
        std::cerr << "Cannot import RofiWorld. Invalid file path." << file_path << std::endl;
        return nullptr;
    }

    const auto json_object = read_file(file_path).as_object();

    boost::json::array modules_array;
    boost::json::array spaceJoints_array;
    boost::json::array moduleJoints_array;

    get_rofiworld_data(json_object, modules_array, spaceJoints_array, moduleJoints_array);

    rofiworld_ptr rofiworld = std::make_shared<rofi::RofiWorld>();

    if (!import_modules(rofiworld, modules_array))
    {
        std::cerr << "Cannot import RofiWorld. Invalid module type." << file_path << std::endl;
        return nullptr;
    }

    std::set<module_ptr> spaceJoint_modules;
    if (!import_spaceJoints(rofiworld, spaceJoints_array, spaceJoint_modules))
    {
        std::cerr << "Cannot import RofiWorld. Specified spaceJoint component is other than 0." << file_path << std::endl;
        return nullptr;
    }

    import_moduleJoints(rofiworld, moduleJoints_array, spaceJoint_modules);

    rofiworld->setName(std::move(file_path.stem().string()));

    return rofiworld;
}

void get_rofiworld_data(const boost::json::object &json_object, 
                        boost::json::array &modules_array, 
                        boost::json::array &spaceJoints_array, 
                        boost::json::array &moduleJoints_array)
{
    modules_array = json_object.at("modules").as_array();
    spaceJoints_array = json_object.at("spaceJoints").as_array();
    moduleJoints_array = json_object.at("moduleJoints").as_array();
}

bool import_modules(rofiworld_ptr rofiworld, const boost::json::array &modules_array)
{
    const std::string internal_modules_directory = "./data/rofi/internal/";
    // const auto universal = import_module(modules_directory + std::string("UM.json"));
    // const auto cube = import_module(modules_directory + std::string("cube_full.json"));

    for (const auto &module_value : modules_array)
    {
        const auto module_object = module_value.as_object();
        const auto module_type = module_object.at("type");
        const auto module_type_string = module_type.if_string();
        const auto module_id = static_cast<uint64_t>(module_object.at("id").as_int64());

        module_ptr module_copy = nullptr;

        if (!module_type_string)
            return false;

        if (*module_type_string == "universal")
        {
            module_copy = import_module(internal_modules_directory + std::string("UM.json"));

            rofi::VoxelGraph::linkImportVoxelGraphToNodes(module_copy->getParts());
            rofi::Module::setModuleNodeHierarchy(module_copy->getParts(), std::string(*module_type_string));

            rotate_universal_joints(module_object, rofiworld, module_copy);
        }
        else if (*module_type_string == "cube")
        {
            module_copy = import_module(internal_modules_directory + std::string("cube_full.json"));

            rofi::VoxelGraph::linkImportVoxelGraphToNodes(module_copy->getParts());
            rofi::Module::setModuleNodeHierarchy(module_copy->getParts(), std::string(*module_type_string));
        }
        else if (*module_type_string == "pad")
        {
            const auto width = static_cast<uint64_t>(module_object.at("width").as_int64());
            const auto height = static_cast<uint64_t>(module_object.at("height").as_int64());

            module_copy = import_pad(width, height);

            rofi::VoxelGraph::linkImportVoxelGraphToNodes(module_copy->getParts());
            rofi::Module::setModuleNodeHierarchy(module_copy->getParts(), std::string(*module_type_string));
        }
        else
            return false;

        rofiworld->addModule(module_copy);
        rofiworld->changeModuleID(module_copy, module_id);
    }

    return true;
}

void rotate_universal_joints(const boost::json::object &module_object, rofiworld_ptr rofiworld, module_ptr um_module)
{
    const auto alpha = module_object.at("alpha").is_int64() ? module_object.at("alpha").as_int64() : module_object.at("alpha").as_double();
    const auto shoe_a = um_module->getConnectorByDock("A-Z");
    rofiworld->rotateConnector(shoe_a, static_cast<float>(alpha));

    const auto beta = module_object.at("beta").is_int64() ? module_object.at("beta").as_int64() : module_object.at("beta").as_double();
    const auto shoe_b = um_module->getConnectorByDock("B-Z");
    rofiworld->rotateConnector(shoe_b, static_cast<float>(beta));

    const auto gamma = module_object.at("gamma").is_int64() ? module_object.at("gamma").as_int64() : module_object.at("gamma").as_double();
    const auto body_b = um_module->getParts()->getVoxels().front()->getNode()->getPositionLocal() == glm::vec3(0.0f) 
                        ? um_module->getParts()->getVoxels().back()
                        : um_module->getParts()->getVoxels().front();
    rofiworld->rotateVoxel(body_b, static_cast<float>(gamma));
}

module_ptr import_pad(const uint64_t width, const uint64_t height)
{
    using namespace rofi;

    voxel_graph_ptr vg = std::make_shared<VoxelGraph>();
    std::map<objectbase_ptr, uint64_t> component_ids;
    uint64_t id_counter;

    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);
            component_ids.emplace(open_connector, id_counter);

            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));

            ++id_counter;
        }
    }

    std::string name = std::string("pad") + std::to_string(width) + std::to_string(height);
    std::string type = "pad";
    return std::make_shared<Module>(vg, name, type, component_ids);
}

bool import_spaceJoints(rofiworld_ptr rofiworld, const boost::json::array &spaceJoints_array, std::set<module_ptr> &spaceJoint_modules)
{
    
 
    for (const auto &spaceJoint_value : spaceJoints_array)
    {
        if (!import_spaceJoint(rofiworld, spaceJoint_value.as_object(), spaceJoint_modules))
            return false;
    }
    

    return true;
}

bool import_spaceJoint(rofiworld_ptr rofiworld, const boost::json::object &spaceJoint_object, std::set<module_ptr> &spaceJoint_modules)
{
    const auto to = spaceJoint_object.at("to").as_object();
    const auto module = import_spaceJoint_to(rofiworld, to);
    if (!module)
        return false;

    spaceJoint_modules.emplace(module);

    const auto point = spaceJoint_object.at("point").as_array();
    import_spaceJoint_point(module, point);
    const auto joint = spaceJoint_object.at("joint").as_object();
    import_spaceJoint_joint(rofiworld, module, joint);

    return true;
}

module_ptr import_spaceJoint_to(rofiworld_ptr rofiworld, const boost::json::object &to_object)
{
    if (to_object.at("component").as_int64() != 0)
        return nullptr;

    const auto id = static_cast<uint64_t>(to_object.at("id").as_int64());
    ASSUMPTION(rofiworld->containsID(id));
    return rofiworld->getModuleByID(id);
}

void import_spaceJoint_point(const module_ptr module, const boost::json::array &point)
{
    auto position = glm::vec3(0.0f);
    for (int i = 0; i < 3; ++i)
    {
        const auto &value = point[i];
        if (value.is_int64())
            position[i] = static_cast<float>(value.as_int64());
        else
            position[i] = static_cast<float>(value.as_double());
    }

    module->getNode()->setPosVec(convert_to_editor_coordinates(position));
}

void import_spaceJoint_joint(rofiworld_ptr rofiworld, const module_ptr module, const boost::json::object &joint)
{
    const auto rofi_rotation_mat = get_transformation_matrix(joint.at("sourceToDestination"));

    const auto editor_rotation_mat = convert_to_editor_import_coordinates(rofi_rotation_mat);

    const auto root_connector = std::dynamic_pointer_cast<rofi::Connector>(module->getComponentByID(0));
    ASSUMPTION(root_connector);

    auto node = root_connector->getNode();
    glm::mat4 mat = editor_rotation_mat;
    while (!node->getParent().expired())
    {
        mat = mat * glm::inverse(node->getModelMatLocal());
        node = node->getParent().lock();
    }

    rofiworld->rebuildNodeTree(module, module->getNode());
    module->getNode()->setRotationQuat(mat);

}

void import_moduleJoints(rofiworld_ptr rofiworld, const boost::json::array &moduleJoints_array, std::set<module_ptr> &spaceJoint_modules)
{
    std::set<module_ptr> searched_modules;
    for (const auto &module : spaceJoint_modules)
    {
        std::queue<module_ptr> q;
        q.push(module);
        
        while (!q.empty())
        {
            const auto source_module = q.front();
            for (const auto &moduleJoint_value : moduleJoints_array)
            {
                const auto oppposite_module = import_moduleJoint(rofiworld, source_module, searched_modules, moduleJoint_value.as_object());
                if (oppposite_module && !searched_modules.contains(oppposite_module))
                    q.push(oppposite_module);
            }
            searched_modules.emplace(q.front());
            q.pop();
        }
    }
}

module_ptr import_moduleJoint(rofiworld_ptr rofiworld, module_ptr module, const std::set<module_ptr> &searched_modules, const boost::json::object &moduleJoint_object)
{
    const auto from_object = moduleJoint_object.at("from").as_object();
    const auto [from_module, from_connector] = get_moduleJoint_module_connector(rofiworld, from_object);

    const auto to_object = moduleJoint_object.at("to").as_object();
    const auto [to_module, to_connector] = get_moduleJoint_module_connector(rofiworld, to_object);

    if (from_module != module && to_module != module)
        return nullptr;
    if (searched_modules.contains(from_module) || searched_modules.contains(to_module))
        return nullptr;

    const auto orientation_string = moduleJoint_object.at("orientation").as_string();
    const auto mutual_orientation = get_moduleJoint_orientation(orientation_string);

    rofiworld->rebuildNodeTree(from_module, from_connector->getNode());
    rofiworld->getVoxelGraph()->joinConnectors(from_connector, to_connector);
    // NOTE: order of connectors is swapped because we have to build additional modules around the spaceJoint module
    //       -> we go to "source"
    const auto fst_connector = from_module == module ? to_connector : from_connector;
    const auto snd_connector = from_module == module ? from_connector : to_connector;
    ASSUMPTION(fst_connector != snd_connector);
    rofiworld->teleportModulesByConnectors(fst_connector, snd_connector, mutual_orientation);

    return from_module == module ? to_module : from_module;
}

std::pair<module_ptr, connector_ptr> get_moduleJoint_module_connector(rofiworld_ptr rofiworld, const boost::json::object &where_object)
{
    const auto id = static_cast<uint64_t>(where_object.at("id").as_int64());
    const auto module = rofiworld->getModuleByID(id);

    const auto dock = where_object.at("connector");
    connector_ptr connector;
    if (dock.is_string())
        return {module, module->getConnectorByDock(std::string(dock.as_string()))};
    else // number
        return {module, std::dynamic_pointer_cast<rofi::Connector>(module->getComponentByID(dock.as_int64()))};
}

rofi::cardinal get_moduleJoint_orientation(const boost::json::string &orientation_string)
{
    using enum rofi::cardinal;
    if (orientation_string == "North")
        return North;
    if (orientation_string == "East")
        return East;
    if (orientation_string == "South")
        return South;
    if (orientation_string == "West")
        return West;

    ASSUMPTION(false);
    return Invalid;
}
