#include <edit/editor.hpp>
#include <algo/raycast.hpp>
#include <algo/misc.hpp>
#include <gfx/shapes.hpp>
#include <filein/module_loader.hpp>
#include <filein/rofiworld_loader.hpp>

#include <iostream>
#include <iterator>
#include <ranges>

#include <string.h>

namespace edit
{

void Editor::setError(std::string message)
{
    error_msg = message;
    error_start_second = timer.passed_seconds();
}

void Editor::manageUI()
{
    if (keyboard.just_pressed().contains("F1"))
        ui.help_controls = !ui.help_controls;

    // TO DO: fix - shows err message immediately after correct add operation
    // if (timer.passed_seconds() - ui.vocal_error_start_second > 2)
    // {
    //     ui.vocal_error = mouse.just_released().contains("MouseLeft");
    //     ui.vocal_error_start_second = ui.vocal_error ? timer.passed_seconds() : ui.vocal_error_start_second;
    // }

    ui.face_highlight = scene->getHighlight() != nullptr;

    if (auto selection = scene->getSelection())
    {
        voxel_selected = isVoxel(selection);
        connector_selected = isConnector(selection);
        ui.voxel = voxel_selected ? getVoxel(selection) : nullptr;
        ui.connector = connector_selected ? getConnector(selection) : nullptr;
    }
    else
    {
        voxel_selected = false;
        connector_selected = false;
        ui.voxel = nullptr;
        ui.connector = nullptr;
    }

    manageUISaveModule();
    manageUISaveRofiWorld();

    manageUIImportRofiWorld();
    manageUIClearRofiWorld();
    manageUISelectRofiWorld();
}

void Editor::manageUISaveModule()
{
    if (editor_phase != phase::one)
        return;
    
    bool saved = false;
    bool exported = false;
    if (ui.save_module_prompt)
    {
        if (!checkModuleExport(voxel_graph, editor_build_mode_I))
        {
            ui.save_module_error = true;
            ui.save_module_visible = true;
            
            setError("Error: Invalid Configuration");
        }
        else
        {
            saved = true;
            ui.save_module_error = false;
            
            setModuleTypeBuffer(editor_build_mode_I);
            export_module(voxel_graph, ui.name_buffer, ui.module_id, module_type_buffer);
            addToModules(std::string(ui.name_buffer), std::string(module_type_buffer));
        }
        ui.save_module_prompt = false;
        ui.export_file_prompt = false;
    }

    if (ui.export_file_prompt)
    {
        if (!checkModuleExport(voxel_graph, editor_build_mode_I))
        {
            ui.export_file_error = true;
            ui.export_file_visible = true;
            setError("Error: Invalid Configuration");
        }
        else
        {
            exported = true;
            ui.export_file_error = false;
            setModuleTypeBuffer(editor_build_mode_I);
            export_module(voxel_graph, ui.name_buffer, ui.module_id, module_type_buffer);
        }
        ui.export_file_prompt = false;
    }

    if (saved)
        resetModuleVoxelGraph(editor_build_mode_I);

    if (saved || exported)
    {
        ui.module_id = 0;
        ui.name_buffer[0] = '\0';
        memset(module_type_buffer, '\0', 256);
    }
}

void Editor::manageUISaveRofiWorld()
{
    if (editor_phase != phase::two)
        return;
    
    bool saved = false;
    bool exported = false;
    if (ui.save_rofiworld_prompt)
    {
        // Not sure if there should be an invalid state here
        if (false)
        {
            ui.save_rofiworld_error = true;
            ui.save_rofiworld_visible = true;
        }
        else
        {
            saved = true;
            ui.save_rofiworld_prompt = false;
            ui.save_rofiworld_error = false;
            addToRofiWorlds(rofiworld, ui.save_rofiworld_use_current_name ? rofiworld->getName() : std::string(ui.name_buffer));
        }
    }

    if (ui.export_file_prompt)
    {
        exported = true;
        ui.export_file_prompt = false;
        pickSpaceJoints(rofiworld, ui.export_rofiworld_use_selected_space_joint ? selected_module : nullptr);
        export_rofiworld(rofiworld, selected_space_joints, ui.name_buffer);
    }

    if (saved || exported)
    {
        selected_space_joints.clear();
        ui.name_buffer[0] = '\0';
    }
}

void Editor::manageUIImportRofiWorld()
{
    if (editor_phase != phase::two)
        return;

    if (!ui.import_file_prompt)
        return;

    const std::string rofiworlds_directory = "./data/rofi/rofiworlds/";
    auto imported_rofiworld = import_rofiworld(std::filesystem::path(rofiworlds_directory + std::string(ui.name_buffer) + std::string(".json")));
    if (imported_rofiworld)
    {
        for (const auto &module : imported_rofiworld->getModulesSet())
            setRoFIMesh(module->getParts());

        resetBuildModeIIVariables();
        setRofiWorldNodeTreeToScene(rofiworld->getNodeTree(), imported_rofiworld->getNodeTree());
        rofiworld = imported_rofiworld;
        voxel_graph = imported_rofiworld->getVoxelGraph();

        ui.import_file_error = false;
        ui.name_buffer[0] = '\0';
    }
    else
    {
        setError("Error: Cannot Import Configuration");
        ui.import_file_visible = true;
        ui.import_file_error = true;
    }

    ui.import_file_prompt = false;
}

void Editor::manageUIClearRofiWorld()
{
    if (!ui.start_new_rofiworld_prompt)
        return;

    resetBuildModeIIVariables();
    removeRofiWorldNodeTreeFromScene(rofiworld->getNodeTree());

    rofiworld = std::make_shared<rofi::RofiWorld>();
    voxel_graph = rofiworld->getVoxelGraph();

    ui.start_new_rofiworld_prompt = false;
}

void Editor::manageUISelectRofiWorld()
{
    if (editor_phase != phase::two)
        return;

    if (!ui.select_rofiworld_prompt || ui.selected_rofiworld == rofiworld)
    {
        ui.select_rofiworld_prompt = false;
        return;
    }

    resetBuildModeIIVariables();

    setRofiWorldNodeTreeToScene(rofiworld->getNodeTree(), ui.selected_rofiworld->getNodeTree());
    rofiworld = ui.selected_rofiworld;
    voxel_graph = ui.selected_rofiworld->getVoxelGraph();


    ui.select_rofiworld_prompt = false;
}

bool Editor::checkModuleExport(const voxel_graph_ptr &voxel_graph, const build_mode_I build_mode)
{
    using enum build_mode_I;

    switch(build_mode)
    {
        case cube:
            return !voxel_graph->getVoxels().empty();
        case universal:
            return !voxel_graph->getVoxels().empty() && voxel_graph->getVoxels().front()->hasSharedRotation();
        case pad:
            return true;
        case unknown:
            return !voxel_graph->empty();
    }
    ASSUMPTION(false);
    return false;
}

void Editor::resetBuildModeIIVariables()
{
    selected_module = nullptr;
    removeModuleFromScene(selected_add_module);
    selected_add_module = nullptr;
    prev_selected_add_module = nullptr;
    prev_rebuild_node = nullptr;
    can_rotate_component = true;
    rotation_locked_module = nullptr;
    collided_modules = {};
    selected_connectors = {nullptr, nullptr};
    first_connector_highlights = {glm::vec3(0.0f), nullptr};
    second_connector_highlights = {glm::vec3(0.0f), nullptr};
    selected_modules_connect = { nullptr, nullptr };
    can_connect_modules = true;
    selected_space_joints = {};

    connector_selected = false;
    voxel_selected = false;

    scene->manageSelection(nullptr);
    scene->manageHighlight(glm::vec3(0.0f), nullptr);
    scene->manageSecondarySelection({});
    scene->manageSecondaryHighlights({});
}

void Editor::update()
{
    updateEnvironment();

    /* User Input */
    changePhase();
    changeMode();

    /* Editor Functionality */
    selectPhase();

    manageUI();

}

void Editor::updateEnvironment()
{
    using enum phase;

    enterPhase(prev_phase);

    switch(editor_phase)
    {
        case one:
            convertComponentToConType();
            updateRoFIMesh(voxel_graph);
            exitModeI(prev_mode_I, prev_build_mode_I);
            break;
        case two:
            exitModeII(prev_mode_II, prev_build_mode_II);
            break;
    }

    prev_phase = editor_phase;
    prev_mode_I = editor_mode_I;
    prev_mode_II = editor_mode_II;
    prev_build_mode_I = editor_build_mode_I;
    prev_build_mode_II = editor_build_mode_II;
}

void Editor::changePhase()
{
    if (keyboard.down().contains("LeftAlt"))
    {
        if (keyboard.just_pressed().contains("1"))
            editor_phase = phase::one;
        else if (keyboard.just_pressed().contains("2"))
            editor_phase = phase::two;
    }

    /* BREAKPOINT DEBUGGING */
    if (keyboard.down().contains("LeftShift") && keyboard.just_pressed().contains("P"))
        std::cout << "Breakpoint\n";

}

void Editor::selectPhase()
{
    switch(editor_phase)
    {
        case phase::one:
            selectModePhaseOne();
            break;
        case phase::two:
            selectModePhaseTwo();
            break;
        default:
            ASSUMPTION(false);
            break;
    }
}

void Editor::enterPhase(const phase previous_phase)
{
    using enum phase;
    if (previous_phase == editor_phase)
        return;

    if (previous_phase == one)
    {
        enterPhaseTwo();
    }
    else if (previous_phase == two)
    {
        enterPhaseOne();
    }
}

void Editor::enterPhaseOne()
{
    if (voxel_graph == rofiworld->getVoxelGraph())
    {
        scene->manageSelection(nullptr);
        scene->manageHighlight(glm::vec3(0.0f), nullptr);
        scene->manageCameraAxes(false);
        removeRofiWorldNodeTreeFromScene(rofiworld->getNodeTree());
        removeModuleFromScene(selected_module);
        removeModuleFromScene(selected_add_module);
        selected_add_module = nullptr;
        voxel_graph = nullptr;
    }

    initializeDefaultVoxelGraphs();
}

void Editor::enterPhaseTwo()
{
    if (voxel_graph != rofiworld->getVoxelGraph())
    {
        scene->manageSelection(nullptr);
        scene->manageHighlight(glm::vec3(0.0f), nullptr);
        scene->manageCameraAxes(true);
        removeVoxelGraphFromScene(voxel_graph);
        addRofiWorldNodeTreeToScene(rofiworld->getNodeTree());
        voxel_graph = rofiworld->getVoxelGraph();
    }
    selected_module = nullptr;
    hover_nearest_node = nullptr;
}

void Editor::changeMode()
{
    switch(editor_phase)
    {
        case phase::one:
            changeModeI();
            break;
        case phase::two:
            changeModeII();
            break;
        default:
            ASSUMPTION(false);
    }
}

void Editor::changeModeI()
{
    using enum mode_I;
    auto previous_mode = editor_mode_I;

    /* Independent change */
    if (keyboard.down().contains("LeftCtrl"))
    {
        if (keyboard.just_pressed().contains("1"))
            editor_mode_I = build;
        else if (keyboard.just_pressed().contains("2"))
            editor_mode_I = spectate;
        else if (keyboard.just_pressed().contains("3"))
            editor_mode_I = debug;
    }
}

void Editor::changeModeII()
{
    using enum mode_II;
    auto previous_mode = editor_mode_II;

    if (keyboard.down().contains("LeftCtrl"))
    {
        if (keyboard.just_pressed().contains("1"))
            editor_mode_II = build;
        else if (keyboard.just_pressed().contains("2"))
            editor_mode_II = spectate;
        else if (keyboard.just_pressed().contains("3"))
            editor_mode_II = debug;
    }
}

void Editor::exitModeI(const mode_I previous_mode, const build_mode_I previous_build_mode)
{
    exitBuildModeI(previous_build_mode);

    using enum mode_I;
    if (previous_mode == editor_mode_I)
        return;

    if (previous_mode == spectate)
        exitSpectate();
    else if (previous_mode == build)
        exitBuild();
}

void Editor::exitBuildModeI(const build_mode_I previous_build_mode)
{
    if (previous_build_mode == editor_build_mode_I)
        return;

    using enum build_mode_I;

    if (voxel_graph != module_type_voxel_graphs[editor_build_mode_I])
    {
        scene->manageSelection(nullptr);
        scene->manageHighlight(glm::vec3(0.0f), nullptr);
        hover_nearest_node = nullptr;
        setVoxelGraphToScene(voxel_graph, module_type_voxel_graphs[editor_build_mode_I]);
        voxel_graph = module_type_voxel_graphs[editor_build_mode_I];
    }
}

void Editor::exitModeII(const mode_II previous_mode, const build_mode_II previous_build_mode)
{
    exitBuildModeII(previous_build_mode);

    using enum mode_II;
    if (previous_mode == editor_mode_II)
        return;
    
    if (previous_mode == spectate)
        exitSpectate();
    else if (previous_mode == build)
        exitBuild();
}

void Editor::selectModePhaseOne() 
{
    /* EDIT */
    switch (editor_mode_I)
    {
        case mode_I::build:
            changeBuildModeI();
            selectBuildModeI();
            break;
        case mode_I::spectate:
            doSpectateAction();
            break;
        case mode_I::debug:
            doDebugAction();
            break;
        default:
            ASSUMPTION(false);
            break;
    }
}

void Editor::selectModePhaseTwo() 
{
    switch (editor_mode_II)
    {
        case mode_II::build:
            changeBuildModeII();
            selectBuildModeII();
            break;
        case mode_II::spectate:
            doSpectateAction();
            break;
        case mode_II::debug:
            doDebugAction();
            break;
        default:
            ASSUMPTION(false);
            break;
    }
}

void Editor::changeBuildModeI()
{
    if (keyboard.down().contains("LeftShift"))
    {
        if (keyboard.just_pressed().contains("1"))
            editor_build_mode_I = build_mode_I::cube;
        else if (keyboard.just_pressed().contains("2"))
            editor_build_mode_I = build_mode_I::universal;
        else if (keyboard.just_pressed().contains("3"))
            editor_build_mode_I = build_mode_I::pad;
        else if (keyboard.just_pressed().contains("4"))
            editor_build_mode_I = build_mode_I::unknown;
    }
}

void Editor::changeBuildModeII()
{
    if (keyboard.down().contains("LeftShift"))
    {
        if (keyboard.just_pressed().contains("1"))
            editor_build_mode_II = build_mode_II::add_module;
        else if (keyboard.just_pressed().contains("2"))
            editor_build_mode_II = build_mode_II::connect_module;
        else if (keyboard.just_pressed().contains("3"))
            editor_build_mode_II = build_mode_II::manipulate_module;
        else if (keyboard.just_pressed().contains("4"))
            editor_build_mode_II = build_mode_II::rotate_component;
    }
}

void Editor::selectBuildModeI()
{
    switch (editor_build_mode_I)
    {
        case build_mode_I::cube:
            doBuildCubeAction();
            break;
        case build_mode_I::universal:
            doBuildUniversalAction();
            break;
        case build_mode_I::pad:
            doBuildPadAction();
            break;
        case build_mode_I::unknown:
            doBuildUnknownAction();
            break;
        default:
            ASSUMPTION(false);
            break;
    }

    connector_selected = isConnector(scene->getSelection());
    voxel_selected = isVoxel(scene->getSelection());
}

void Editor::selectBuildModeII()
{
    if (prev_build_mode_II != editor_build_mode_II)
    {
        // NOTE: This is not ideal, gets called twice (also in updateEnvironment)
        //       could be fixed by using different selected_module variable in each mode
        exitBuildModeII(prev_build_mode_II);
        selected_module = nullptr;
    }
    
    switch (editor_build_mode_II)
    {
        case build_mode_II::add_module:
            doAddModuleAction();
            break;
        case build_mode_II::connect_module:
            doConnectModuleAction();
            break;
        case build_mode_II::manipulate_module:
            doManipulateModuleAction();
            break;
        case build_mode_II::rotate_component:
            doRotateComponentAction();
            break;
        default:
            ASSUMPTION(false);
            break;
    }
}

/* ----------------------------------------------- */
/* DO ACTION                                       */
/* ----------------------------------------------- */

void Editor::doBuildCubeAction()
{
    hoverOver();
    selectObject();
    updateAllowedComponents();
    updateBuildHint();
    controlBuild();
    selection_previous_frame = scene->getSelection();
}

void Editor::doBuildUniversalAction()
{
    hoverOver();
    selectObject();
    updateAllowedComponents();
    updateBuildHint();
    controlBuild();
    selection_previous_frame = scene->getSelection();
}

void Editor::doBuildPadAction()
{
    selectObject();
    updateAllowedComponents();
    createPad(selected_pad_width, selected_pad_height);
    selection_previous_frame = scene->getSelection();
}

void Editor::doBuildUnknownAction()
{
    hoverOver();
    selectObject();
    updateAllowedComponents();
    updateBuildHint();
    controlBuild();
    selection_previous_frame = scene->getSelection();
}

void Editor::updateAllowedComponents()
{
    switch(editor_build_mode_I)
    {
        using enum build_mode_I;
        using enum rofi::component;
        case cube:
            allowed_components = { voxel, open_con };
            break;
        case universal:
            allowed_components = { voxel, open_con, rotation_con, shared_rotation_con };
            break;
        case pad:
            allowed_components = {};
            return;
        case unknown:
            allowed_components = { voxel, fixed_con, open_con, rotation_con, shared_rotation_con };
            break;
        default:
            ASSUMPTION(false);
            break;
    }
    if (std::find(allowed_components.begin(), allowed_components.end(), chosen_component) == allowed_components.end())
        chosen_component = allowed_components.front();
}

void Editor::updateBuildHint()
{
    if (!scene->getHighlight())
        return;

    glm::vec3 color = checkBuildValid() ? glm::vec3(0.0f, 1.0f, 0.0f) : glm::vec3(1.0f, 0.0f, 0.0f);
    auto highlight_mesh = scene->getNodeMesh(scene->getHighlight());
    
    for (auto &mesh : highlight_mesh)
        mesh->setColor(color);
}

bool Editor::checkBuildValid()
{
    switch(editor_build_mode_I)
    {
        using enum build_mode_I;
        case cube:
            return checkCubeBuildValid();
        case universal:
            return checkUniversalBuildValid();
        case pad:
            return checkPadBuildValid();
        case unknown:
            return checkUnknownBuildValid();
        default:
            ASSUMPTION(false);
            return false;
    }
}

bool Editor::checkCubeBuildValid()
{
    using enum rofi::component;

    if (isConType(chosen_component))
        return chosen_component == open_con && checkConnectorRequest();
    else
        return voxel_graph->getVoxels().empty();
}

bool Editor::checkUniversalBuildValid()
{
    using enum rofi::component;

    if (isConType(chosen_component))
        return checkConnectorRequest() && (isConnector(scene->getSelection()) || chosen_component != open_con);
    else
        return voxel_graph->getVoxels().empty();
}

bool Editor::checkPadBuildValid()
{
    return true;
}

bool Editor::checkUnknownBuildValid()
{
    if (isConType(chosen_component))
        return checkConnectorRequest();
    else
        return checkVoxelRequest();
}

void Editor::doSpectateAction()
{
    controlSpectate();
}

void Editor::doDebugAction()
{
    controlDebugSelection();

    switch (control_state)
    {
        case state::select:
            selectObject();
            break;
        case state::move:
            moveObject(scene->getSelection());
            break;
        case state::rotate:
            rotateObject();
            break;
        case state::plus:
            addObject();
            break;
        default:
            ASSUMPTION(false);
            break;
    }
}

/* ----------------------------------------------- */
/* CONTROL                                         */
/* ----------------------------------------------- */

void Editor::controlBuild()
{
    using namespace rofi;

    controlComponentSelection();
    convertComponentToConType();

    bool lmb_released = mouse.just_released().contains("MouseLeft");
    bool delete_pressed = keyboard.just_pressed().contains("Delete");

    if (delete_pressed)
    {
        removeComponent();
        return;
    }
    
    if (!lmb_released)
        return;

    if (!checkBuildValid())
        return;

    switch(chosen_component)
    {
        case component::voxel:
            addVoxel();
            break;
        case component::fixed_con:
        case component::open_con:
        case component::rotation_con:
        case component::shared_rotation_con:
            addConnector();
            break;
        default:
            ASSUMPTION(false);
            break;
    }
}

void Editor::controlComponentSelection()
{
    if (keyboard.just_pressed().empty() 
        || keyboard.down().contains("LeftCtrl") 
        || keyboard.down().contains("LeftAlt") 
        || keyboard.down().contains("LeftShift"))
        return;

    switch(editor_build_mode_I)
    {
        using enum build_mode_I;
        case cube:
            controlCubeComponentSelection();
            break;
        case universal:
            controlUniversalComponentSelection();
            break;
        case pad:
            controlPadComponentSelection();
            break;
        case unknown:
            controlUnknownComponentSelection();
            break;
        default:
            ASSUMPTION(false);
            break;
    }
}

void Editor::controlCubeComponentSelection()
{
    auto pressed = keyboard.just_pressed();
    if (pressed.contains("1"))
        chosen_component = rofi::component::voxel;
    if (pressed.contains("2"))
        chosen_component = rofi::component::open_con;
}

void Editor::controlUniversalComponentSelection()
{
    auto pressed = keyboard.just_pressed();
    if (pressed.contains("1"))
        chosen_component = rofi::component::voxel;
    if (pressed.contains("2"))
        chosen_component = rofi::component::open_con;
    if (pressed.contains("3"))
        chosen_component = rofi::component::rotation_con;
    if (pressed.contains("4"))
        chosen_component = rofi::component::shared_rotation_con;
}

void Editor::controlPadComponentSelection()
{
    auto pressed = keyboard.just_pressed();
    if (pressed.contains("1"))
        chosen_component = rofi::component::open_con;
}

void Editor::controlUnknownComponentSelection()
{
    auto pressed = keyboard.just_pressed();
    if (pressed.contains("1"))
        chosen_component = rofi::component::voxel;
    if (pressed.contains("2"))
        chosen_component = rofi::component::fixed_con;
    if (pressed.contains("3"))
        chosen_component = rofi::component::open_con;
    if (pressed.contains("4"))
        chosen_component = rofi::component::rotation_con;
    if (pressed.contains("5"))
        chosen_component = rofi::component::shared_rotation_con;
}

void Editor::controlSpectate()
{
    if (keyboard.just_pressed().contains("H"))
    {
        ui_visible = !ui_visible;
        scene->toggleVisibility(scene->getSelectionAABB(), ui_visible);
        scene->toggleVisibility(scene->getHighlight(), ui_visible);
        scene->toggleVisibility(scene->getGrid(), ui_visible);
        scene->toggleVisibility(scene->getObjectAxes(), ui_visible);
        scene->toggleVisibility(scene->getCameraAxes(), ui_visible);
        scene->toggleVisibility(scene->getCameraAxesDebug(), ui_visible);
    }
}

void Editor::controlDebugSelection()
{ 
    if (keyboard.just_pressed().empty() 
        || keyboard.down().contains("LeftCtrl") 
        || keyboard.down().contains("LeftAlt"))
        return;

    auto pressed = keyboard.just_pressed();
    if (pressed.contains("1"))
        control_state = state::select;
    if (pressed.contains("2"))
        control_state = state::move;
    if (pressed.contains("3"))
        control_state = state::rotate;
    if (pressed.contains("4"))
        control_state = state::plus;
}

/* ----------------------------------------------- */
/* EXIT                                            */
/* ----------------------------------------------- */

void Editor::exitSpectate()
{
    ui_visible = true;
    scene->toggleVisibility(scene->getSelectionAABB(), true);
    scene->toggleVisibility(scene->getHighlight(), true);
    scene->toggleVisibility(scene->getGrid(), true);
    scene->toggleVisibility(scene->getObjectAxes(), true);
}

void Editor::exitBuild()
{
    hover_nearest_node = nullptr;
    scene->manageHighlight(glm::vec3(0.0f), nullptr);
}

/* ----------------------------------------------- */
/* GENERAL EDIT                                    */
/* ----------------------------------------------- */

void Editor::hoverOver()
{
    auto [ ray_position, ray_direction ] = get_pick_ray(scene, mouse, window);

    auto [ hit_coord, nearest_node ] = find_intersection(scene->getScene(), ray_position, ray_direction);

    hover_nearest_node = nearest_node;

    scene->manageHighlight(hit_coord, nearest_node);
}

void Editor::selectObject()
{
    if (!mouse.just_released().contains("MouseLeft"))
        return;

    glm::vec3 hit_coord;
    node_ptr nearest_node;

    if (!hover_nearest_node)
    {
        auto [ ray_position, ray_direction ] = get_pick_ray(scene, mouse, window);
        std::tie(hit_coord, nearest_node) = find_intersection(scene->getScene(), ray_position, ray_direction);
        /* VISUAL REPRESENTATION OF RAY */
        // scene->addRay(ray_position, hit_coord);
    }
    else
        nearest_node = hover_nearest_node;

    scene->manageSelection(nearest_node);
}

void Editor::rotateObject()
{
    if (!scene->getSelection())
        return;
    auto controller = controls->getActiveObjCtrl();
    controller->setup(timer.dt(), scene->getSelection());
    controller->mouseRotate(mouse);
}

void Editor::moveObject(node_ptr node_to_move)
{
    if(!scene->getSelection())
        return;

    // Mouse held down with init. click not on object
    if (!object_moving && !mouse.just_pressed().contains("MouseLeft")) 
        return;

    // User stopped movement
    if (object_moving && !mouse.down().contains("MouseLeft"))
    {
        object_moving = false;
        return;
    }

    auto [ ray_position, ray_direction ] = get_pick_ray(scene, mouse, window);

    auto [ _ignore, nearest_node ] = find_intersection(scene->getScene(), ray_position, ray_direction);

    // Initial click not onto the selected object or its child
    if (!object_moving && (nearest_node != scene->getSelection() && !node_to_move->isParentOf(nearest_node))) 
        return;

    useObjectController(node_to_move, controls->getActiveObjCtrl(), scene->getActiveCameras()[0].lock(), &ray_position, &ray_direction);
}

void Editor::useObjectController(node_ptr node, obj_control_ptr controller,  camera_ptr camera, glm::vec3* ray_position, glm::vec3* ray_direction)
{

    glm::vec3 ray_pos;
    glm::vec3 ray_dir;

    if (ray_position == nullptr || ray_direction == nullptr)
    {
        std::tie(ray_pos, ray_dir) = get_pick_ray(scene, mouse, window);
        ray_position = &ray_pos;
        ray_direction = &ray_dir;
    }

    controller->setup(timer.dt(), node);

    if (keyboard.down().contains("LeftShift"))
        controller->mouseElevationMove(*ray_position, *ray_direction, 
                                         camera->getNode()->getRotationWorld(), 
                                         prev_plane_intersect, object_moving);
    else
        controller->mouseHorizontalMove(*ray_position, *ray_direction, prev_plane_intersect, object_moving);
}

void Editor::addObject()
{
    if (!scene->getSelection() || !(mouse.just_released().contains("MouseLeft") || mouse.down().contains("MouseLeft")))
        return;

    glm::vec3 position = scene->getSelection()->getPositionWorld();
    glm::quat rotation = scene->getSelection()->getRotationWorld();
    glm::vec3 scale = scene->getSelection()->getScaleWorld();

    auto [ ray_position , ray_direction ] = get_pick_ray(scene, mouse, window);

    bool cam_is_perspective = scene->getActiveCameras()[0].lock()->isPerspective();
    float distance = cam_is_perspective
                     ? glm::length(ray_position - position)
                     : 1.0f;
    
    auto new_item_pos = ray_position + distance * ray_direction;

    auto selection_copy = Node::create(new_item_pos, rotation, scale);

    // TO DO: rewrite ifs in this method - ugly
    if ((!scene->getPreAddAABB() && mouse.down().contains("MouseLeft"))
        || mouse.just_released().contains("MouseLeft"))
    {    
        for (auto &object : scene->getSelection()->getObjects())
            selection_copy->addObject(object);   
    }

    if (!scene->getPreAddAABB() && mouse.down().contains("MouseLeft"))
        scene->addPreAddAABB(std::move(selection_copy));
    else if (mouse.down().contains("MouseLeft"))
        scene->getPreAddAABB()->setFrame(new_item_pos, rotation, scale);
    else if (mouse.just_released().contains("MouseLeft"))
    {
        scene->addNode(std::move(selection_copy));
        scene->setSelection(scene->getScene().back());
        scene->manageSelection(scene->getSelection());
        scene->manageHighlight(glm::vec3(), nullptr);

        scene->removeNode(scene->getPreAddAABB());
        scene->setPreAddAABB(nullptr);
    }
}

/* ----------------------------------------------- */
/* ROFI MANAGEMENT                                 */
/* ----------------------------------------------- */


void Editor::initializeDefaultVoxelGraphs()
{
    using enum build_mode_I;
    using namespace rofi;

    initializeCube();
    initializeUniversal();
    initializePad();
    initializeUnknown();

    // NOTE: Intial editor_build_mode_I is mode_I::cube
    setVoxelGraphToScene(voxel_graph, module_type_voxel_graphs[editor_build_mode_I]);
    voxel_graph = module_type_voxel_graphs[editor_build_mode_I];
}

void Editor::initializeCube()
{
    using enum build_mode_I;
    using namespace rofi;

    module_type_voxel_graphs[cube] = std::make_shared<VoxelGraph>();
    const auto rot = glm::angleAxis(glm::radians(90.0f), glm::vec3(0.0f, 0.0f, -1.0f)) 
                     * glm::angleAxis(glm::radians(90.0f), glm::vec3(0.0f, -1.0f, 0.0f));
    auto cube_node = Node::create(glm::vec3(0.0f), rot);
    auto cube_voxel = module_type_voxel_graphs[cube]->addVoxel(cube_node);
    addVoxelMesh(cube_node);
    cube_node->addObject(cube_voxel);
}

void Editor::initializeUniversal()
{
    using enum build_mode_I;
    using namespace rofi;

    module_type_voxel_graphs[universal] = std::make_shared<VoxelGraph>();

    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 uni_voxel_graph = module_type_voxel_graphs[universal];

    auto fst_voxel = uni_voxel_graph->addVoxel(fst_voxel_node);
    auto snd_voxel = uni_voxel_graph->addVoxel(snd_voxel_node);
    auto shared_con = uni_voxel_graph->addConnector(shared_con_node, connector_type::shared_rotation, enum_to_vec(side::z_pos));
    uni_voxel_graph->join(shared_con, fst_voxel, enum_to_vec(side::z_pos));
    uni_voxel_graph->join(shared_con, snd_voxel, enum_to_vec(side::z_pos));

    auto fst_rot_con_node = Node::create(glm::vec3(-0.5f, 0.0f, 0.0f), get_connector_face_rotation(enum_to_vec(side::x_neg)));
    auto snd_rot_con_node = Node::create(glm::vec3(1.5f, 0.0f, 0.0f), get_connector_face_rotation(enum_to_vec(side::x_pos)));

    auto fst_rot_con = uni_voxel_graph->addConnector(fst_rot_con_node, connector_type::rotation, enum_to_vec(side::x_pos), enum_to_vec(side::z_neg));
    auto snd_rot_con = uni_voxel_graph->addConnector(snd_rot_con_node, connector_type::rotation, enum_to_vec(side::x_pos), enum_to_vec(side::z_neg));

    std::vector<connector_ptr> rot_cons = {fst_rot_con, snd_rot_con};
    for (auto &x_rot_con : rot_cons)
    {
        auto rot_con = std::dynamic_pointer_cast<rofi::RotationConnector>(x_rot_con);
        ASSUMPTION(rot_con);
        auto bound_1 = rot_con->getBoundConnectors().first.lock();
        auto bound_2 = rot_con->getBoundConnectors().second.lock();
        auto fst_node = bound_1->getNode();
        auto snd_node = bound_2->getNode();
        fst_node->addObject(bound_1);
        snd_node->addObject(bound_2);
        addConnectorMesh(fst_node, rofi::connector_type::empty);
        addConnectorMesh(snd_node, rofi::connector_type::empty);
    }

    uni_voxel_graph->join(fst_rot_con, fst_voxel, enum_to_vec(side::z_neg));
    uni_voxel_graph->join(snd_rot_con, snd_voxel, enum_to_vec(side::z_neg));

    fst_voxel_node->addObject(fst_voxel);
    snd_voxel_node->addObject(snd_voxel);
    shared_con_node->addObject(shared_con);
    fst_rot_con_node->addObject(fst_rot_con);
    snd_rot_con_node->addObject(snd_rot_con);

    addConnectorMesh(shared_con_node, connector_type::shared_rotation);
    scene->addMesh(fst_voxel_node, body_mesh);
    scene->addMesh(snd_voxel_node, body_mesh);
    scene->addMesh(fst_rot_con_node, empty_shoe_mesh);
    scene->addMesh(snd_rot_con_node, empty_shoe_mesh);

    uni_voxel_graph->joinImplicit();
    // module_type_voxel_graphs[universal] = load_configuration(...);
}

void Editor::initializePad()
{
    using enum build_mode_I;
    using namespace rofi;

    module_type_voxel_graphs[pad] = std::make_shared<VoxelGraph>();
    std::size_t initial_width = 1;
    std::size_t initial_height = 1;
    generatePad(initial_width, initial_height);
}

void Editor::initializeUnknown()
{
    using enum build_mode_I;
    using namespace rofi;

    module_type_voxel_graphs[unknown] = std::make_shared<VoxelGraph>();
    auto unknown_cube_node = Node::create();
    auto unknown_cube_voxel = module_type_voxel_graphs[unknown]->addVoxel(unknown_cube_node);
    addVoxelMesh(unknown_cube_node);
    unknown_cube_node->addObject(unknown_cube_voxel);
}

void Editor::createPad(std::size_t &width, std::size_t &height)
{
    using enum build_mode_I;
    using namespace rofi;

    if (!is_creating_pad)
        return;

    removeVoxelGraphFromScene(module_type_voxel_graphs[pad]);

    module_type_voxel_graphs[pad] = std::make_shared<VoxelGraph>();

    generatePad(width, height);

    is_creating_pad = false;

    voxel_graph = module_type_voxel_graphs[pad];
    addVoxelGraphToScene(voxel_graph);
}

void Editor::generatePad(std::size_t &width, std::size_t &height)
{
    using enum build_mode_I;
    using namespace rofi;

    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 = module_type_voxel_graphs[pad]->addConnector(open_con_node, connector_type::open);
            addConnectorMesh(open_con_node, connector_type::fixed);
            open_con_node->addObject(open_connector);

            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 = module_type_voxel_graphs[pad]->addPadBoard(pad_board_node);
            scene->addMesh(pad_board_node, pad_board_mesh);
            pad_board_node->addObject(pad_board);

            module_type_voxel_graphs[pad]->join(open_connector, pad_board, enum_to_vec(side::y_pos));
        }
    }
}

