/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 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_ClosestPointEngine_H
#define SOFA_ClosestPointEngine_H

#include "initContactMapping.h"
#include <sofa/core/DataEngine.h>
#include <sofa/core/objectmodel/BaseObject.h>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/helper/kdTree.h>
#include <sofa/helper/OptionsGroup.h>
#include <sofa/helper/vectorData.h>
#include <sofa/defaulttype/RigidTypes.h>

namespace sofa
{

namespace component
{

namespace engine
{


/**
 * For each source point, computes the closest point in targets
 * distances are computed based on positions, and possibly normals and extra features (eg. curvature)
 * currently only L2 metrics are supported,
 * each feature can be weighted separately
 *
 * @author Benjamin Gilles
 * @date 2015
 */


template <class T>
class ClosestPointEngine : public core::DataEngine
{

public:
    typedef core::DataEngine Inherited;
    SOFA_CLASS(SOFA_TEMPLATE(ClosestPointEngine,T),Inherited);

    typedef typename T::Coord Coord;
    typedef typename T::VecCoord VecCoord;
    typedef typename T::Real Real;
    typedef helper::vector<Real> VecReal;
    enum {sizeT = T::deriv_total_size };

    typedef helper::vector<unsigned int> ROI;
    typedef helper::ReadAccessor<Data< ROI > > raROI;

    typedef defaulttype::StdRigidTypes<T::deriv_total_size, Real> rigidType;
    typedef typename rigidType::Coord rigidCoord;

    Data< ROI > f_sourceROI;            ///< List of indices from source. If empty and sourceROISize is not set, all vertices are considered
    Data<unsigned int> f_sourceROISize; ///< if set, resample 'sourceROISize' points and set the ROI accordingly
    Data< ROI > f_targetROI;            ///< List of indices from target. If empty and targetROISize is not set, all vertices are considered
    Data<unsigned int> f_targetROISize; ///< if set, resample 'targetROISize' points and set the ROI accordingly
    Data< ROI > f_rejectROI;            ///< List of indices from target (typically the border), that if selected, will cancel correspondences

    Data<VecCoord> f_sourcePosition;    ///< positions of source points
    Data<VecCoord > f_targetPosition;   ///< positions of target points
    Data<Real > f_weightPosition;       ///< [optional] weight of positions wrt. other features during distance computation

    Data< rigidCoord> f_sourceRigidPosition;  ///< [optional] rigid offset of the source
    Data< rigidCoord> f_targetRigidPosition;  ///< [optional] rigid offset of the target

    Data<VecCoord> f_sourceNormal;            ///< [optional] source normals to be included in distance computation
    Data<VecCoord > f_targetNormal;           ///< [optional] target normals to be included in distance computation
    Data<Real > f_weightNormal;               ///< [optional] weight of normals wrt. other features during distance compution
    Data<Real > f_normalThreshold;            ///< [optional] dotproduct threshold [-1 1] below which correspondences are ignored
    Data<Real > f_distanceThreshold;            ///< [optional] distance above which correspondences are ignored

    Data<unsigned int> f_nbFeatures;
    helper::vectorData<VecReal> vf_sourceFeature;           ///< [optional] source scalar features to be included in distance computation
    helper::vectorData<VecReal > vf_targetFeature;          ///< [optional] target scalar features to be included in distance computation
    helper::vectorData<Real > vf_weightFeature;             ///< [optional] weight of features wrt. other features during distance compution

    Data<unsigned int> f_cacheSize;                         ///< number of closest points used in kdtree cache to speed up closest point computation.
    Data<bool> f_useBV;                                     ///< filter out source points not in target bounding volume ?
    Data<Real> f_offsetBV;                                  ///< bounding volume dilation
    Data<helper::OptionsGroup> f_method;                    ///< acceleration method for closest point search
    Data<helper::vector<defaulttype::Vec2i> > f_closest;    ///< output pairs
    Data<bool> d_revertPairs;                               ///< if True pairs(target_index, source_index). False by default pairs(source_index,target_index)

    Data< bool > d_parallel;		///< use openmp ?

    Data<int> drawMode; //Draw Mode: 0=None - 1=Line - 2=Cylinder - 3=Arrow
    Data<float> showArrowSize;


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

