/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library 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 *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_MAPPING_BaseProjectionToMeshEngine_INL
#define SOFA_COMPONENT_MAPPING_BaseProjectionToMeshEngine_INL

#include "ProjectionToMeshEngine.h"
#include <sofa/core/visual/VisualParams.h>
#include <sofa/helper/AdvancedTimer.h>

namespace sofa
{

namespace component
{

namespace engine
{



template <class T>
BaseProjectionToMeshEngine< T >::BaseProjectionToMeshEngine()
    : Inherit1()
    , d_source(initData(&d_source,"sourcePosition","source dofs to project"))
    , d_target(initData(&d_target,"targetPosition","mesh dofs"))
    , f_indexPairs (initData(&f_indexPairs, "indexPairs", "input list of closest point pairs (parent1 index, parent2 index)"))
    , d_useRestLength( initData (&d_useRestLength, false, "useRestLength", "should a rest length be substracted to effective distances?") )
    , d_restLength( initData (&d_restLength, "restLength", "Rest length of the connections (use initial lengths if not set)") )
    , o_closests(initData(&o_closests,"output","closest primitives with weights, normal, distance"))
    , d_updateOnlyWhenAnimating( initData(&d_updateOnlyWhenAnimating,"updateOnlyWhenAnimating","updateOnlyWhenAnimating"))
{
    this->addAlias(&d_target, "position");
    this->addAlias(&o_closests, "closests");
}




template <class T>
void BaseProjectionToMeshEngine< T >::init()
{
    addInput(&d_source);
    addInput(&d_target);
    addInput(&f_indexPairs);
    addInput(&d_useRestLength);
    addInput(&d_restLength);

    addOutput(&o_closests);

    reinit();
    Inherit1::init();
}


template <class T>
void BaseProjectionToMeshEngine< T >::reinit()
{
    initialDistances.clear();

    projectToMeshInit();

    Inherit1::reinit();

    o_closests.setDirtyValue();
//    update();
}



template <class T>
void BaseProjectionToMeshEngine< T >::update()
{
    if( d_updateOnlyWhenAnimating.getValue() && !this->getContext()->getRootContext()->getAnimate() )
    {
        serr << "skipping update because not animating" << sendl;
        this->setDirtyOutputs();
        return;
    }

    helper::ScopedAdvancedTimer advancedTimer("BaseProjectionToMeshEngine");

    const InVecCoord& source = d_source.getValue();
    const InVecCoord& target = d_target.getValue();
    const helper::vector<defaulttype::Vec2i>& indexPairs = f_indexPairs.getValue();

    bool useRestLength = d_useRestLength.getValue();
    const helper::vector<Real>* restLength = d_restLength.isSet()?&d_restLength.getValue():nullptr;

    cleanDirty();

    std::vector<ClosestStruct>& closests = *o_closests.beginWriteOnly();

    closests.resize(indexPairs.size());


    for(size_t i=0;i<indexPairs.size();++i)
    {
        const int iP = indexPairs[i][0], iC = indexPairs[i][1];

        projectToMeshGetClosest( closests[i],source[iP],iC,target );

        if( useRestLength ) closests[i].dist -= getRestLength( iP, closests[i].dist, restLength );
    }

    o_closests.endEdit();

}


template <class T>
inline typename BaseProjectionToMeshEngine< T >::Real BaseProjectionToMeshEngine< T >::getRestLength( const unsigned int index, const Real &actualDistance, const helper::vector<Real>* restLength )
{
    // user-defined rest length
    if(restLength)
    {
        // global rest length
        if( restLength->size() == 1 ) return restLength->front();

        // specific rest length for each source points
        assert( restLength->size() == d_source.getValue().size() );
        return (*restLength)[index];
    }

    // using first availble distance as rest length

    // already computed rest length
    typename IndextoRealMap::iterator it=initialDistances.find(index);
    if(it!=initialDistances.end()) return it->second;

    // new rest length
    initialDistances[index]=actualDistance;
    return actualDistance;
}



////////////////////////////////////////////////////////////


template <class T>
ProjectionToVertexPlaneEngine< T >::ProjectionToVertexPlaneEngine()
    : Inherit1()
    , d_normals2(initData(&d_normals2,InVecCoord(),"normals2","Normals of parent2 (for projection to tangent plane method)"))
{
    this->addAlias(&d_normals2, "normal");
}



template <class T>
void ProjectionToVertexPlaneEngine< T >::projectToMeshInit()
{
    if (this->d_normals2.getValue().size() != (size_t)this->d_target.getValue().size()) { serr << "normals and target positions must be of same size" << sendl; exit(EXIT_FAILURE); }
    sout<<this->getName()<<" uses projection to tangent plane method"<<sendl;
}



template <class T>
void ProjectionToVertexPlaneEngine< T >::projectToMeshGetClosest(ClosestStruct& closest,const InCoord& p, const unsigned int& c, const InVecCoord& positions)
{
    this->projectToMesh.getClosest(closest,p,c,positions,d_normals2.getValue());
}



////////////////////////////////////////////////////////////



template <class T>
ProjectionToMeshEngine< T >::ProjectionToMeshEngine()
    : Inherit1()
    , d_triangles2(initData(&d_triangles2,SeqTriangles(),"triangles2","Triangles of parent2 (for projection to mesh method)"))
    , d_quads2(initData(&d_quads2,SeqQuads(),"quads2","Quads of parent2 (for projection to mesh method)"))
    , d_edges2(initData(&d_edges2,SeqEdges(),"edges2","Edges of parent2 (for projection to mesh method)"))
{
    this->addAlias(&d_edges2, "edges");
    this->addAlias(&d_triangles2, "triangles");
    this->addAlias(&d_quads2, "quads");
}



template <class T>
void ProjectionToMeshEngine< T >::projectToMeshInit()
{
    this->projectToMesh.init(this->d_triangles2.getValue(),this->d_quads2.getValue(),this->d_edges2.getValue());
    sout<<this->getName()<<" uses projection to triangles/quads/edges method"<<sendl;
}



template <class T>
void ProjectionToMeshEngine< T >::projectToMeshGetClosest(ClosestStruct& closest,const InCoord& p, const unsigned int& c, const InVecCoord& positions)
{
    this->projectToMesh.getClosest(closest,p,c,positions,d_triangles2.getValue(),d_quads2.getValue(),d_edges2.getValue());
}







} // namespace engine

} // namespace component

} // namespace sofa

#endif // SOFA_COMPONENT_MAPPING_BaseProjectionToMeshEngine_INL