void Editor::setVoxelGraphToScene(voxel_graph_ptr prev_vg, voxel_graph_ptr new_vg)
{
    removeVoxelGraphFromScene(prev_vg);
    addVoxelGraphToScene(new_vg);
}

void Editor::addVoxelGraphToScene(voxel_graph_ptr vg)
{
    if (!vg)
        return;

    for (auto &voxel : vg->getVoxels())
        scene->addNode(voxel->getNode());
    for (auto &connector : vg->getConnections())
        scene->addNode(connector->getNode());

}

void Editor::removeVoxelGraphFromScene(voxel_graph_ptr vg)
{
    if (!vg)
        return;

    for (auto &voxel : vg->getVoxels())
        scene->removeNode(voxel->getNode());
    for (auto &connector : vg->getConnections())
        scene->removeNode(connector->getNode());
}

void Editor::resetModuleVoxelGraph(const build_mode_I type)
{
    auto prev_vg = module_type_voxel_graphs[type];

    switch(type)
    {
        using enum build_mode_I;
        case cube:
            initializeCube();
            break;
        case universal:
            initializeUniversal();
            break;
        case pad:
            initializePad();
            break;
        case unknown:
            initializeUnknown();
            break;
    }

    setVoxelGraphToScene(prev_vg, module_type_voxel_graphs[type]);
    voxel_graph = module_type_voxel_graphs[type];
}

