/*****************************************************************************
 * $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$
 ****************************************************************************/


#ifndef SIMULATOR_SIMULATORS_SOFA_SOFASIMULATOR_H
#define SIMULATOR_SIMULATORS_SOFA_SOFASIMULATOR_H

#include "InteractiveSimulator.h"

#include <sofa/simulation/tree/TreeSimulation.h>
#include <sofa/component/container/MechanicalObject.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/simulation/common/Node.h>
#include <sofa/simulation/tree/GNode.h>

// simplification of sofa interface
#define SofaVector sofa::helper::vector

// Loads for Sofa
#include "TranslationConstraint.h"

/// correspondance between an atom index and index of the DOF in a mechanical object
namespace std {
/// definition of a couple (=STL pair) [unsigned int mecObjectIndex, unsigned int dofIndex]
typedef pair<unsigned int, unsigned int> MechanicalObjectDOFIndex;
/** definition of the association set (=map in STL) AtomDOFMap
  * AtomDOFMap associates an atom index with the the MechanicalObjectDofIndex
  */
typedef map <unsigned int, MechanicalObjectDOFIndex> AtomDOFMap;
/** there is one MechanicalObjectAtomDOFMap per Mechanical Object:
  * this is a map where first is the atom index and second is the DOF index for this mechanical object
  */
typedef map<unsigned int, unsigned int> MechanicalObjectAtomDOFMap;
/** there is one MechanicalObjectDOFAtomMap per Mechanical Object:
  * this is a map where first is the DOF index for this mechanical object and second is the atom index
  */
typedef map<unsigned int, unsigned int> MechanicalObjectDOFAtomMap;
}

/**
 *
 * @ingroup group_cepmodeling_libraries_mml
 *
 * @brief
 * TODO Comment class here.
 */
class SofaSimulator : public InteractiveSimulator {

public:
    SofaSimulator(MonitoringManager* monitoringManager);

    SofaSimulator(MonitoringManager* monitoringManager, const char* file);

    virtual ~SofaSimulator();

    ///@name Simulator inherited
    ///@{
    void doMove(double dt);
    void init();
    void getPosition(int index, double position[3]);
    void getForce(int index, double force[3]);
    void end() {}

    void createPml(const char* inputFile, const char* pmlFile);
    ///@}

    std::string getScnFile();

private:
    /// get the sofa graph node root
    sofa::simulation::Node* getGNode();

    /// get the MechanicalObjectAtomDOFMap for a given mechancial object
    std::MechanicalObjectAtomDOFMap& getMechanicalObjectAtomDOFMap(unsigned int mechObjectIndex);

    /// get the MechanicalObjectAtomDOFMap for a given mechancial object
    std::MechanicalObjectDOFAtomMap& getMechanicalObjectDOFAtomMap(unsigned int mechObjectIndex);

    ///Return the Coord std::vector corresponding to the Atom number i
    sofa::defaulttype::Vec3Types::Coord getDOFPosition(unsigned int atomIndex);

    /// get the position of the DOF of index dofIndex in the MechanicalObject of index mechObjectIndex
    sofa::defaulttype::Vec3Types::Coord getDOFPosition(unsigned int mechObjectIndex, unsigned int dofIndex);

    ///Return the force std::vector corresponding to the Atom number i
    sofa::defaulttype::Vec3Types::Deriv getDOFForce(unsigned int atomIndex);

    /// get the force of the DOF of index dofIndex in the MechanicalObject of index mechObjectIndex
    sofa::defaulttype::Vec3Types::Deriv getDOFForce(unsigned int mechObjectIndex, unsigned int dofIndex);

    /// get the atom id corresponding to DOF of index dofIndex in the MechanicalObject of index mechObjectIndex
    unsigned int getAtomIndex(unsigned int mechObjectIndex, unsigned int dofIndex);

