/*****************************************************************************
 * $CAMITK_LICENCE_BEGIN$
 *
 * CamiTK - Computer Assisted Medical Intervention ToolKit
 * (c) 2001-2018 Univ. Grenoble Alpes, CNRS, TIMC-IMAG UMR 5525 (GMCAO)
 *
 * Visit http://camitk.imag.fr for more information
 *
 * This file is part of CamiTK.
 *
 * CamiTK is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * CamiTK is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with CamiTK.  If not, see <http://www.gnu.org/licenses/>.
 *
 * $CAMITK_LICENCE_END$
 ****************************************************************************/

#include "SofaSimulator.h"

#include "SimulatorFactory.h"
#include "SurfaceExtractor.h"

#include <sofa/simulation/common/xml/initXml.h>
#include <sofa/component/init.h>
#include <sofa/helper/Utils.h>
#include <sofa/helper/system/PluginManager.h>

#ifdef SOFA_1_0_RC1
#include <sofa/simulation/common/xml/initXml.h>
#include <sofa/simulation/common/Node.h>
#include <sofa/simulation/tree/TreeSimulation.h>
#include <sofa/component/init.h>
#include <sofa/helper/system/PluginManager.h>
#include <sofa/helper/system/SetDirectory.h>
#endif

#include <string>
#include <iostream>
#include <fstream>
#include <set>
#include <QFileInfo>
#include <QDir>

//-- PML
#include <pml/Atom.h>
#include <pml/StructuralComponent.h>
#include <pml/MultiComponent.h>

#ifdef MML_GENERATE_GUI
#include "SofaWidget.h"
#endif

using namespace std;

using namespace sofa::core::behavior; // ::BaseMechanicalState
using namespace sofa::component::container;
using namespace sofa::core::objectmodel; // ::BaseContext::SearchDown
using namespace sofa::core;
using namespace sofa::component;
using namespace sofa::simulation::tree;
using namespace sofa::simulation;
using namespace sofa::defaulttype;

MML_DECL_CLASS(SofaSimulator)

bool sofaRegistered = SimulatorFactory::getInstance()->registerClass<SofaSimulator>("sofa", true);