void Editor::addToModules(std::string name, std::string type)
{
    scene->manageSelection(nullptr);
    scene->manageHighlight(glm::vec3(0.0f), nullptr);
    const std::string modules_directory = "./data/rofi/modules/";
    const std::string module_file = name + std::string(".json");
    const std::filesystem::path module_path = modules_directory + module_file;
    module_ptr new_module = import_module(module_path);
    rofi::VoxelGraph::linkImportVoxelGraphToNodes(new_module->getParts());
    rofi::Module::setModuleNodeHierarchy(new_module->getParts(), type);
    setRoFIMesh(new_module->getParts());

    if (const auto it = std::find_if(modules.begin(), modules.end(), 
                                     [&](const module_ptr module) { return module->getName() == name; } );
        it != modules.end())
    {
        modules.erase(it);
    }

    modules.emplace_back(new_module);
}

void Editor::addToRofiWorlds(rofiworld_ptr rofiworld, const std::string name)
{
    rofiworld->setName(name);
    rofiworld_selection.emplace_back(rofiworld);
}

void Editor::removeFromModules(voxel_graph_ptr vg)
{
    std::erase_if(modules, [&vg](const module_ptr &module) { return module->getParts() == vg; });
}

void Editor::removeFromModules(module_ptr module)
{
    std::erase(modules, module);
}