    ClosestPointEngine()    :   Inherited()
      , f_sourceROI(initData(&f_sourceROI, "sourceROI", "List of indices from source. If empty and sourceROISize is not set, all vertices are considered"))
      , f_sourceROISize(initData(&f_sourceROISize, (unsigned)0, "sourceROISize", "if set, resample 'sourceROISize' points and set the ROI accordingly"))
      , f_targetROI(initData(&f_targetROI, "targetROI", "List of indices from target. If empty and targetROISize is not set, all vertices are considered"))
      , f_targetROISize(initData(&f_targetROISize, (unsigned)0, "targetROISize", "if set, resample 'targetROISize' points and set the ROI accordingly"))
      , f_rejectROI(initData(&f_rejectROI, "rejectROI", "List of indices from target (typically the border), that if selected, will cancel correspondences"))
      , f_sourcePosition (initData(&f_sourcePosition, "sourcePosition", "source positions"))
      , f_targetPosition (initData(&f_targetPosition, "targetPosition", "target positions"))
      , f_weightPosition( initData (&f_weightPosition, (Real)1.0, "weightPosition", "position weight") )
      , f_sourceRigidPosition(initData(&f_sourceRigidPosition,rigidCoord(),"sourceRigidPosition","optional rigid offset of the provided source points to match simulated positions"))
      , f_targetRigidPosition(initData(&f_targetRigidPosition,rigidCoord(),"targetRigidPosition","optional rigid offset of the provided target points to match simulated positions"))
      , f_sourceNormal (initData(&f_sourceNormal, "sourceNormal", "source normals"))
      , f_targetNormal (initData(&f_targetNormal, "targetNormal", "target normals"))
      , f_weightNormal( initData (&f_weightNormal, (Real)0.0, "weightNormal", "normal weight") )
      , f_normalThreshold( initData (&f_normalThreshold, (Real)-1.0, "normalThreshold", "threshold on normal dotproduct [-1,1] below which correspondences are ignored") )
      , f_distanceThreshold( initData (&f_distanceThreshold, (Real)std::numeric_limits<Real>::max(), "distanceThreshold", "threshold on distance above which correspondences are ignored") )
      , f_nbFeatures( initData (&f_nbFeatures, (unsigned)0, "nbFeatures", "number of features") )
      , vf_sourceFeature(this,"sourceFeature", "source feature", helper::DataEngineInput)
      , vf_targetFeature(this,"targetFeature", "target feature", helper::DataEngineInput)
      , vf_weightFeature(this,"weightFeature", "feature weight", helper::DataEngineInput, 1.0)
      , f_cacheSize(initData(&f_cacheSize,(unsigned int)5,"cacheSize","number of closest points used in kdtree cache to speed up closest point computation."))
      , f_useBV(initData(&f_useBV, false, "useBV", "filter out source points not in target bounding volume ?"))
      , f_offsetBV(initData(&f_offsetBV, (Real)0, "offsetBV", "Bounding volume dilation"))
      , f_method ( initData ( &f_method,"method","method" ) )
      , f_closest (initData(&f_closest, "indexPairs", "output list of closest point pairs (source index, target index)"))
      , d_revertPairs(initData(&d_revertPairs, false, "revertPairs", "revert pair ordering?"))
      , d_parallel(initData(&d_parallel, false, "parallel", "use openmp parallelisation?"))
      , drawMode(initData(&drawMode,0,"drawMode","The way springs will be drawn:\n- 0: None\n- 1: Line\n- 2:Cylinder\n- 3: Arrow."))
      , showArrowSize(initData(&showArrowSize,0.01f,"showArrowSize","size of the axis."))
      , d_KdTree(initData(&d_KdTree,"KdTree","Optional KdTree structure"))
    {
        vf_sourceFeature.resize(f_nbFeatures.getValue());
        vf_targetFeature.resize(f_nbFeatures.getValue());
        vf_weightFeature.resize(f_nbFeatures.getValue());

        helper::OptionsGroup options(2, "0 - Brute force" , "1 - KdTree");
#ifdef SOFA_HAVE_IMAGE
        options.setNbItems(options.size()+1);
        options.setItemName(options.size()-1,std::string("2 - Image"));
#endif
        //        "3 - Flann"
        //        "4 - kNN"
        options.setSelectedItem(1);
        f_method.setValue(options);
    }


    virtual ~ClosestPointEngine()
    {
    }