SofaSimulator::SofaSimulator(MonitoringManager* monitoringManager): InteractiveSimulator(monitoringManager) {

    // check for Sofa scn
    QFileInfo mmlFileInfo = QFileInfo(monitoringManager->getMmlFileName().c_str());
    QString scnFileName = mmlFileInfo.absolutePath() + "/" + mmlFileInfo.baseName() + ".scn";
    scnFile = scnFileName.toStdString();

    // if scn file already exist, use this file
    ifstream iscn(scnFile.c_str());

    if (!iscn) {
        // scn file do not exist in the same directory, create one in temp directory
        scnFileName = QDir::tempPath() + "/" + mmlFileInfo.baseName() + ".scn";
        scnFile = scnFileName.toStdString();

        // create msh from pmlFile in temporary directory as well
        QString mshFileName = QDir::tempPath() + "/" + QFileInfo(monitoringManager->getPmlFileName().c_str()).baseName() + ".msh";
        int type = 0;
        PhysicalModel* pml = monitoringManager->getPml();
        ofstream msh;
        msh.open(mshFileName.toStdString().c_str(), ios::out);

        //create nodes
        msh << "$NOD" << endl;
        msh << pml->getNumberOfAtoms()  << endl;
        double pos[3];
        StructuralComponent* atoms = pml->getAtoms();

        for (unsigned int i = 0; i < atoms->getNumberOfStructures(); i++) {
            Atom* a = dynamic_cast<Atom*>(atoms->getStructure(i));
            a->getPosition(pos);
            msh << a->getIndex() + 1 << " " << pos[0] << " " << pos[1] << " " << pos[2] << endl;
        }

        msh << "$ENDNOD" << endl;

        //create elements if an "Elements" components exists
        Component* elements = pml->getComponentByName("Elements");

        if (elements) {
            msh << "$ELM" << endl;
            int nbElements = elements->getNumberOfCells();
            msh << nbElements << endl;

            for (int i = 0; i < nbElements; i++) {

                msh << i + 1 << " ";

                // get the cell
                Cell* cell = elements->getCell(i);

                switch (cell->getType()) {

                    case StructureProperties::HEXAHEDRON:

                        msh << "5 1 1 8";
                        type = 5;
                        break;

                    case StructureProperties::TETRAHEDRON:

                        msh << "4 1 1 4";
                        type = 4;
                        break;

                    case StructureProperties::QUAD:

                        msh << "3 1 1 4";
                        break;

                    case StructureProperties::TRIANGLE:

                        msh << "2 1 1 3";
                        break;


                    default:
                        std::cerr << "export to Sofa : unknown type for cell " << cell->getIndex() + 1 << ", neither HEXAHEDRON, TRIANGLE, TETRAHEDRON nor QUAD" << std::endl;
                        continue;
                }

                for (unsigned int k = 0; k < cell->getNumberOfStructures(); k++) {
                    msh << " " << cell->getStructure(k)->getIndex() + 1;
                }

                msh << endl;

            }

            msh << "$ENDELM" << endl;
        }
        else {
            std::cerr << " no \"Elements\" component found to export Elements in msh format" << std::endl;
        }

        msh.close();

        //create a simple scn
        // TODO convert LML in sofa constraint
        // to choose a possible modeling techniques we assume that only one type of elements is used
        ofstream scn;
        scn.open(scnFile.c_str(), ios::out);
#if defined(SOFA_1_0_RC1) || defined(SOFA_SVN) || defined(SOFA_STABLE)
        scn << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << endl;
        scn << "<!-- Created from " << monitoringManager->getPmlFileName() << " by CamiTK/mml -->" << endl;
        scn << "<Node name=\"Root\" gravity=\"0 0 0\" dt=\"" << monitoringManager->getDt() << "\" >" << endl;
        scn << "    <VisualStyle displayFlags=\"showVisualModels showBehaviorModels showCollisionModels showBoundingCollisionModels showMappings showMechanicalMappings showForceFields showInteractionForceFields\"/>" << endl;
        scn << "    <CollisionPipeline depth=\"6\" verbose=\"0\" draw=\"0\" />" << endl;
        scn << "    <BruteForceDetection name=\"N2\" />" << endl;
        scn << "    <MinProximityIntersection name=\"Proximity\" alarmDistance=\"0.5\" contactDistance=\"0.3\" />" << endl;
        scn << "    <CollisionResponse name=\"Response\" response=\"default\" />" << endl;
        scn << "    <CollisionGroup name=\"Group\" />" << endl;
        scn << "    <!-- Physics -->" << endl;
        scn << "    <Node name=\"MML\" gravity=\"0 0 0\">" << endl;
        scn << "        <!-- solver -->" << endl;
        scn << "        <EulerImplicit name=\"cg_odesolver\"  printLog=\"false\" />" << endl;
        scn << "        <CGLinearSolver iterations=\"25\" name=\"linear solver\" tolerance=\"1.0e-9\" threshold=\"1.0e-9\" />" << endl;
        scn << "        <!-- mesh -->" << endl;
        scn << "        <MeshGmshLoader name=\"meshLoader\" filename=\"" << mshFileName.toStdString() << "\" />" << endl;
        scn << "        <Mesh src=\"@meshLoader\" />" << endl;
        scn << "        <!-- mechanical object -->" << endl;
        scn << "        <MechanicalObject src=\"@meshLoader\" />" << endl;
        scn << "        <DiagonalMass massDensity=\"0.1\" />" << endl;
        scn << "        <!-- force field -->" << endl;
        switch (type) {
            case 5:
                //hexa
                scn << "       <HexahedronFEMForceField name=\"FEM\" method=\"large\" poissonRatio=\"0.45\" youngModulus=\"1440\" />" << endl;
                break;
            case 4:
                //tetra
                scn << "       <TetrahedralCorotationalFEMForceField name=\"FEM\" youngModulus=\"1440\" poissonRatio=\"0.45\" method=\"large\" />" << endl;
                break;
            default:
                ;
        }
        scn << "    </Node>" << endl;
        scn << "</Node>" << endl;
#endif

#ifdef MML_SOFA_1_0_BETA4
        scn << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << endl;
        scn << "<Node name=\"root\" gravity=\"0 0 0\" dt=\"" << monitoringManager->getDt() << "\" showBehaviorModels=\"0\" showCollisionModels=\"0\" showMappings=\"0\" showForceFields=\"0\">" << endl;
        scn << "  <Node name=\"MML\" gravity=\"0 0 0\" depend=\"topo dofs\">" << endl;
        scn << "    <!-- solver -->" << endl;
        scn << "    <EulerImplicitSolver name=\"cg_odesolver\"  printLog=\"0\" />" << endl;
        scn << "    <CGLinearSolver template=\"GraphScattered\" name=\"linear solver\"  iterations=\"25\"  tolerance=\"1e-09\" threshold=\"1e-09\" />" << endl;
        scn << "    <MeshLoader name=\"meshLoader\"  filename=\"" << mshFile << "\" />" << endl;
        scn << "    <Object type=\"Mesh\" />" << endl;
        scn << "    <MechanicalObject template=\"Vec3d\" name=\"dofs\"  position=\"0 0 0\"  velocity=\"0 0 0\"  force=\"0 0 0\"  derivX=\"0 0 0\"  free_position=\"0 0 0\"  free_velocity=\"0 0 0\"  restScale=\"1\" />" << endl;
        scn << "    <DiagonalMass template=\"Vec3d\" name=\"computed using mass density\"  massDensity=\"1\" />" << endl;
        scn << "    <!--modeling technique -->" << endl;

        switch (type) {
            case 5:
                //hexa
                scn << "    <HexahedronFEMForceField template=\"Vec3d\" name=\"FEM\"  method=\"large\"  poissonRatio=\"0.45\"  youngModulus=\"1440\" />" << endl;
                break;
            case 4:
                //tetra
                scn << "    <TetrahedronSetTopologyContainer name=\"topo\" />" << endl;
                scn << "    <TetrahedronSetGeometryAlgorithms template=\"Vec3d\" name=\"GeomAlgo\" />" << endl;
                scn << "    <TetrahedralCorotationalFEMForceField template=\"Vec3d\" name=\"FEM\"  method=\"large\"  poissonRatio=\"0.45\"  youngModulus=\"1440\" />" << endl;
                break;
            default:
                ;
        }

        scn << "  </Node>" << endl;
        scn << "</Node>" << endl;
#endif
        scn.close();

    }

    build();

    // create the atom / DOF map
    unsigned int id = 0; // TODO: Warning! the actual PML should be used to make
    // perfect correspondance with the DOF index (for e.g. using two matching positions)

    StructuralComponent* sc = monitoringManager->getPml()->getAtoms();

    for (unsigned int i = 0; i < mechanicalObjects.size(); i++) {
        mechanicalObjectAtomDOFMap.push_back(new std::MechanicalObjectAtomDOFMap());
        mechanicalObjectDOFAtomMap.push_back(new std::MechanicalObjectDOFAtomMap());

        for (unsigned int j = 0; j < getMechanicalObjectDOFPosition(i).size(); j++) {
            // insert in the global map
            unsigned int atomId = ((Atom*)(sc->getStructure(id)))->getIndex();
            atomsToDOF.insert(std::pair<unsigned int, std::MechanicalObjectDOFIndex> (atomId, std::MechanicalObjectDOFIndex(i, j)));
            // insert in the local map
            mechanicalObjectAtomDOFMap.back()->insert(std::pair<unsigned int, unsigned int>(atomId, j));
            mechanicalObjectDOFAtomMap.back()->insert(std::pair<unsigned int, unsigned int>(j, atomId));
            id++;
        }
    }

}