void Editor::convertComponentToConType()
{
    using namespace rofi;
    switch (chosen_component)
    {
        case component::voxel:
            break;
        case component::fixed_con:
            chosen_con_type = connector_type::fixed;
            break;
        case component::open_con:
            chosen_con_type = connector_type::open;
            break;
        case component::rotation_con:
            chosen_con_type = connector_type::rotation;
            break;
        case component::shared_rotation_con:
            chosen_con_type = connector_type::shared_rotation;
            break;
        default:
            ASSUMPTION(false);
            break;
    }
}

void Editor::setModuleTypeBuffer(const build_mode_I build_mode)
{
    std::string type;
    switch(build_mode)
    {
        using enum build_mode_I;
        case cube:
            type = "cube";
            break;
        case universal:
            type = "universal";
            break;
        case pad:
            type = "pad";
            break;
        case unknown:
            type = "unknown";
            break;
        default:
            ASSUMPTION(false);
            break;
    }

    auto type_c_str = type.c_str();
    strncpy(module_type_buffer, type_c_str, type.length());
}

bool Editor::isConType(rofi::component component)
{
    if (component == rofi::component::voxel)
        return false;
    if (component == rofi::component::fixed_con
        || component == rofi::component::open_con
        || component == rofi::component::rotation_con
        || component == rofi::component::shared_rotation_con)
        return true;

    ASSUMPTION(false);
    return false;
}

bool Editor::isVoxel(node_ptr selection) const
{
    if (!selection)
        return false;
        
    for (auto object : selection->getObjects())
    {
        auto obj = dynamic_pointer_cast<rofi::Voxel>(object);
        if (obj)
            return true;
    }
    return false;
}
bool Editor::isConnector(node_ptr selection) const
{
    if (!selection)
        return false;
    
    for (auto object : selection->getObjects())
    {
        auto obj = dynamic_pointer_cast<rofi::Connector>(object);
        if (obj)
            return true;
    }
    return false;
}

voxel_ptr Editor::getVoxel(node_ptr selection) const
{
    for (auto object : selection->getObjects())
    {
        auto vox_obj = std::dynamic_pointer_cast<rofi::Voxel>(object);
        if (vox_obj)
            return vox_obj;

        auto con_obj = std::dynamic_pointer_cast<rofi::Connector>(object);
        if (con_obj && con_obj->getType(true) == rofi::connector_type::rotation)
            return con_obj->getOppositeConnection().lock();
    }
    ASSUMPTION(false);
    return nullptr;
}

connector_ptr Editor::getConnector(node_ptr selection) const
{
    for (auto &object : selection->getObjects())
    {
        auto con_obj = dynamic_pointer_cast<rofi::Connector>(object);
        if(con_obj)
            return con_obj;
        if (!scene->getHighlight())
            continue;
        auto vox_obj = dynamic_pointer_cast<rofi::Voxel>(object);
        if (!vox_obj)
            continue;

        ASSUMPTION(vox_obj->isConnectedAt(scene->getHighlightDirection()));
        return vox_obj->getConnectorAt(scene->getHighlightDirection()).lock();
    }
    ASSUMPTION(false);
    return nullptr;
}

glm::vec3 Editor::getNewItemLocalDirection() const
{
    auto selection = scene->getSelection();
    ASSUMPTION(selection);
    ASSUMPTION(isConnector(selection) || isVoxel(selection));

    return scene->getHighlightDirection();

}

glm::vec3 Editor::generateAxisDirection()
{
    ASSUMPTION(scene->getHighlight());

    auto local_direction = glm::abs(scene->getHighlightDirection());
    glm::vec3 axis_direction = glm::vec3();
    if (local_direction == glm::vec3(1.0f, 0.0f, 0.0f))
        axis_direction = primary_axis ? glm::vec3(0.0f, 0.0f, 1.0f) : glm::vec3(0.0f, 1.0f, 0.0f);
    if (local_direction == glm::vec3(0.0f, 1.0f, 0.0f))
        axis_direction = primary_axis ? glm::vec3(1.0f, 0.0f, 0.0f) : glm::vec3(0.0f, 0.0f, 1.0f);
    if (local_direction == glm::vec3(0.0f, 0.0f, 1.0f))
        axis_direction = primary_axis ? glm::vec3(1.0f, 0.0f, 0.0f) : glm::vec3(0.0f, 1.0f, 0.0f);

    ASSUMPTION(axis_direction != glm::vec3());

    return axis_direction;
}

