#ifndef ROFIWORLD_INCLUDED
#define ROFIWORLD_INCLUDED

#include <common/node.hpp>
#include <rofi/module.hpp>
#include <rofi/voxel.hpp>
#include <rofi/voxel_graph.hpp>

#include <memory>
#include <set>
#include <vector>

namespace rofi
{

enum class slot { ApX, AnX, AnZ, BpX, BnX, BnZ };

class RofiWorld
{
    std::string name;

    voxel_graph_ptr voxel_graph = std::make_shared<rofi::VoxelGraph>();
    
    /* Modules present in RofiWorld with assigned id */
    std::map<module_ptr, uint64_t> modules_map;
    std::map<node_ptr, module_ptr> nodes_map;
    std::set<module_ptr> modules_set;
    std::set<uint64_t> ids_set;
    std::set<node_ptr> modules_nodes_set;
    /* Tree of Module nodes that allows for correct render and robot motion */
    std::vector<node_ptr> node_tree;

public:

    bool empty() const { return voxel_graph->empty() && node_tree.empty(); }

    const std::string &getName() const { return name; }
    void setName(const std::string &_name) { name = _name; }
    void setName(const std::string &&_name) { name = _name; }

    const std::vector<node_ptr> &getNodeTree() const { return node_tree; }
    voxel_graph_ptr getVoxelGraph() const { return voxel_graph; }

    const std::map<module_ptr, uint64_t> &getModulesMap() const { return modules_map; }
    const std::set<module_ptr> &getModulesSet() const { return modules_set; }
    const uint64_t getModuleID(const module_ptr module) const { return modules_map.at(module); }
    const module_ptr getModuleByID(const uint64_t id) const;
    bool containsID(uint64_t id) const { return ids_set.contains(id); }
    void changeModuleID(module_ptr module, uint64_t new_id);
    /* Returns module with given node that is either directly associated with the node or nearest parent node.
     * If no such module is found, returns nullptr. */
    module_ptr getModuleWithNode(const node_ptr node) const;
    /* Returns an std::vector of modules connected with given module via a ModuleLink */
    std::vector<module_ptr> getNeighborModules(module_ptr module) const;
    /* Returns immediate children nodes of the given module_node */
    std::vector<node_ptr> getDependentModulesNodes(node_ptr module_node) const;

    void addModule(module_ptr module);
    void removeModule(module_ptr module);

    void rotateVoxel(voxel_ptr voxel, float angle);
    void rotateConnector(connector_ptr connector, float angle);
    void teleportModulesByConnectors(const connector_ptr source_connector, const connector_ptr destination_connector, const cardinal mutual_orientation);

    bool reachable(module_ptr fst_module, module_ptr snd_module) const;
    std::set<module_ptr> reachableModules(objectbase_ptr component) const;


    bool rebuildNodeTree(module_ptr module, node_ptr root_node);
    bool rebuildNodeTree(voxel_ptr voxel, node_ptr root_node);

    bool detectCollision(std::set<module_ptr> &collided_modules) const;

private:
    uint64_t findAvailableID(const std::set<uint64_t> &ids_set);

    void disconnectModule(module_ptr module);

    void addNodeToNodeTree(node_ptr node);
    // Removes nodes from node tree with children release to top level of rofiworld_node_tree
    void removeNodeFromNodeTree(const node_ptr node_to_remove);
    void removeNodeFromNodeTreeRec(const node_ptr node_to_remove, std::vector<node_ptr> &node_tree);
    void extendNodeTree(const std::vector<node_ptr> &nodes);

    void traverseConnectedModules(module_ptr module, module_ptr initial_module, connector_ptr initial_shoe, std::set<module_ptr> &found_modules, bool &has_cycle) const;
    void detectCycle(module_ptr next_module, module_ptr initial_module, connector_ptr opposite_connector, connector_ptr initial_shoe, bool &has_cycle) const;

    void destroyNodeTree(const std::set<node_ptr> &module_set_nodes, std::vector<node_ptr> &node_tree);
};

}

using rofiworld_ptr = std::shared_ptr<rofi::RofiWorld>;

#endif