// -------------------- constructor --------------------
SofaSimulator::SofaSimulator(MonitoringManager* monitoringManager, const char* file): InteractiveSimulator(monitoringManager, file) {
    scnFile = string(file);

    build();
}

// -------------------- destructor --------------------
SofaSimulator::~SofaSimulator() {
    if (getGNode() != NULL) {
        sofa::simulation::getSimulation()->unload(getGNode());
    }

    groot = NULL;
}


// -------------------- build --------------------
void SofaSimulator::build() {

#if defined(SOFA_1_0_RC1) || defined(SOFA_SVN) || defined(SOFA_STABLE)
    // retrieve plugins
    sofa::component::init();
    sofa::simulation::xml::initXml();

    // mimicking sofa set/get path methods using local variable names
    const std::string sofaPathPrefix = string(SOFA_BUILD_DIR);
#ifdef WIN32
    const std::string pluginDir = sofaPathPrefix;
#else
    const std::string pluginDir = sofaPathPrefix + "/lib";
#endif
    sofa::helper::system::PluginRepository.addFirstPath(pluginDir);

    // Initialise paths
    const std::string configDirectoryPath = sofaPathPrefix + "/config";
    const std::string  screenshotDirectoryPath = sofaPathPrefix + "/screenshots";

    // manage plugins from plugin file loadedPlugins.ini
    // This file can be found in the build path.
    // Beware that this file can only be setup/initialized/populated by runSofa application (in Edit -> Plugin Manager menu)
    // or sofa modeler.
    // Simulate the initialization of m_pluginMap by a Sofa GUI application
    // Next line is extracted from SofaPluginManager::loadPluginsFromIniFile()
    std::string path = configDirectoryPath + "/loadedPlugins.ini";
    sofa::helper::system::PluginManager::getInstance().readFromIniFile(path);

    sofa::helper::system::PluginManager::getInstance().init();

    // -- from GUIManager::Init
    // double init!
    sofa::component::init();
    sofa::simulation::xml::initXml();
    // from SOFA_SOURCE_DIR find the directory to share and examples files (do not use the etc/sofa.ini file
    sofa::helper::system::DataRepository.addFirstPath(string(SOFA_SOURCE_DIR) + "/share");
    sofa::helper::system::DataRepository.addFirstPath(string(SOFA_SOURCE_DIR) + "/examples");

    // look for the scn file
    QFileInfo scnFileInfo(scnFile.c_str());
    /*TODO CLEANUP
    std::string fileName = sofa::helper::system::DataRepository.getFile(scnFileInfo.absoluteDir().canonicalPath().toStdString());
    */
    sofa::helper::system::DataRepository.addFirstPath(scnFileInfo.absoluteDir().canonicalPath().toStdString());

    /* TODO CLEANUP    std::cout << "DataRepository is now: " << std::endl;
        const std::vector< std::string > paths = sofa::helper::system::DataRepository.getPaths();
        std::copy( &paths[0], &paths[paths.size()], ostream_iterator<string>(cout, " "));
    */
#endif

#ifdef MML_SOFA_1_0_BETA4
    sofa::simulation::setSimulation(new sofa::simulation::tree::TreeSimulation());

    sofa::component::init();
    sofa::simulation::xml::initXml();
    sofa::helper::system::PluginManager::getInstance().init();
    sofa::helper::system::PluginManager::getInstance().initRecentlyOpened();
#endif

    //-- load the SOFA scn
#if defined(SOFA_1_0_RC1) || defined(SOFA_SVN) || defined(SOFA_STABLE)
    // strong hypothesis: we don't use SOFA DAG, but sofa tree architecture
    //groot = sofa::core::objectmodel::SPtr_dynamic_cast<sofa::simulation::Node>( sofa::simulation::tree::getSimulation()->load( fileName.c_str() ) );
    groot = sofa::core::objectmodel::SPtr_dynamic_cast<sofa::simulation::Node>(sofa::simulation::tree::getSimulation()->load(scnFile.c_str()));
#endif

#ifdef MML_SOFA_1_0_BETA4
    groot = dynamic_cast<sofa::simulation::Node*>(sofa::simulation::getSimulation()->load(scnFile.c_str()));
#endif

    if (getGNode() == NULL) {
#if defined(SOFA_1_0_RC1) || defined(SOFA_SVN) || defined(SOFA_STABLE)
        groot = sofa::simulation::tree::getSimulation()->createNewGraph("");
#endif

#ifdef MML_SOFA_1_0_BETA4
        groot = sofa::simulation::getSimulation()->newNode("");
#endif
        cerr << "SofaSimulator::build: error: cannot load " << scnFile << "." << endl;
    }

    // init root
    sofa::simulation::tree::getSimulation()->init(getGNode());

    // calling sofa::simulation::tree::getSimulation()->reset ( getGNode() ) is not needed as
    // it is done in runSofa main (sse RealGUI::setScene -> RealGUI::resetScene )
    // as it will be done automatically when the sofa simulator will be initialized
    // (see SofaSimulator::init)

    // sofa scn dt prevails
    monitoringManager->setDt(getGNode()->getDt());

#ifdef MML_GENERATE_GUI
    widget = new SofaWidget(NULL, this);
#endif

    // get all mechanical objects
    getGNode()->get<BaseMechanicalState>(&mechanicalObjects, BaseContext::SearchDown);

    // if there is no mechanical objects
    if (mechanicalObjects.size() == 0) {
        if (getGNode() != NULL) {
            sofa::simulation::getSimulation()->unload(getGNode());
        }

        groot = NULL;
        cerr << "SofaSimulator::build: error: cannot find any MechanicalObject in the scene " << endl;
    }


}