void Editor::addConnector()
{
    if (!checkConnectorRequest())
        return;

    auto selection = scene->getSelection();
    

    if (isConnector(selection))
    {
        modifyRotationConnector(getConnector(selection));
        return;
    }

    ASSUMPTION(isVoxel(selection));

    auto voxel_local_direction = getNewItemLocalDirection();
    auto voxel = getVoxel(selection);

    if (chosen_con_type == rofi::connector_type::shared_rotation)
    {
        auto voxel_world_direction = glm::round(voxel->getNode()->getRotationMatWorld() * glm::vec4(voxel_local_direction, 0.0f));
        ASSUMPTION(glm::length(voxel_world_direction) == 1.0f);
        auto shared_voxel_rotation = get_shared_voxel_face_rotation(voxel_world_direction);

        voxel->getNode()->setRotationQuat(shared_voxel_rotation);
        voxel_local_direction = rofi::enum_to_vec(rofi::side::z_pos);
        ASSUMPTION(glm::length(voxel_local_direction) == 1.0f);
    }

    auto node = createConnectorNode(selection, voxel_local_direction);
    scene->addNode(node);
    addConnectorMesh(node, chosen_con_type);

    auto connector = createConnector(node, voxel_local_direction);
    node->addObject(connector);

    voxel_graph->join(connector, voxel, voxel_local_direction);

    // TO DO: this should happen in voxel_graph
    if (chosen_con_type == rofi::connector_type::fixed || chosen_con_type == rofi::connector_type::shared_rotation)
    {
        glm::vec3 world_direction = voxel->getNode()->getRotationMatWorld() * glm::vec4(voxel_local_direction, 1.0f);
        auto snd_voxel_node = Node::create(voxel->getNode()->getPositionWorld() + world_direction, connector->getNode()->getRotationWorld());
        if (voxel_graph->cellAvailable(snd_voxel_node))
            snd_voxel_node = addVoxelAtPosition(voxel->getNode()->getPositionWorld() + world_direction, connector->getNode()->getRotationWorld());
        else
        {
            if (chosen_con_type == rofi::connector_type::shared_rotation)
                voxel_graph->getVoxelAtPosition(snd_voxel_node)->getNode()->setRotationQuat(snd_voxel_node->getRotationWorld());
            snd_voxel_node = nullptr;
        }
            
        selection = snd_voxel_node;
    }

    voxel_graph->joinImplicit();

    scene->manageSelection(chosen_con_type == rofi::connector_type::open ? node : selection);
    scene->manageHighlight(glm::vec3(), nullptr);
}

bool Editor::checkConnectorRequest()
{
    auto selection = scene->getSelection();
    if (!selection)
        return false;

    if (isVoxel(selection))
        return checkConnectorRequestOnVoxel(selection);
    else if (isConnector(selection))
        return checkConnectorRequestOnRotationConnector(selection);
    else 
    {
        setError("Add Connector Error: No RoFI object selected");
        return false;
    }
}

bool Editor::checkConnectorRequestOnVoxel(node_ptr selection)
{
    if (!scene->getHighlight())
        return false;

    ASSUMPTION(isVoxel(selection));
    auto voxel = getVoxel(selection);

    auto voxel_local_direction = getNewItemLocalDirection();

    if (voxel->isConnectedAt(voxel_local_direction))
    {
        setError("Add Connector Error: Voxel already contains a Connector in this direction");
        return false;
    }

    if (!voxel->slotCompatible(voxel_local_direction, chosen_con_type))
    {
        setError("Add Connector Error: Voxel slot not compatible with this Connector type");
        return false;
    }

    switch(chosen_con_type)
    {
        using enum rofi::connector_type;
        case rotation:
        {
            if (!voxel->hasSharedRotation())
                return false;

            auto axis_direction = generateAxisDirection();
            auto under_rotation = glm::cross(scene->getHighlightDirection(), axis_direction);

            if (voxel->isConnectedAt(rofi::vec_to_enum(axis_direction)) 
                || voxel->isConnectedAt(rofi::vec_to_enum(-axis_direction))
                || voxel->isConnectedAt(rofi::vec_to_enum(under_rotation))
                || voxel->isConnectedAt(rofi::vec_to_enum(-under_rotation)))
            {
                setError("Add Connector Error: Cannot hook Rotation Connector to Voxel, sides are occupied");
                return false;
            }

            break;
        }
        case shared_rotation:
        {
            if (!voxel->hasNoConnections())
            {
                setError("Add Connector Error: Voxel already connected, cannot add Shared Connector");
                return false;
            }

            auto world_direction = voxel->getNode()->getRotationMatWorld() * glm::vec4(voxel_local_direction, 1.0f);
            auto neighbor_voxel_position = voxel->getNode()->getPositionWorld() + glm::vec3(world_direction);

            if (!voxel_graph->cellAvailable(neighbor_voxel_position))
            {
                voxel_ptr neighbor_voxel = voxel_graph->getVoxelAtPosition(neighbor_voxel_position);
                if (!neighbor_voxel->hasNoConnections())
                {
                    setError("Add Connector Error: Neighboring Voxel already connected, cannot add Shared Connector");
                    return false;
                }
            }
            // No break here is correct
        }
        case fixed:
        {
            auto world_direction = voxel->getNode()->getRotationMatWorld() * glm::vec4(voxel_local_direction, 1.0f);
            auto neighbor_voxel_position = voxel->getNode()->getPositionWorld() + glm::vec3(world_direction);

            if (!voxel_graph->cellAvailable(neighbor_voxel_position))
            {
                voxel_ptr neighbor_voxel = voxel_graph->getVoxelAtPosition(neighbor_voxel_position);
                auto neighbor_voxel_local_direction = neighbor_voxel->getNode()->getInverseRotationMatWorld() * world_direction;

                if (!neighbor_voxel->slotCompatible(glm::vec3(-neighbor_voxel_local_direction), chosen_con_type)
                    || neighbor_voxel->hasSharedRotation() && !neighbor_voxel->hasRotationConnector())
                {
                    setError("Add Connector Error: Neigbouring Voxel is not compatible with selected Connector");
                    return false;
                }
            }
        }
        // No break here is correct
        case open:
        {
            if (voxel->hasSharedRotation() && !voxel->hasRotationConnector())
            {
                setError("Add Connector Error: Voxel of Body type has no Connector Shoe");
                return false;
            }
            break;
        }
        default:
            ASSUMPTION(false);
            break;
    }

    return true;
}

bool Editor::checkConnectorRequestOnRotationConnector(node_ptr selection)
{
    ASSUMPTION(isConnector(selection));
    auto connector = getConnector(selection);

    if (connector->getType(true) != rofi::connector_type::rotation)
    {
        setError("Add Connector Error: Selected Connector not of Rotation type");
        return false;
    }
    if (chosen_con_type == rofi::connector_type::empty && connector->isFull())
    {
        setError("Add Connector Error: Cannot change to this Connector type - Connector occupied");
        return false;
    }

    if (chosen_con_type != rofi::connector_type::fixed && chosen_con_type != rofi::connector_type::open
        && chosen_con_type != rofi::connector_type::empty)
    {
        setError("Add Connector Error: Cannot change to this Connector type");
        return false;
    }

    return true;
}

void Editor::addConnectorMesh(node_ptr node, rofi::connector_type con_type)
{
    mesh_ptr mesh;

    using enum rofi::connector_type;
    switch(con_type)
    {
        case fixed:
            mesh = fixed_connector_mesh;
            break;
        case open:
            mesh = open_connector_mesh;
            break;
        case rotation:
            mesh = empty_shoe_mesh;
            break;
        case shared_rotation:
            mesh = shared_connector_mesh;
            break;
        case empty:
            mesh = empty_connector_mesh;
            break;
        default:
            ASSUMPTION(false);
            break;
    }
    
    scene->addMesh(node, mesh);
}

connector_ptr Editor::createConnector(node_ptr node, glm::vec3 voxel_local_direction)
{
    using namespace rofi;
    using enum connector_type;
    connector_ptr connector;

    switch(chosen_con_type)
    {
        case fixed:
        case open:
            connector = voxel_graph->addConnector(node, chosen_con_type);
            break;
        case rotation:
            connector = voxel_graph->addConnector(node, chosen_con_type, generateAxisDirection(), voxel_local_direction);
            addRotationConnectorSides(connector);
            break;
        case shared_rotation:
            connector = voxel_graph->addConnector(node, chosen_con_type, enum_to_vec(side::z_pos));
            break;
    }

    return connector;
}

node_ptr Editor::createConnectorNode(node_ptr selection, glm::vec3 voxel_local_direction)
{
        /* Local (in Voxel space) position */
    auto position = voxel_local_direction * 0.5f;
    /*Rotate */
    position = selection->getRotationMatWorld() * glm::vec4(position, 1.0f);
    /* Translate */
    position += selection->getPositionWorld();

    auto node = Node::create(position);

    auto face_rotation = get_connector_face_rotation(voxel_local_direction);
    node->setRotationQuat(face_rotation);
    node->rotateRotationQuat(scene->getSelection()->getRotationWorld());

    return node;
}

node_ptr Editor::createConnectorNode(connector_ptr connector)
{
    auto node = scene->addNode(connector->getNode());
    node->addObject(connector);

    return node;
}

void Editor::addRotationConnectorSides(connector_ptr connector)
{
    auto rot_con = std::dynamic_pointer_cast<rofi::RotationConnector>(connector);
    ASSUMPTION(rot_con);
    auto fst_node = createConnectorNode(rot_con->getBoundConnectors().first.lock());
    auto snd_node = createConnectorNode(rot_con->getBoundConnectors().second.lock());
    addConnectorMesh(fst_node, rofi::connector_type::empty);
    addConnectorMesh(snd_node, rofi::connector_type::empty);
}

void Editor::modifyRotationConnector(connector_ptr connector)
{
    auto rot_con = std::dynamic_pointer_cast<rofi::RotationConnector>(connector);
    ASSUMPTION(rot_con);

    rot_con->setSemanticType(chosen_con_type);
    auto con_node = findConnectorNode(rot_con);
    changeRotationConnectorMesh(con_node, connector, scene->getHighlightDirection());

    voxel_graph->joinImplicit();

    scene->manageSelection(con_node);
    scene->manageHighlight(scene->getHighlightDirection(), con_node);

}

void Editor::changeRotationConnectorMesh(node_ptr node, connector_ptr connector, glm::vec3 direction)
{
    // MTO DO: use direction to add connectors on non-head sides

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

    scene->removeAllMesh(node);

    mesh_ptr mesh;
    bool is_head = rot_con->isHead();
    bool is_full = connector->isFull();

    switch(chosen_con_type)
    {
        using enum rofi::connector_type;
        case empty:
            mesh = is_head ? empty_shoe_mesh : empty_connector_mesh;
            break;
        case fixed:
            mesh = is_full ? connected_connector_mesh : fixed_connector_mesh;
            if (is_head)
                scene->addMesh(node, empty_shoe_mesh);  
            break;
        case open: 
            mesh = is_full ? connected_connector_mesh : open_connector_mesh;
            if (is_head)
                scene->addMesh(node, empty_shoe_mesh); 
            break;
        default:
            ASSUMPTION(false);
            mesh = nullptr;
    }
    scene->addMesh(node, mesh);
}

node_ptr Editor::findConnectorNode(connector_ptr connector)
{
    for (auto &node : scene->getScene())
    {
        for (auto &object : node->getObjects())
        {
            if (std::dynamic_pointer_cast<rofi::Connector>(object) == connector)
                return node;
        }
    }

    return nullptr;
}