    /// get a mechanical object by its index
    sofa::component::container::MechanicalObject<sofa::defaulttype::Vec3Types>* getMechanicalObject(unsigned int mechObjectIndex);

    /// get the number of mechanical objects
    unsigned int getNumberOfMechanicalObjects();

    /// get the positions of all DOF for mechanical object mechObjectIndex
    sofa::defaulttype::Vec3Types::VecCoord getMechanicalObjectDOFPosition(unsigned int mechObjectIndex);

    /// get the forces of all DOF for mechanical object mechObjectIndex
    sofa::defaulttype::Vec3Types::VecDeriv getMechanicalObjectDOFForce(unsigned int mechObjectIndex);

    /// build structure
    void build();

    /// translate loads into constraints
    void buildConstraints();

    /// the sofa graph node root
#if defined(SOFA_1_0_RC1) || defined(SOFA_SVN) || defined(SOFA_STABLE)
    sofa::simulation::Node::SPtr groot;
#endif

#ifdef MML_SOFA_1_0_BETA4
    sofa::simulation::Node* groot;
#endif

    /// all the mechanical objects
    SofaVector<BaseMechanicalState*> mechanicalObjects;

    ///create a correspondance between the atoms and the DOFs (indexAtom<->indexMechObject[indexDOF])
    std::AtomDOFMap atomsToDOF;

    /// list of all MechanicalObjectAtomDOFMap
    std::vector<std::MechanicalObjectAtomDOFMap*> mechanicalObjectAtomDOFMap;

    /// list of all MechanicalObjectDOFAtomMap
    std::vector<std::MechanicalObjectDOFAtomMap*> mechanicalObjectDOFAtomMap;

    /// the load constraints (i.e. Translation) for each mechanical Objects
    std::vector<TranslationConstraint<sofa::defaulttype::Vec3Types> *> translations;

    /// path to .scn file
    std::string scnFile;
};

// -------------------- getGNode --------------------
inline sofa::simulation::Node* SofaSimulator::getGNode() {
    if (groot == NULL) {
        //-- TODO build a sofaGRoot using the nodes
    }

#if defined(SOFA_1_0_RC1) || defined(SOFA_SVN) || defined(SOFA_STABLE)
    return groot.get();
#endif

#ifdef MML_SOFA_1_0_BETA4
    return groot;
#endif
}

// -------------------- getNumberOfMechanicalObjects --------------------
inline unsigned int SofaSimulator::getNumberOfMechanicalObjects() {
    return mechanicalObjects.size();
}

// -------------------- getMechanicalObjectAtomDOFMap --------------------
inline std::MechanicalObjectAtomDOFMap& SofaSimulator::getMechanicalObjectAtomDOFMap(unsigned int mechObjectIndex) {
    return (*mechanicalObjectAtomDOFMap[mechObjectIndex]);
}

// -------------------- getMechanicalObjectDOFAtomMap --------------------
inline std::MechanicalObjectDOFAtomMap& SofaSimulator::getMechanicalObjectDOFAtomMap(unsigned int mechObjectIndex) {
    return (*mechanicalObjectDOFAtomMap[mechObjectIndex]);
}

// -------------------- getMechanicalObjectDOFPosition --------------------
inline sofa::defaulttype::Vec3Types::VecCoord SofaSimulator::getMechanicalObjectDOFPosition(unsigned int mechObjectIndex) {
    return (*getMechanicalObject(mechObjectIndex)->getX());
}

// -------------------- getMechanicalObjectDOFForce --------------------
inline sofa::defaulttype::Vec3Types::VecDeriv SofaSimulator::getMechanicalObjectDOFForce(unsigned int mechObjectIndex) {
    return (*getMechanicalObject(mechObjectIndex)->getF());
}

// -------------------- getScnFile --------------------
inline std::string SofaSimulator::getScnFile() {
    return scnFile;
}

#endif // SIMULATOR_SIMULATORS_SOFA_SOFASIMULATOR_H