//--------------- getDOFPosition -------------------
Vec3Types::Coord SofaSimulator::getDOFPosition(unsigned int atomIndex) {
    std::AtomDOFMap::iterator it = atomsToDOF.find(atomIndex);
    return getDOFPosition((it->second).first, (it->second).second);
}

Vec3Types::Coord SofaSimulator::getDOFPosition(unsigned int mechObjectIndex, unsigned int dofIndex) {
    return getMechanicalObjectDOFPosition(mechObjectIndex)[dofIndex];
}

//--------------- getDOFForce -------------------
Vec3Types::Deriv SofaSimulator::getDOFForce(unsigned int atomIndex) {
    std::AtomDOFMap::iterator it = atomsToDOF.find(atomIndex);
    return getDOFForce((it->second).first, (it->second).second);
}

Vec3Types::Deriv SofaSimulator::getDOFForce(unsigned int mechObjectIndex, unsigned int dofIndex) {
    return getMechanicalObjectDOFForce(mechObjectIndex)[dofIndex];
}

//--------------- getAtomIndex -------------------
unsigned int SofaSimulator::getAtomIndex(unsigned int mechObjectIndex, unsigned int dofIndex) {
    std::MechanicalObjectDOFAtomMap::iterator it = getMechanicalObjectDOFAtomMap(mechObjectIndex).find(dofIndex);
    return it->second;
}