void Editor::addVoxel()
{
    /* INITIAL VOXEL CASE */
    if (voxel_graph->getVoxels().empty() && voxel_graph->getConnections().empty())
    {
        addVoxelAtPosition(glm::vec3(0.0f, 0.0f, 0.0f));
        return;
    }
        
    if(!checkVoxelRequest())
        return;
    
    auto connector = getConnector(scene->getSelection());

    auto local_direction = getNewItemLocalDirection();

    auto node = createVoxelNode(connector);

    if (!check_grid_alignment(node))
    {
        setError("Cannot add voxel there - not grid aligned");
        return;
    }

    auto voxel = createVoxel(node);
    node->addObject(voxel);
    addVoxelMesh(node);

    // NOTE: The tiny little '-' very important here
    voxel_graph->join(connector, -local_direction, voxel);
    voxel_graph->joinImplicit();

    scene->manageSelection(node);
    scene->manageHighlight(glm::vec3(), nullptr);
}

node_ptr Editor::addVoxelAtPosition(node_ptr node)
{
    if (!voxel_graph->cellAvailable(node))
    {
        setError("Cell occupied");
        return nullptr;
    }

    scene->addNode(node);    
    auto voxel = createVoxel(node);
    node->addObject(voxel);
    addVoxelMesh(node);

    scene->manageSelection(node);
    scene->manageHighlight(glm::vec3(), nullptr);
    voxel_graph->joinImplicit();

    return node;
}
node_ptr Editor::addVoxelAtPosition(glm::vec3 position, glm::quat rotation)
{
    return addVoxelAtPosition(Node::create(position, rotation));
}

bool Editor::checkVoxelRequest()
{
    if (voxel_graph->getVoxels().empty() && voxel_graph->getConnections().empty())
        return true;

    /* CHECK VALID HIGHLIGHT */
    if (!scene->getHighlight())
        return false;

    if (!isConnector(scene->getSelection()))
    {
        setError("To add a voxel, you must select a connector to add it to");
        return false;
    }

    auto connector = getConnector(scene->getSelection());
    if (connector->isFull())
    {
        setError("Connector already has 2 connections");
        return false;
    }

    auto con_type = connector->getType(false);
    using enum rofi::connector_type;
    switch(con_type)
    {
        case shared_rotation:
            setError("Invalid connector selected");
            return false;
        case empty:
            setError("Cannot connect Voxel to an empty connector");
            return false;
        case rotation:
            ASSUMPTION(false);
            break;
        case open:
            if (connector->isEmpty())
            {
                auto local_direction = getNewItemLocalDirection();
                if (local_direction != rofi::enum_to_vec(rofi::side::z_neg) 
                    && local_direction != rofi::enum_to_vec(rofi::side::z_pos))
                    return false;

                break;
            }    
        case fixed:
        {
            auto other_voxel = connector->getOppositeConnection().lock();
            auto other_voxel_side = other_voxel->getSideWithConnector(connector);
            ASSUMPTION(other_voxel_side != rofi::side::undefined);
            return true;
        }
    }

    return true;
}

void Editor::addVoxelMesh(node_ptr node)
{
    scene->addMesh(node, voxel_mesh);
}

voxel_ptr Editor::createVoxel(node_ptr node)
{
    using namespace rofi;
    auto voxel = voxel_graph->addVoxel(node);
    
    return voxel;
}

node_ptr Editor::createVoxelNode(connector_ptr connector)
{
    /* Local (in connector space) position */
    auto position = getNewItemLocalDirection() * 0.5f;
    /* Rotation */
    position = connector->getNode()->getRotationMatWorld() * glm::vec4(position, 1.0f);
    /* Translation */
    position += connector->getNode()->getPositionWorld();

    auto rotation = connector->getNode()->getRotationWorld();

   auto node = Node::create(position, rotation);
   scene->addNode(node);

   return node;
}

void Editor::removeComponent()
{
    auto selection = scene->getSelection();
    if (!selection)
        return;

    if (isConnector(selection))
        removeConnector();
    else if (isVoxel(selection))
        removeVoxel();
    else
        setError("Select a component to remove");
}

void Editor::removeConnector()
{
    auto selection = scene->getSelection();
    if (!isConnector(selection))
    {
        setError("removeConnector called with no connector selected");
        return;
    }

    auto connector = getConnector(selection);
    auto rot_con = std::dynamic_pointer_cast<rofi::RotationConnector>(connector);

    /* Change Rotation Connector type back to empty instead of delete */
    if (rot_con && rot_con->getType(false) != rofi::connector_type::empty)
    {
        chosen_con_type = rofi::connector_type::empty;

        addConnector();
        return;
    }

    /* REMOVE CONNECTOR FROM VOXEL_GRAPH */
    voxel_graph->disjoinAll(connector);
    voxel_graph->removeConnector(connector);

    if (rot_con)
    {
        scene->removeNode(findConnectorNode(rot_con->getBoundConnectors().first.lock()));
        scene->removeNode(findConnectorNode(rot_con->getBoundConnectors().second.lock()));
    }

    scene->removeNode(selection);
    scene->manageSelection(nullptr);
    scene->manageHighlight(glm::vec3(), nullptr);

    seekAndDestroyOrphans();
}

void Editor::removeVoxel()
{
    auto selection = scene->getSelection();
    if (!isVoxel(selection))
    {
        setError("removeVoxel called with no voxel selected");
        return;
    }

    /* REMOVE VOXEL FROM VOXEL_GRAPH */
    auto voxel = getVoxel(selection);
    voxel_graph->disjoinAll(voxel);
    voxel_graph->removeVoxel(voxel);

    scene->removeNode(selection);
    scene->manageSelection(nullptr);
    scene->manageHighlight(glm::vec3(), nullptr);

    seekAndDestroyOrphans();
}

void Editor::seekOrphans(std::vector<node_ptr> &to_remove_nodes, 
                         std::vector<connector_ptr> &to_remove_connectors)
{
    for (auto node : scene->getScene())
    {
        for (auto object : node->getObjects())
        {
            auto con = std::dynamic_pointer_cast<rofi::Connector>(object);
            if (!con)
                continue;

            if (!voxel_graph->contains(con))
                to_remove_nodes.emplace_back(node);
            else if (con->isStranded())
            {
                to_remove_nodes.emplace_back(node);
                to_remove_connectors.emplace_back(con);
            }
            else if (auto fst_voxel = con->getConnection().first.lock(); 
                     fst_voxel 
                     && fst_voxel->hasRotationConnector() 
                     && (!fst_voxel->hasSharedRotation() || fst_voxel->getSharedConnector().lock()->isStranded()))
            {
                to_remove_nodes.emplace_back(node);
                to_remove_connectors.emplace_back(con);
            }
        }
    }
}

void Editor::destroyOrphans(const std::vector<node_ptr> &to_remove_nodes,
                            const std::vector<connector_ptr> &to_remove_connectors)
{
    for (auto &con : to_remove_connectors)
        voxel_graph->disjoinAll(con);

    voxel_graph->removeConnectors(to_remove_connectors);

    for (auto &node : to_remove_nodes)
        scene->removeNode(node);
}

void Editor::seekAndDestroyOrphans()
{
    std::vector<node_ptr> to_remove_nodes;
    std::vector<connector_ptr> to_remove_connectors;

    seekOrphans(to_remove_nodes, to_remove_connectors);
    // MTO DO: saveChildren() if nodes would have children worth saving
    destroyOrphans(to_remove_nodes, to_remove_connectors);
}

void Editor::updateRoFIMesh(const voxel_graph_ptr vg)
{
    for (auto &voxel : vg->getVoxels())
    {
        bool changed = false;
        if (voxel->hasSharedRotation() && !scene->containsMesh(voxel->getNode(), body_mesh))
        {
            changed = true;
            scene->removeAllMesh(voxel->getNode());
            scene->addMesh(voxel->getNode(), body_mesh);
        }
        else if (!voxel->hasSharedRotation() && (!scene->containsMesh(voxel->getNode(), voxel_mesh) && !scene->containsMesh(voxel->getNode(), pad_board_mesh)))
        {
            changed = true;
            scene->removeAllMesh(voxel->getNode());
            scene->addMesh(voxel->getNode(), voxel_mesh);
        }

        if (changed && voxel->getNode() == scene->getSelection())
        {
            scene->manageSelection(voxel->getNode());
        }
    }
}

void Editor::toggleRoFIInvalidStateMesh(bool invalid, const std::set<module_ptr> &modules)
{
    for (auto &module : modules)
        setRoFIMesh(module->getParts(), invalid);
}

void Editor::setRoFIMesh(const voxel_graph_ptr vg, bool invalid_state)
{
    for (const auto &voxel : vg->getVoxels())
        setVoxelMesh(voxel, invalid_state);

    for (const auto &connector : vg->getConnections())
        setConnectorMesh(connector, invalid_state);
}

void Editor::setVoxelMesh(const voxel_ptr voxel, bool invalid_state)
{
    if (std::dynamic_pointer_cast<rofi::ModuleLink>(voxel))
        return;

    int i = invalid_state ? 1 : 0;
    auto voxel_meshes_local = std::dynamic_pointer_cast<rofi::PadBoard>(voxel) ? pad_board_meshes : voxel_meshes;

    scene->removeAllMesh(voxel->getNode());
    const auto mesh = voxel->hasSharedRotation() ? body_meshes[i] : voxel_meshes_local[i];
    scene->addMesh(voxel->getNode(), mesh);
}

void Editor::setConnectorMesh(const connector_ptr connector, bool invalid_state)
{
    scene->removeAllMesh(connector->getNode());

    mesh_ptr mesh;
    bool is_full = connector->isFull();
    int i = invalid_state ? 1 : 0;

    switch (connector->getType(true))
    {
        using enum rofi::connector_type;
        case fixed:
            mesh = is_full ? connected_connector_meshes[i] : fixed_connector_meshes[i];
            break;
        case open:
            mesh = is_full ? connected_connector_meshes[i] : connector->isOnPad() ? fixed_connector_meshes[i] : open_connector_meshes[i];
            break;
        case shared_rotation:
            mesh = shared_connector_meshes[i];
            break;
        case rotation:
        {
            const auto rot_con = std::dynamic_pointer_cast<rofi::RotationConnector>(connector);
            ASSUMPTION(rot_con);
            bool is_head = rot_con->isHead();
            switch(connector->getType(false))
            {
                case empty:
                    mesh = is_head ? empty_shoe_meshes[i] : empty_connector_meshes[i];
                    break;
                case fixed:
                    mesh = is_full ? connected_connector_meshes[i] : fixed_connector_meshes[i];
                    if (is_head)
                        scene->addMesh(connector->getNode(), empty_shoe_meshes[i]);
                    break;
                case open:
                    mesh = is_full ? connected_connector_meshes[i] : open_connector_meshes[i];
                    if (is_head)
                        scene->addMesh(connector->getNode(), empty_shoe_meshes[i]);
                    break;
                default:
                    ASSUMPTION(false);
            }
            
            break;
        }
        default:
            ASSUMPTION(false);
    }
    scene->addMesh(connector->getNode(), mesh);
}

/* ----------------------------------------------- */
/* ROFI MANAGEMENT -- PHASE TWO                    */
/* ----------------------------------------------- */

