/*****************************************************************************
 * $CAMITK_LICENCE_BEGIN$
 *
 * CamiTK - Computer Assisted Medical Intervention ToolKit
 * (c) 2001-2025 Univ. Grenoble Alpes, CNRS, Grenoble INP - UGA, TIMC, 38000 Grenoble, France
 *
 * 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_TRANSLATIONCONSTRAINT_H
#define SIMULATOR_SIMULATORS_SOFA_TRANSLATIONCONSTRAINT_H


#include "sofa/core/behavior/ProjectiveConstraintSet.h"
#include "sofa/core/behavior/MechanicalState.h"
//#include "sofa/core/VisualModel.h"

#include <vector>
#include <map>

#include <Loads.h>

template<class DataTypes>

/**
 *
 * @ingroup group_cepmodeling_libraries_mml
 *
 * @brief
 * TODO Comment class here.
 */
class TranslationConstraint : public sofa::core::behavior::ProjectiveConstraintSet<DataTypes> { //, public VisualModel
public :
    ///template types
    typedef typename DataTypes::VecCoord VecCoord;
    typedef typename DataTypes::VecDeriv VecDeriv;
    typedef typename DataTypes::VecCoord::iterator VecCoordIterator;
    typedef typename DataTypes::VecDeriv::iterator VecDerivIterator;
    typedef typename DataTypes::Coord Coord;
    typedef typename DataTypes::Deriv Deriv;

    typedef typename DataTypes::MatrixDeriv::RowType MatrixDerivRowType;

    ///constructor
    TranslationConstraint(Loads* loadsList, const std::map<unsigned int, unsigned int>& atomIndexToDOFIndex, sofa::core::behavior::MechanicalState<DataTypes>* mm);

    ~TranslationConstraint() { }

    /// return the targets list
    std::vector<unsigned int> getTargets() {
        return targets;
    }

    ///fix or translate a point
    TranslationConstraint<DataTypes>* addConstraint(unsigned int index, Deriv trans);
    TranslationConstraint<DataTypes>* removeConstraint(int index);

    /// Constraint inherits
    void projectResponse(VecDeriv& dx); ///< project dx to constrained space
    virtual void projectVelocity(VecDeriv& v); ///< project dx to constrained space (dx models a velocity)
    virtual void projectPosition(VecCoord& x); ///< project x to constrained space (x models a position)

    void projectResponse(MatrixDerivRowType& dx) {}

    /// -- VisualModel interface
    void draw();
    void initTextures() { }
    void update() { }

    /// set initial time (context->getTime() remind the same when simulation is rewind)
    void setInitTime(double time);

    //TODO need to define which getclass to choose because of double inherance...
    //sofa::core::objectmodel::BaseClass* getClass() const {return nullptr;}

private:

    /// fix a point on the axe specified (0=x, 1=y, 2=z)
    void fixDOF(int index, int axe);
    /// the mechanical model
    sofa::core::behavior::MechanicalState<DataTypes>* mmodel;
    /// the set of vertex targets
    std::vector<unsigned int> targets;
    /// list of translations
    VecDeriv translations;
    /// list of fixed directions
    VecDeriv directionsNULLs;
    /// initial positions
    VecDeriv initPos;
    /// initial time
    double initTime;
    /// the lml loads
    std::vector<Load*> loads;
    ///link between PML object indexes and sofa Dofs Indexes
    std::map<unsigned int, unsigned int> atomToDOFIndexes;
};

#endif // SIMULATOR_SIMULATORS_SOFA_TRANSLATIONCONSTRAINT_H