// -------------------- getMechanicalObject --------------------
MechanicalObject<Vec3Types>* SofaSimulator::getMechanicalObject(unsigned int mechObjectIndex) {
    return dynamic_cast<MechanicalObject<Vec3Types> *>(mechanicalObjects[mechObjectIndex]);
}

//--------------- doMove -------------------
void SofaSimulator::doMove(double dt) {
    // if their is no graph root, nothing can be done
    if (!getGNode()) {
        return;
    }

    // set the dt
    getGNode()->getContext()->setDt(dt);

    //-- animate one step
#ifdef SOFA_STABLE
    // Warning animate method does not need any dt. 0.0 is always at least used in Sofa v15_03
    sofa::simulation::tree::getSimulation()->animate(getGNode(), /*dt*/0.0);
#else
    sofa::simulation::tree::getSimulation()->animate(getGNode(), dt);
#endif
}

// --------------- getPosition -------------------
void SofaSimulator::getPosition(int index, double position[3]) {
    Vec3Types::Coord pos;
    std::AtomDOFMap::iterator it = atomsToDOF.find(index);

    if (it != atomsToDOF.end()) {
        pos = getMechanicalObjectDOFPosition((it->second).first)[(it->second).second];
        position[0] = pos[0];
        position[1] = pos[1];
        position[2] = pos[2];
    }
}