    virtual void init()
    {
        addInput(&f_sourceROI);
        addInput(&f_sourceROISize);
        addInput(&f_targetROI);
        addInput(&f_targetROISize);
        addInput(&f_rejectROI);
        addInput(&f_sourcePosition);
        addInput(&f_targetPosition);
        addInput(&f_weightPosition);
        addInput(&f_sourceRigidPosition);
        addInput(&f_targetRigidPosition);
        addInput(&f_sourceNormal);
        addInput(&f_targetNormal);
        addInput(&f_weightNormal);
        addInput(&f_normalThreshold);
        addInput(&f_distanceThreshold);
        addInput(&f_nbFeatures);
        vf_sourceFeature.resize(f_nbFeatures.getValue());
        vf_targetFeature.resize(f_nbFeatures.getValue());
        vf_weightFeature.resize(f_nbFeatures.getValue());
        addInput(&f_cacheSize);
        addInput(&f_useBV);
        addInput(&f_offsetBV);
        addInput(&f_method);
        addInput(&d_revertPairs);
        addOutput(&f_closest);
        setDirtyValue();


        // track these Data to check if they changed since the last update
        m_dataTracker.trackData(f_targetPosition);
        m_dataTracker.trackData(f_targetROI);
        m_dataTracker.trackData(f_method);
        m_dataTracker.trackData(f_useBV);
        m_dataTracker.trackData(f_offsetBV);
        m_dataTracker.trackData(f_rejectROI);


        reinit();
    }

    virtual void reinit();

    /// Parse the given description to assign values to this object's fields and potentially other parameters
    void parse ( sofa::core::objectmodel::BaseObjectDescription* arg )
    {
        vf_sourceFeature.parseSizeData(arg, f_nbFeatures);
        vf_targetFeature.parseSizeData(arg, f_nbFeatures);
        vf_weightFeature.parseSizeData(arg, f_nbFeatures);
        Inherit1::parse(arg);
    }

    /// Assign the field values stored in the given map of name -> value pairs
    void parseFields ( const std::map<std::string,std::string*>& str )
    {
        vf_sourceFeature.parseFieldsSizeData(str, f_nbFeatures);
        vf_targetFeature.parseFieldsSizeData(str, f_nbFeatures);
        vf_weightFeature.parseFieldsSizeData(str, f_nbFeatures);
        Inherit1::parseFields(str);
    }

protected:

    virtual void update();
    inline Real getDistance(const unsigned int i,const unsigned int j) const;
    inline bool isRejected(const unsigned int i,const unsigned int j) const;

    void draw(const core::visual::VisualParams* vparams);

    // methods
    void bruteForceMethod();
    void kdTreeMethod(const bool targetDirty);
#ifdef SOFA_HAVE_IMAGE
    void imageMethod(const bool targetDirty);
#endif

    bool usePosition;
    bool useNormal;
    std::vector<bool> useFeature;

    bool useNormalRejection;

    std::set<unsigned int> rejectROI; ///< rejectROI as a set (and not a vector) for efficient look-up

    // resampling according to ROISizes
    void resampleROI(const bool useSource);
    void updateVoronoi(std::vector<Real>& distances, std::vector<unsigned int>& voronoi, const VecCoord& pos, const unsigned int index);

    // bounding volume
    typedef defaulttype::Vec<9,Real> BVCoord; // here we use DOPs
    typedef defaulttype::Vec<2,BVCoord> BVType;
    BVType BV;
    void updateTargetBoundingVolume();
    inline bool isInBoundingVolume(const unsigned int i) const;
    inline void getBVCoord(BVCoord& c, const Coord& p) const;

    // data used for rigid offseting
    VecCoord offseted_sourcePosition;
    VecCoord offseted_sourceNormal;
    const VecCoord* sourcePosition;
    const VecCoord* sourceNormal;
    void udateOffsetedPosition(); // point to the transformed input points, or input points if there is no transformation

    // storage of closest point as a vector for parallelization
    helper::vector<unsigned int> closest;

    // data used for kdtree method
    typedef helper::kdTree<Coord> KDT;
    Data<KDT> d_KdTree;

    typedef typename KDT::distanceSet distanceSet;
    typedef typename KDT::distanceToPoint distanceToPoint;
    // storage for cache acceleration
    helper::vector< distanceSet >  closests;
    helper::vector< distanceToPoint > cacheThresh_max;
    helper::vector< distanceToPoint > cacheThresh_min;
    VecCoord previousPosition;
};


} // namespace engine
} // namespace component
} // namespace sofa

#endif // SOFA_IMAGE_ClosestPointEngine_H
