/******************************************************************************
*       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_TargetInterpolationEngine_H
#define SOFA_COMPONENT_TargetInterpolationEngine_H

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

namespace sofa
{

namespace component
{

namespace engine
{

/**
 * linearly interpolate target positions
 * @author Benjamin Gilles
 * @date 2016
 */
template <class DataTypes>
class TargetInterpolationEngine : public core::DataEngine
{
public:
    typedef core::DataEngine Inherited;
    SOFA_CLASS(SOFA_TEMPLATE(TargetInterpolationEngine, 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 TargetInterpolationEngine<DataTypes>* = NULL) { return DataTypes::Name();   }

    Data< InVecCoord > d_target1;
    Data< InVecCoord > d_target2;
    Data< helper::vector<unsigned> > d_indices1; ///< optional list of valid targets
    Data< helper::vector<unsigned> > d_indices2; ///< optional list of valid targets
    Data< InVecCoord > d_target;
    Data< helper::vector<unsigned> > d_indices; ///< optional list of valid targets
    Data< Real > d_weight;


protected:

    TargetInterpolationEngine():   Inherited()
      , d_target1(initData(&d_target1,"targets1","input target1"))
      , d_target2(initData(&d_target2,"targets2","input target2"))
      , d_indices1(initData(&d_indices1, "indices1", "optional list of valid targets1"))
      , d_indices2(initData(&d_indices2, "indices2", "optional list of valid targets2"))
      , d_target(initData(&d_target,"targets","computed targets"))
      , d_indices(initData(&d_indices, "indices", "list of valid targets"))
      , d_weight(initData(&d_weight,(Real)1.,"weight","weight of target1 wrt. target2"))
    {
        this->addAlias(&d_target1, "target1");
        this->addAlias(&d_target2, "target2");
        this->addAlias(&d_target, "target");
    }

    virtual ~TargetInterpolationEngine() {}

public:

    virtual void init()
    {
        addInput(&d_target1);
        addInput(&d_target2);
        addInput(&d_indices1);
        addInput(&d_indices2);
        addInput(&d_weight);
        addOutput(&d_target);
        addOutput(&d_indices);
        reinit();
        Inherited::init();
    }

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


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

        Real weight = d_weight.getValue();

        // trigger update only if targets are used in blending
        const InVecCoord* target1 = (weight>0)?&d_target1.getValue():NULL;
        const InVecCoord* target2 = (weight<1)?&d_target2.getValue():NULL;
        const helper::vector<unsigned>* indices1 = (weight>0 && d_indices1.isSet())?&d_indices1.getValue():NULL;
        const helper::vector<unsigned>* indices2 = (weight<1 && d_indices2.isSet())?&d_indices2.getValue():NULL;
        const size_t nbp = target1?target1->size():target2->size();

        cleanDirty();

        InVecCoord& targets = *d_target.beginWriteOnly();
        helper::vector<unsigned>& indices = *d_indices.beginWriteOnly();
        indices.clear();

        if(weight==0)
        {
            targets = (*target2);
            if(indices2) indices= (*indices2);
        }
        else if(weight==1)
        {
            targets = (*target1);
            if(indices1) indices= (*indices1);
        }
        else
        {
            targets.resize(nbp);
            if(indices1 && indices2)
            {
                std::set<unsigned> indices1set(indices1->begin(),indices1->end());
                std::set<unsigned> indices2set(indices2->begin(),indices2->end());
                for(size_t i=0;i<nbp;++i)
                {
                    bool in1 = (indices1set.find(i)!=indices1set.end());
                    bool in2 = (indices2set.find(i)!=indices2set.end());
                    if(in1 || in2) indices.push_back(i);
                    if(in1 && in2) targets[i] = (*target1)[i]*weight + (*target2)[i]*(1.-weight);
                    else if(in1 && !in2)  targets[i] = (*target1)[i];
                    else if(!in1 && in2)  targets[i] = (*target2)[i];
                    else  targets[i] = (*target1)[i]; // use one of the invalid target (should be the source position)
                }
            }
            else for(size_t i=0;i<nbp;++i)  targets[i] = (*target1)[i]*weight + (*target2)[i]*(1.-weight);
        }

        d_target.endEdit();
        d_indices.endEdit();
    }


};


} // namespace engine

} // namespace component

} // namespace sofa

#endif //SOFA_COMPONENT_TargetInterpolationEngine_H