// --------------- getForce -------------------
void SofaSimulator::getForce(int index, double force[3]) {
    Vec3Types::Deriv fo;
    std::AtomDOFMap::iterator it = atomsToDOF.find(index);

    if (it != atomsToDOF.end()) {
        fo = getMechanicalObjectDOFForce((it->second).first)[(it->second).second];
        force[0] = fo[0];
        force[1] = fo[1];
        force[2] = fo[2];
    }
}


//--------------- init -------------------
void SofaSimulator::init() {
    sofa::simulation::tree::getSimulation()->reset(getGNode());

    if (monitoringManager->isLmlPresent()) {
        buildConstraints();    //TODO just update?
    }
}

//--------------- buildConstraints -------------------
void SofaSimulator::buildConstraints() {
    // destroy constraints if exist
    if (translations.size() > 0) {
        for (unsigned int i = 0; i < translations.size(); i++) {
            // check all MO (TODO : a LMLConstraint should have a ptr to its MO and should be able to clear itself from its context when deleted)
            for (unsigned int j = 0; j < getNumberOfMechanicalObjects(); j++) {
                if (getMechanicalObject(j)) {
                    getMechanicalObject(j)->getContext()->removeObject(translations[i]);
                }
            }

#if !defined(SOFA_1_0_RC1) && !defined(SOFA_SVN) && !defined(SOFA_STABLE)
            delete translations[i];
#endif
        }

        translations.clear();
    }

    // create the constraints
    if (translations.size() == 0 && monitoringManager->getLml()) {
        for (unsigned int i = 0; i < getNumberOfMechanicalObjects(); i++) {
            TranslationConstraint<Vec3Types>* t = new TranslationConstraint<Vec3Types>(monitoringManager->getLml(), getMechanicalObjectAtomDOFMap(i), getMechanicalObject(i));

            if (t->getTargets().size() > 0) {
                if (getMechanicalObject(i)) {
                    getMechanicalObject(i)->getContext()->addObject(t);
                    t->setInitTime(getGNode()->getTime());
                    translations.push_back(t);
                }
            }
            else {
                delete t;
            }
        }
    }
}