void Editor::doAddModuleAction()
{
    controlAddModule();
    updateSelectedModule();
    addModule();
    detectCollision(is_placing_module|| is_removing_module, rofiworld);
}

void Editor::doConnectModuleAction()
{
    controlConnectModule();
    selectModule();
    updateSelectedConnectors();
    updateConnectionHint();
    connectModule();
    detectCollision(is_connecting_modules || is_disconnecting_modules, rofiworld);
}

void Editor::doManipulateModuleAction()
{
    controlManipulateModule();
    selection_previous_frame = scene->getSelection();
    selectModule();
    manipulateModule();
    detectCollision(is_moving_module || is_rotating_module,  rofiworld);
}

void Editor::doRotateComponentAction()
{
    selectModule();
    updateSelectedComponent();
    updateRotationHint();
    rotateComponent();
    detectCollision(can_rotate_component, rofiworld);
}

void Editor::exitBuildModeII(const build_mode_II previous_build_mode)
{
    if (previous_build_mode == editor_build_mode_II)
        return;

    using enum build_mode_II;
    if (previous_build_mode == add_module)
        exitAddModule();
    else if (previous_build_mode == connect_module)
        exitConnectModule();
    else if (previous_build_mode == manipulate_module)
        exitManipulateModule();
    else if (previous_build_mode == rotate_component)
        exitRotateComponent();
}

void Editor::exitAddModule()
{
    removeModuleFromScene(selected_add_module);
    is_placing_module = false;
    selected_add_module = nullptr;
    prev_selected_add_module = nullptr;
    scene->manageSelection(nullptr);
    selected_module = nullptr;
}

void Editor::exitConnectModule()
{
    scene->manageSelection(nullptr);
    scene->manageHighlight(glm::vec3(0.0f), nullptr);
    scene->manageSecondarySelection({});
    scene->manageSecondaryHighlights({});
    hover_nearest_node = nullptr;
    selected_module = nullptr;
    selected_connectors = {nullptr, nullptr};
    first_connector_highlights = {glm::vec3(0.0f), nullptr};
    second_connector_highlights = {glm::vec3(0.0f), nullptr};

    is_connecting_modules = false;
    is_disconnecting_modules = false;
}

void Editor::exitManipulateModule()
{
    scene->manageSelection(nullptr);
    selected_module = nullptr;
    is_moving_module = false;
    is_rotating_module = false;
    is_removing_module = false;
}

void Editor::exitRotateComponent()
{
    scene->manageSelection(nullptr);
    selected_module = nullptr;
    prev_rebuild_node = nullptr;
    can_rotate_component = true;
    is_rotating_component = false;

    if (rotation_locked_module)
    {
        toggleRoFIInvalidStateMesh(false, {rotation_locked_module});
    }
    rotation_locked_module = nullptr;
}

void Editor::controlAddModule()
{
    if (keyboard.just_pressed().contains("Escape"))
        selected_add_module = nullptr;

    is_placing_module = selected_add_module && mouse.just_pressed().contains("MouseLeft");

    is_removing_module = selected_module && keyboard.just_pressed().contains("Delete");
}

void Editor::controlConnectModule()
{
    if (keyboard.just_pressed().contains("Escape"))
    {
        selected_connectors = {nullptr, nullptr};
        first_connector_highlights = {glm::vec3(0.0f), nullptr};
        second_connector_highlights = {glm::vec3(0.0f), nullptr};
        selected_modules_connect = { nullptr, nullptr };
        scene->manageSelection(nullptr);
        scene->manageHighlight(glm::vec3(0.0f), nullptr);
        scene->manageSecondarySelection({first_connector_highlights.second, second_connector_highlights.second});
        scene->manageSecondaryHighlights({first_connector_highlights});
        return;
    }
    
    is_connecting_modules = keyboard.down().contains("M") || is_connecting_modules;
}

void Editor::controlManipulateModule()
{
    is_moving_module = (mouse.down().contains("MouseLeft") && 
                        (keyboard.down().contains("LeftCtrl")) || keyboard.down().contains("LeftShift"))
                       || is_moving_module;
}

void Editor::updateSelectedModule()
{
    if (!selected_add_module)
        selectModule();
    else if (selected_module && selected_add_module)
    {
        selected_module = nullptr;
        scene->manageSelection(nullptr);
    }
    
    if (prev_selected_add_module != selected_add_module)
    {
        setModuleToScene(prev_selected_add_module, selected_add_module);
        setModuleToMouse();
    }
        
    prev_selected_add_module = selected_add_module;
}

void Editor::setModuleToMouse()
{
    if (!selected_add_module)
        return;

    auto [ ray_position, ray_direction ] = get_pick_ray(scene, mouse, window);

    glm::vec3 plane_intersect = get_horizontal_plane_intersect(selected_add_module->getNode(), ray_position, ray_direction);
    plane_intersect.y = prev_plane_intersect.y;
    selected_add_module->getNode()->setPosVec(plane_intersect);
    prev_plane_intersect = plane_intersect;
}

void Editor::updateSelectedConnectors()
{
    if (!mouse.just_released().contains("MouseLeft"))
        return;

    auto selection = scene->getSelection();
    if (!selection)
        return;

    if (!checkSelectedConnectorsValid())
    {
        selected_module = nullptr;
        scene->manageSelection(nullptr);
        scene->manageHighlight(glm::vec3(0.0f), nullptr);
        return;
    }

    auto connector = getConnector(selection);

    if (connector == selected_connectors.first)
    {
        selected_connectors = {nullptr, nullptr};
        selected_modules_connect = { nullptr, nullptr };
        first_connector_highlights = {glm::vec3(0.0f), nullptr};
        second_connector_highlights = {glm::vec3(0.0f), nullptr};
        selected_module = nullptr;
        scene->manageSelection(nullptr);
        scene->manageHighlight(glm::vec3(0.0f), nullptr);
    }
    else if (!selected_connectors.first 
             || (selected_connectors.first
                 && rofiworld->getModuleWithNode(selected_connectors.first->getNode()) == rofiworld->getModuleWithNode(selection)))
    {
        selected_connectors.first = connector;
        selected_modules_connect.first = selected_module;
        first_connector_highlights = {scene->getHighlightDirection(), selection};
    }
    else if (connector == selected_connectors.second)
    {
        selected_connectors.second = nullptr;
        selected_modules_connect.second = nullptr;
        second_connector_highlights = {glm::vec3(0.0f), nullptr};
        selected_module = nullptr;
        scene->manageSelection(nullptr);
        scene->manageHighlight(glm::vec3(0.0f), nullptr);
    }
    else if (!selected_connectors.second || (selected_connectors.first && selected_connectors.second))
    {
        selected_connectors.second = connector;
        selected_modules_connect.second = selected_module;
        second_connector_highlights = {scene->getHighlightDirection(), selection};
    }

    scene->manageSecondarySelection({first_connector_highlights.second, second_connector_highlights.second});
    scene->manageSecondaryHighlights({first_connector_highlights});
}

void Editor::updateConnectionHint()
{
    if (can_connect_modules)
        return;

    if (timer.passed_seconds() - error_start_second > 1)
        can_connect_modules = true;
}

bool Editor::checkSelectedConnectorsValid()
{
    auto selection = scene->getSelection();

    if (!isConnector(selection))
        return false;

    if (auto con_type = getConnector(selection)->getType(false);
        con_type == rofi::connector_type::empty || con_type == rofi::connector_type::shared_rotation)
        return false;

    if (selected_connectors.first && selected_connectors.second && selection != selected_connectors.first->getNode()
        && rofiworld->getModuleWithNode(selected_connectors.first->getNode()) == rofiworld->getModuleWithNode(selection))
        return false;

    return true;
}

void Editor::updateSelectedComponent()
{
    if (!mouse.just_released().contains("MouseLeft"))
        return;

    auto selection = scene->getSelection();
    if (!selection)
        return;

    if (!checkSelectedComponentValid())
    {
        selected_module = nullptr;
        scene->manageSelection(nullptr);
        scene->manageHighlight(glm::vec3(0.0f), nullptr);
    }
}

bool Editor::checkSelectedComponentValid()
{
    auto selection = scene->getSelection();

    if (!isConnector(selection) && !isVoxel(selection))
        return false;

    if (isVoxel(selection) && !getVoxel(selection)->hasSharedRotation())
        return false;

    if (isConnector(selection))
    {
        if (getConnector(selection)->getType(true) != rofi::connector_type::rotation)
            return false;

        if (!std::dynamic_pointer_cast<rofi::RotationConnector>(getConnector(selection))->isHead())
            return false;
    }

    return true;
}

void Editor::updateRotationHint()
{
    if (!rotation_locked_module)
        return;

    if (timer.passed_seconds() - error_start_second > 1)
        toggleRoFIInvalidStateMesh(false, {rotation_locked_module});
    else
        toggleRoFIInvalidStateMesh(true, {rotation_locked_module});
}

void Editor::addModule()
{
    if (!selected_module && !selected_add_module)
        return;

    if (selected_add_module && is_placing_module)
    {
        object_moving = false;
        placeModule(selected_add_module);
        return;
    }
    else if (selected_add_module)
        useObjectController(selected_add_module->getNode(), controls->getActiveObjCtrl(), scene->getActiveCameras()[0].lock());
    else if (selected_module && is_removing_module)
        removeModule(selected_module);
}

void Editor::placeModule(module_ptr module)
{
    auto module_copy = rofi::Module::copy(module);
    rofiworld->addModule(module_copy);
    
    addModuleToScene(module_copy);
}

void Editor::setModuleToScene(module_ptr prev_module, module_ptr new_module)
{
    removeModuleFromScene(prev_module);
    addModuleToScene(new_module);
}

void Editor::addModuleToScene(module_ptr module)
{
    if (!module)
        return;

    scene->addNode(module->getNode());
}

void Editor::removeModuleFromScene(module_ptr module)
{
    if (!module)
        return;

    scene->removeNode(module->getNode());
}

void Editor::manipulateModule()
{
    if (!selected_module)
        return;

    if (mouse.just_released().contains("MouseLeft"))
        rebuildRofiWorldNodeTree(selected_module, selected_module->getNode(), rofiworld);

    if (is_rotating_module)
        rotateModule(selected_module, module_rotation_angles);
    else if (is_moving_module)
    {
        moveObject(selected_module->getNode());
        //ui move
        selected_module->getNode()->translatePosVecWorld(module_position);
    }
    else
        object_moving = false;
}

void Editor::selectModule()
{
    if (!mouse.just_released().contains("MouseLeft") || mouse.down().contains("MouseLeft"))
        return;

    selectObject();
    auto nearest_node = scene->getSelection();

    if (!nearest_node)
        selected_module = nullptr;
    else
    {
        selected_module = rofiworld->getModuleWithNode(nearest_node);
        if (!selected_module)
            return;
        prev_plane_intersect = selected_module->getNode()->getPositionWorld();
    }
}

