/******************************************************************************
*       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_TargetFromProjectionEngine_H
#define SOFA_COMPONENT_TargetFromProjectionEngine_H

#include "initContactMapping.h"

#include <list>

#include "projectToMesh.h"
#include <sofa/core/DataEngine.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/helper/AdvancedTimer.h>

namespace sofa
{

namespace component
{

namespace engine
{

/**
 * compute target positions, given barycentric coordinates from closest struct data
 * @author Benjamin Gilles
 * @date 2016
 */
template <class DataTypes>
class TargetFromProjectionEngine : public core::DataEngine
{
public:
    typedef core::DataEngine Inherited;
    SOFA_CLASS(SOFA_TEMPLATE(TargetFromProjectionEngine, DataTypes), Inherited);

    typedef typename DataTypes::Real    Real;
    typedef typename DataTypes::Coord    InCoord;
    typedef typename DataTypes::VecCoord InVecCoord;

    typedef helper::ProjectToMesh<InCoord> ProjectToMesh;
    typedef typename ProjectToMesh::IndexToWeightPair IndexToWeightPair;
    typedef typename ProjectToMesh::ClosestStruct ClosestStruct;

    virtual std::string getTemplateName() const    { return templateName(this);    }
    static std::string templateName(const TargetFromProjectionEngine<DataTypes>* = NULL) { return DataTypes::Name();   }

    Data< InVecCoord > d_source;
    Data< InVecCoord > d_target;

    Data<helper::vector<defaulttype::Vec2i> > f_indexPairs;    ///< input pairs of correspondences from closest point engine
    Data< std::list<std::size_t> > d_validPairs; ///< optional list of valid index pairs
    Data< helper::vector<ClosestStruct> > d_closests;  ///< closest point on mesh for each correspondence from projection*Engine

    Data< helper::vector<unsigned> > d_indices; ///< output indices of source points having a target

protected:

    TargetFromProjectionEngine():   Inherited()
      , d_source(initData(&d_source,"position","source position to project"))
      , d_target(initData(&d_target,"targets","computed targets"))
      , f_indexPairs(initData(&f_indexPairs, "indexPairs", "input list of closest point pairs (source index, target index)"))
      , d_validPairs(initData(&d_validPairs, "validPairs", "optional set of valid index pairs"))
      , d_closests(initData(&d_closests,"closests","closest primitives with weights, normal, distance"))
      , d_indices(initData(&d_indices, "indices", "indices of valid targets"))
    {
        this->addAlias(&d_source, "source");
        this->addAlias(&d_target, "target");
    }

    virtual ~TargetFromProjectionEngine() {}

public:

    virtual void init()
    {
        addInput(&d_source);
        addInput(&f_indexPairs);
        addInput(&d_validPairs);
        addInput(&d_closests);
        addOutput(&d_target);
        addOutput(&d_indices);
        reinit();
        Inherited::init();
    }

    virtual void reinit()
    {
        Inherited::reinit();
        update();
    }


    virtual void update()
    {
        helper::ScopedAdvancedTimer advancedTimer("TargetFromProjectionEngine");

        updateAllInputsIfDirty();

        cleanDirty();

        const InVecCoord& source = d_source.getValue();
        const helper::vector<defaulttype::Vec2i>& indexPairs = f_indexPairs.getValue();
        helper::ReadAccessor<Data< helper::vector<ClosestStruct> > > closests (d_closests);
        helper::WriteOnlyAccessor<Data< InVecCoord > > targets(d_target);
        helper::WriteOnlyAccessor<Data< helper::vector<unsigned> > > indices(d_indices);
        indices.clear();

        const size_t nbp = source.size();

        targets.resize(nbp);
        for(size_t i=0;i<nbp;++i)  targets[i]=InCoord();
        std::vector<unsigned int> counter(nbp,0); // use a counter to average multiple targets

        helper::IndexPairIterator indexPairIterator( indexPairs.size(), d_validPairs.getValue(), d_validPairs.isSet() );
        for ( ; indexPairIterator.count()<indexPairIterator.size() ; ++indexPairIterator )
        {
            const size_t i = indexPairIterator.get();
            const int iP = indexPairs[i][0];
            targets[iP] -= closests[i].u*closests[i].dist;
            counter[iP]++;
        }
        for(size_t i=0;i<nbp;++i)
        {
            if(counter[i]!=0) { targets[i]/=(Real)counter[i]; indices.push_back(i);}
            targets[i] += source[i];
        }

    }


};


} // namespace engine

} // namespace component

} // namespace sofa

#endif //SOFA_COMPONENT_TargetFromProjectionEngine_H