// -------------------- createPml --------------------
void SofaSimulator::createPml(const char* inputFile, const char* pmlFile) {   //TODO enlever input...
    std::string pmlName;

    PhysicalModel* pm;

    // if there is no mechanical objects
    if (mechanicalObjects.size() == 0) {
        if (getGNode() != NULL) {
            sofa::simulation::getSimulation()->unload(getGNode());
        }

        groot = NULL;
        cerr << "SofaSimulator: error: cannot find any MechanicalObject in the scene " << endl;
    }

    std::string filename(inputFile);
    unsigned found = filename.find_last_of("/\\");
    // path: filename.substr(0,found)
    // file: filename.substr(found+1)
    std::string basename = filename.substr(found + 1);   // name without the path
    found = basename.find_last_of(".");
    pmlName = string(basename.substr(0, found));  // name without the path nor the extension

    //-- create the physical model
    pm = new PhysicalModel();
    pm->setName(pmlName);

    // instantiate the structural component
    StructuralComponent* atoms = new StructuralComponent(NULL, "DOF");

    // create the atom structures and create local and global maps between atoms and dof
    for (unsigned int i = 0; i < mechanicalObjects.size(); i++) {
        mechanicalObjectAtomDOFMap.push_back(new std::MechanicalObjectAtomDOFMap());
        mechanicalObjectDOFAtomMap.push_back(new std::MechanicalObjectDOFAtomMap());

        if (getMechanicalObject(i)) {
            for (unsigned int j = 0; j < getMechanicalObjectDOFPosition(i).size(); j++) {
                // get the DOF coord
                double pos[3];
                pos[0] = getMechanicalObjectDOFPosition(i)[j][0];
                pos[1] = getMechanicalObjectDOFPosition(i)[j][1];
                pos[2] = getMechanicalObjectDOFPosition(i)[j][2];
                // create the atom
                Atom* a = new Atom(NULL, pos);
                atoms->addStructure(a, false);
                // insert in the global map
                atomsToDOF.insert(std::pair<unsigned int, std::MechanicalObjectDOFIndex> (a->getIndex(), std::MechanicalObjectDOFIndex(i, j)));
                // insert in the local map
                mechanicalObjectAtomDOFMap.back()->insert(std::pair<unsigned int, unsigned int>(a->getIndex(), j));
                mechanicalObjectDOFAtomMap.back()->insert(std::pair<unsigned int, unsigned int>(j, a->getIndex()));
            }
        }
    }

    pm->setAtoms(atoms);

    // creation of informative component
    MultiComponent* inf = new MultiComponent(NULL, "Informative Components");
    // sub informative component with surface cells
    MultiComponent* scells = new MultiComponent(NULL, "surface cells");
    // sub informative component with surface points
    MultiComponent* spoints = new MultiComponent(NULL, "surface points");
    inf->addSubComponent(scells);
    inf->addSubComponent(spoints);

    // ------------- creation of exclusive component (mech objs, hexa, tetras, triangles, quads) -------------
    MultiComponent* exclusiveComponent = new MultiComponent(NULL, "Exclusive Components");
    exclusiveComponent->setExclusive(true);

    // one component per mechanical object
    MultiComponent* mechObjectMC = new MultiComponent(NULL, "Mechanical Objects");
    mechObjectMC->setExclusive(true);
    // atoms representation
    std::vector<StructuralComponent*> moSC;

    for (unsigned int i = 0; i < mechanicalObjects.size(); i++) {
        StructuralComponent* sc = new StructuralComponent(NULL, static_cast<sofa::simulation::Node*>(mechanicalObjects[i]->getContext())->getName());
        moSC.push_back(sc);
        mechObjectMC->addSubComponent(sc);
    }

    for (std::AtomDOFMap::iterator it = atomsToDOF.begin(); it != atomsToDOF.end(); it++) {
        moSC[(it->second).first]->addStructureIfNotIn(pm->getAtom(it->first));
    }

    exclusiveComponent->addSubComponent(mechObjectMC);

    //--------- a multi component named "Elements" with all tetra, hexa, triangles and quads ---------
    MultiComponent* elements = new MultiComponent(NULL, "Elements");

    //one component for hexas or quads (if exist) for each component
    //one component for tetras or triangles (if exist) for each component
    for (unsigned int i = 0; i < mechanicalObjects.size(); i++) {
        if (getMechanicalObject(i) && /*case mecaObj without topology*/mechanicalObjects[i]->getContext()->getMeshTopology()) {     // some are null (e.g. olg model!)
            //external surface
            SurfaceExtractor extract = SurfaceExtractor(pm);
            extract.init();
            StructuralComponent* scell = new StructuralComponent(NULL, string("surfaceCells-") + mechanicalObjects[i]->getContext()->getName());
            StructuralComponent* spoint = new StructuralComponent(NULL, string("surfacePoints-") + mechanicalObjects[i]->getContext()->getName());
            //hexas
            StructuralComponent* hexas = new StructuralComponent(NULL, string("Hexas-") + mechanicalObjects[i]->getContext()->getName());
            hexas->setMode(RenderingMode::WIREFRAME_AND_SURFACE);

            for (int j = 0; j < mechanicalObjects[i]->getContext()->getMeshTopology()->getNbHexahedra(); j++) {
                Cell* c = new Cell(NULL, StructureProperties::HEXAHEDRON);

                for (unsigned int k = 0; k < 8; k++) {
                    c->addStructure(pm->getAtom(getAtomIndex(i, mechanicalObjects[i]->getContext()->getMeshTopology()->getHexahedron(j)[k])));
                }

                // addstructure does not add NULL atoms
                if (c->getNumberOfStructures() == 8) {
                    hexas->addStructure(c);
                    extract.AddCell(c);
                }
                else {
                    delete c;
                }
            }

            elements->addSubComponent(hexas);

            //quads
            //if we have hexas we do not need quads
            if (hexas->getNumberOfCells() == 0) {
                StructuralComponent* quads = new StructuralComponent(NULL, string("Quad-") + mechanicalObjects[i]->getContext()->getName());
                quads->setMode(RenderingMode::WIREFRAME_AND_SURFACE);

                for (int j = 0; j < mechanicalObjects[i]->getContext()->getMeshTopology()->getNbQuads(); j++) {
                    Cell* c = new Cell(NULL, StructureProperties::QUAD);

                    for (unsigned int k = 0; k < 4; k++) {
                        c->addStructure(pm->getAtom(getAtomIndex(i, mechanicalObjects[i]->getContext()->getMeshTopology()->getQuad(j)[k])));
                    }

                    // addstructure does not add NULL atoms
                    if (c->getNumberOfStructures() == 4) {
                        quads->addStructure(c);
                        extract.AddCell(c);
                    }
                    else {
                        delete c;
                    }
                }

                elements->addSubComponent(quads);
            }


            //tetras
            StructuralComponent* tetras = new StructuralComponent(NULL, string("Tetras-") + mechanicalObjects[i]->getContext()->getName());
            tetras->setMode(RenderingMode::WIREFRAME_AND_SURFACE);

            for (int j = 0; j < mechanicalObjects[i]->getContext()->getMeshTopology()->getNbTetrahedra(); j++) {
                Cell* c = new Cell(NULL, StructureProperties::TETRAHEDRON);

                for (unsigned int k = 0; k < 4; k++) {
                    c->addStructure(pm->getAtom(getAtomIndex(i, mechanicalObjects[i]->getContext()->getMeshTopology()->getTetrahedron(j)[k])));
                }

                // addstructure does not add NULL atoms
                if (c->getNumberOfStructures() == 4) {
                    tetras->addStructure(c);
                    extract.AddCell(c);
                }
                else {
                    delete c;
                }
            }

            elements->addSubComponent(tetras);

            //triangles
            //if we have tetras we do not need triangles
            if (tetras->getNumberOfCells() == 0) {
                StructuralComponent* triangles = new StructuralComponent(NULL, string("Triangles-") + mechanicalObjects[i]->getContext()->getName());
                triangles->setMode(RenderingMode::WIREFRAME_AND_SURFACE);

                for (int j = 0; j < mechanicalObjects[i]->getContext()->getMeshTopology()->getNbTriangles(); j++) {
                    Cell* c = new Cell(NULL, StructureProperties::TRIANGLE);

                    for (unsigned int k = 0; k < 3; k++) {
                        c->addStructure(pm->getAtom(getAtomIndex(i, mechanicalObjects[i]->getContext()->getMeshTopology()->getTriangle(j)[k])));
                    }

                    // addstructure does not add NULL atoms
                    if (c->getNumberOfStructures() == 3) {
                        triangles->addStructure(c);
                        extract.AddCell(c);
                    }
                    else {
                        delete c;
                    }
                }

                elements->addSubComponent(triangles);
            }

            extract.generateExternalSurface(*scell, *spoint);
            scells->addSubComponent(scell);
            spoints->addSubComponent(spoint);
        }
    }

    exclusiveComponent->addSubComponent(elements);
    //--------- /elements ---------

    pm->setExclusiveComponents(exclusiveComponent);
    pm->setInformativeComponents(inf);
    //------------- /exclusive component-------------

    ofstream finalPML(pmlFile);
    pm->xmlPrint(finalPML, false);

    delete pm;//TODO do not write pml and pass the pointer to monitoring manager

}