/******************************************************************************
*       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_ClosestPointInterpolationEngine_H
#define SOFA_COMPONENT_ClosestPointInterpolationEngine_H

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

#include <sofa/helper/IndexOpenMP.h>
#ifdef _OPENMP
#include <omp.h>
#endif

namespace sofa
{

namespace component
{

namespace engine
{

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

    typedef typename DataTypes::Coord    InCoord;
    typedef typename DataTypes::Deriv    InDeriv;
    typedef typename DataTypes::VecCoord InVecCoord;
    typedef typename DataTypes::VecDeriv InVecDeriv;
    typedef typename DataTypes::Real     Real;

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

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

    Data< helper::vector<Real> > d_pointData;
    Data< helper::vector<ClosestStruct> > d_closests; ///< store closest point on mesh from each parent1 point
    Data< Real > d_outValue;		///< default value when no interpolation is provided
    Data< bool > d_parallel;		///< use openmp ?
    Data< helper::OptionsGroup > d_interpolation;  ///< nearest, linear

    Data< helper::vector<Real> > d_values;

protected:

    ClosestPointInterpolationEngine():   Inherited()
      , d_pointData(initData(&d_pointData,"pointData","input point values to be interpolated"))
      , d_closests(initData(&d_closests,"closests","closest primitives with weights, normal, distance"))
      , d_outValue(initData(&d_outValue,(Real)0,"defaultValue","default value when no interpolation is provided"))
      , d_parallel(initData(&d_parallel, true, "parallel", "use openmp parallelisation?"))
      , d_interpolation( initData ( &d_interpolation,"interpolation","Interpolation method." ) )
      , d_values(initData(&d_values,"values","interpolated values"))
    {
        this->addAlias(&d_outValue, "outValue");
        helper::OptionsGroup InterpolationOptions(2,"Nearest", "Linear");
        InterpolationOptions.setSelectedItem(1);
        d_interpolation.setValue(InterpolationOptions);

    }

    virtual ~ClosestPointInterpolationEngine() {}

public:

    virtual void init()
    {

        addInput(&d_pointData);
        addInput(&d_closests);
        addInput(&d_outValue);
        addInput(&d_interpolation);
        addOutput(&d_values);

        reinit();
        Inherited::init();
    }

    virtual void reinit()
    {
        Inherited::reinit();
        d_values.setDirtyValue();
        update();
    }


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

        updateAllInputsIfDirty();

        cleanDirty();

        helper::ReadAccessor<Data< helper::vector<ClosestStruct> > > closests (d_closests);
        helper::ReadAccessor<Data< helper::vector<Real> > > pointData (d_pointData);
        helper::WriteOnlyAccessor<Data< helper::vector<Real> > > values(d_values);
        Real outValue = d_outValue.getValue();

        values.resize(closests.size());

        switch(d_interpolation.getValue().getSelectedId())
        {
        case 0: // nearest
        {
#ifdef _OPENMP
#pragma omp parallel for if (this->d_parallel.getValue())
#endif
            for(sofa::helper::IndexOpenMP<size_t>::type i=0;i<closests.size();i++)
                if(!closests[i].indexWeightSet.size())        values[i]=outValue;
                else
                {
                    Real maxw=-1;
                    for(typename std::set<IndexToWeightPair>::iterator it=closests[i].indexWeightSet.begin(); it!=closests[i].indexWeightSet.end(); it++)
                        if(it->second>maxw)
                        {
                            values[i]=pointData[it->first];
                            maxw=it->second;
                        }
                }
        }
            break;

        default : // linear
        {
#ifdef _OPENMP
#pragma omp parallel for if (this->d_parallel.getValue())
#endif
            for(sofa::helper::IndexOpenMP<size_t>::type i=0;i<closests.size();i++)
                if(!closests[i].indexWeightSet.size())        values[i]=outValue;
                else
                {
                    values[i]=0;
                    for(typename std::set<IndexToWeightPair>::iterator it=closests[i].indexWeightSet.begin(); it!=closests[i].indexWeightSet.end(); it++)
                        values[i]+=pointData[it->first]*it->second;
                }
        }
            break;
        }

    }


};


#if defined(SOFA_EXTERN_TEMPLATE) && !defined(SOFA_COMPONENT_MAPPING_ClosestPointInterpolationEngine_CPP)
#ifndef SOFA_FLOAT
extern template class SOFA_ContactMapping_API ClosestPointInterpolationEngine< sofa::defaulttype::Vec3dTypes >;
#endif
#ifndef SOFA_DOUBLE
extern template class SOFA_ContactMapping_API ClosestPointInterpolationEngine< sofa::defaulttype::Vec3fTypes >;
#endif
#endif

} // namespace engine

} // namespace component

} // namespace sofa

#endif //SOFA_COMPONENT_ClosestPointInterpolationEngine_H