void Editor::removeModule(module_ptr module)
{
    removeRofiWorldNodeTreeFromScene(rofiworld->getNodeTree());

    auto neighbor_modules = rofiworld->getNeighborModules(module);
    rofiworld->removeModule(module);

    addRofiWorldNodeTreeToScene(rofiworld->getNodeTree());

    // Previously extruded connectors get their mesh reset to non-extruded
    for (auto &module : neighbor_modules)
        setRoFIMesh(module->getParts());
    
    selected_module = nullptr;
    scene->manageSelection(nullptr);
}

void Editor::connectModule()
{
    if (!is_connecting_modules || !selected_connectors.first || !selected_connectors.second)
        return;

    // Disconnect
    if (rofi::Connector::shares_module_link(selected_connectors.first, selected_connectors.second))
    {
        disconnectModule();
    }
    // Connect
    else if (can_connect_modules = checkConnectModuleValid(selected_connectors.first, selected_connectors.second))
    {
        auto source_node = selected_connectors.first->getNode();
        rebuildRofiWorldNodeTree(rofiworld->getModuleWithNode(source_node), source_node, rofiworld);

        voxel_graph->joinConnectors(selected_connectors.first, selected_connectors.second);

        rofiworld->teleportModulesByConnectors(selected_connectors.first, selected_connectors.second, selected_cardinality);

        setConnectorMesh(selected_connectors.first);
        setConnectorMesh(selected_connectors.second);
    }
    
    if (!can_connect_modules)
    {
        setError("Invalid mutual orientation, cannot connect modules.");
        is_connecting_modules = false;
        return;
    }

    selected_connectors = {nullptr, nullptr};
    selected_modules_connect = {nullptr, nullptr};
    first_connector_highlights = {glm::vec3(0.0f), nullptr};
    second_connector_highlights = {glm::vec3(0.0f), nullptr};
    selected_module = nullptr;
    scene->manageSelection(nullptr);
    scene->manageHighlight(glm::vec3(0.0f), nullptr);
    scene->manageSecondarySelection({});
    scene->manageSecondaryHighlights({});
}

void Editor::disconnectModule()
{
    voxel_graph->disjoinConnectors(selected_connectors.first, selected_connectors.second);
    setConnectorMesh(selected_connectors.first);
    setConnectorMesh(selected_connectors.second);
}

bool Editor::checkConnectModuleValid(const connector_ptr fst_connector, const connector_ptr snd_connector)
{
    auto fst_node = fst_connector->getNode();
    auto snd_node = snd_connector->getNode();

    if (fst_connector->isFull() 
        || snd_connector->isFull())
        return false;

    float epsilon = 0.001f;
    if (rofiworld->reachable(rofiworld->getModuleWithNode(fst_node), rofiworld->getModuleWithNode(snd_node))
        && (!glm::all(glm::epsilonEqual(fst_node->getPositionWorld(), snd_node->getPositionWorld(), epsilon))
            || rofi::Connector::get_mutual_orientation(fst_connector, snd_connector) == rofi::cardinal::Invalid
            || rofi::Connector::get_mutual_orientation(fst_connector, snd_connector) != selected_cardinality))
        return false;

    return true;
}

void Editor::rotateModule(module_ptr selected_module, glm::vec3 &rotation_angles)
{
    if (is_resetting_module_rotation)
    {
        selected_module->getNode()->setRotationQuat(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
        return;
    }
    const auto rotation_axes = coordinate_space_selection == 0 
                         ? std::vector<glm::vec3>{ selected_module->getNode()->getRotationAxisXLocal(), 
                                                   selected_module->getNode()->getRotationAxisYLocal(), 
                                                   selected_module->getNode()->getRotationAxisZLocal() }
                         : std::vector<glm::vec3>{ glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) };

    if (rotation_angles.x != 0.0f)
    {
        auto x_rotation = glm::angleAxis(glm::radians(rotation_angles.x), rotation_axes[0]);
        selected_module->getNode()->rotateRotationQuat(x_rotation);
    }
    if (rotation_angles.y != 0.0f)
    {
        auto y_rotation = glm::angleAxis(glm::radians(rotation_angles.y), rotation_axes[1]);
        selected_module->getNode()->rotateRotationQuat(y_rotation);
    }
    if (rotation_angles.z != 0.0f)
    {
        auto z_rotation = glm::angleAxis(glm::radians(rotation_angles.z), rotation_axes[2]);
        selected_module->getNode()->rotateRotationQuat(z_rotation);
    }
}

void Editor::rotateComponent()
{
    auto selection = scene->getSelection();
    if (!selection || !is_rotating_component)
        return;

    bool is_voxel = isVoxel(selection);
    auto voxel = is_voxel 
                 ? getVoxel(selection)
                 : getConnector(selection)->getConnection().first.lock();
                      
    if (selection != prev_rebuild_node)
    {
        can_rotate_component = rebuildRofiWorldNodeTree(voxel, selection, rofiworld);
        prev_rebuild_node = selection;
    }

    if (!can_rotate_component)
    {
        setError("Cycle detected, rotation not permited.");
        rotation_locked_module = rofiworld->getModuleWithNode(selection);
        return;
    }
    else
    {
        if (rotation_locked_module)
            setRoFIMesh(rotation_locked_module->getParts(), false);
        rotation_locked_module = nullptr;
    }

    if (is_voxel)
        rofiworld->rotateVoxel(voxel, component_rotation_angle);
    else
        rofiworld->rotateConnector(getConnector(selection), component_rotation_angle);
}

void Editor::linkModulesNodes(const std::vector<module_ptr> &modules)
{
    for (const auto &module : modules)
        rofi::VoxelGraph::linkImportVoxelGraphToNodes(module->getParts());
}

void Editor::setModulesNodesHierarchy(const std::vector<module_ptr> &modules)
{
    for (const auto &module : modules)
        rofi::Module::setModuleNodeHierarchy(module->getParts(), module->getType());
}

void Editor::setModulesMesh(const std::vector<module_ptr> &modules)
{
    for (const auto &module : modules)
        setRoFIMesh(module->getParts());
}

void Editor::setRofiWorldNodeTreeToScene(const std::vector<node_ptr> &prev_node_tree, const std::vector<node_ptr> &new_node_tree)
{
    removeRofiWorldNodeTreeFromScene(prev_node_tree);
    addRofiWorldNodeTreeToScene(new_node_tree);
}

void Editor::addRofiWorldNodeTreeToScene(const std::vector<node_ptr> &node_tree)
{
    auto &scene_vector = scene->getScene();
    scene_vector.reserve(scene_vector.size() + node_tree.size());
    scene_vector.insert(scene_vector.end(), node_tree.begin(), node_tree.end());
}

void Editor::removeRofiWorldNodeTreeFromScene(const std::vector<node_ptr> &node_tree)
{
    std::erase_if(scene->getScene(), [&](node_ptr node) 
                                     { return std::find(node_tree.begin(), node_tree.end(), node) != node_tree.end(); });
}

bool Editor::rebuildRofiWorldNodeTree(module_ptr module, node_ptr root_node, rofiworld_ptr rofiworld)
{
    removeRofiWorldNodeTreeFromScene(rofiworld->getNodeTree());

    rofiworld->rebuildNodeTree(module, root_node);

    addRofiWorldNodeTreeToScene(rofiworld->getNodeTree());

    return true;
}

bool Editor::rebuildRofiWorldNodeTree(voxel_ptr voxel, node_ptr root_component_node, rofiworld_ptr rofiworld)
{
    removeRofiWorldNodeTreeFromScene(rofiworld->getNodeTree());

    // This could still happen here on demand, not when initializing
    // auto parent_module = rofiworld->getModuleWithNode(component_node);
    // setModuleNodeHierarchy(parent_module->getParts(), parent_module->getType());

    if (!rofiworld->rebuildNodeTree(voxel, root_component_node))
    {
        addRofiWorldNodeTreeToScene(rofiworld->getNodeTree());
        return false;
    }

    addRofiWorldNodeTreeToScene(rofiworld->getNodeTree());

    return true;
}

/* COLLISION DETECTION */

bool Editor::detectCollision(bool something_changed, rofiworld_ptr rofiworld)
{
    if (!something_changed)
        return false;
    
    std::set<module_ptr> curr_collided_modules;

    auto collision_detected = rofiworld->detectCollision(curr_collided_modules);

    std::set<module_ptr> no_longer_collided_modules;
    std::set_difference(collided_modules.begin(), collided_modules.end(), curr_collided_modules.begin(), curr_collided_modules.end(), 
                        std::inserter(no_longer_collided_modules, no_longer_collided_modules.begin()));

    toggleRoFIInvalidStateMesh(false, no_longer_collided_modules);

    std::set<module_ptr> newly_collided_modules;
    std::set_difference(curr_collided_modules.begin(), curr_collided_modules.end(), collided_modules.begin(), collided_modules.end(),
                        std::inserter(newly_collided_modules, newly_collided_modules.begin()));

    toggleRoFIInvalidStateMesh(true, newly_collided_modules);

    collided_modules = curr_collided_modules;

    rofi_invalid_state = collision_detected;

    return collision_detected;
}

/* ROFIWORLD EXPORT */

void Editor::pickSpaceJoints(rofiworld_ptr rofiworld, module_ptr initial_module)
{
    std::set<module_ptr> found_modules;

    if (initial_module)
        pickSpaceJoint(rofiworld, initial_module, found_modules);

    for (const auto [module, id] : rofiworld->getModulesMap())
    {
        if (found_modules.contains(module))
            continue;

        pickSpaceJoint(rofiworld, module, found_modules);
    }
}

void Editor::pickSpaceJoint(rofiworld_ptr rofiworld, module_ptr module, std::set<module_ptr> &found_modules)
{
    std::set<module_ptr> reachable_modules;

    connector_ptr connector = nullptr;

    if (module->getType() == "universal")
        connector = pickSpaceJointUniversal(module);
    else
        connector = pickSpaceJointGeneric(module);

    reachable_modules = rofiworld->reachableModules(connector);
    selected_space_joints.emplace_back(connector);

    found_modules.insert(reachable_modules.begin(), reachable_modules.end()); 
}

connector_ptr Editor::pickSpaceJointUniversal(const module_ptr module)
{
    connector_ptr connector = nullptr;

    for (const auto &[component, id] : module->getComponentIDs())
    {
        if (id == 0)
        {
            connector = std::dynamic_pointer_cast<rofi::Connector>(component);
            break;
        }
    }

    ASSUMPTION(connector);
    return connector;
}

connector_ptr Editor::pickSpaceJointGeneric(const module_ptr module)
{
    auto module_vg = module->getParts();
    ASSUMPTION(!module_vg->getConnections().empty());

    connector_ptr connector = nullptr;

    for (const auto &[component, id] : module->getComponentIDs())
    {
        if (id == 0)
        {
            connector = std::dynamic_pointer_cast<rofi::Connector>(component);
            break;
        }
    }

    ASSUMPTION(connector);
    return connector;
}


}
