/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program 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 program 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 program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/

#ifndef LINEARSUBSPACE_BaseLSShapeFunction_H
#define LINEARSUBSPACE_BaseLSShapeFunction_H

#include <sofa/core/objectmodel/BaseObject.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/defaulttype/Mat.h>
#include <sofa/defaulttype/MatSym.h>
#include <sofa/defaulttype/Vec.h>
#include <sofa/helper/vector.h>
#include <sofa/helper/SVector.h>
#include <sofa/core/behavior/BaseMechanicalState.h>

#include <Flexible/shapeFunction/BaseShapeFunction.h>


namespace sofa
{
namespace core
{
namespace behavior
{


//template<typename TShapeFunctionTypes>
//struct ShapeFunctionInternalData
//{
//};


/** Similar to Flexible BaseShapeFunction. It adds weigh homogeneous vector for affine parent as used in Linear Subspace.
  */

template <class TLSShapeFunctionTypes>
class BaseLSShapeFunction : virtual public core::objectmodel::BaseObject
//class BaseLSShapeFunction : virtual public core::behavior::BaseShapeFunction<TLSShapeFunctionTypes>
{
public:
    SOFA_ABSTRACT_CLASS(SOFA_TEMPLATE(BaseLSShapeFunction, TLSShapeFunctionTypes) , objectmodel::BaseObject);
//    SOFA_ABSTRACT_CLASS(SOFA_TEMPLATE(BaseLSShapeFunction, TLSShapeFunctionTypes) , SOFA_TEMPLATE(core::behavior::BaseShapeFunction, TLSShapeFunctionTypes));

    typedef TLSShapeFunctionTypes LSShapeFunctionTypes;
    static const unsigned int spatial_dimensions=LSShapeFunctionTypes::spatial_dimensions;

    /** @name types */
    //@{
    typedef typename LSShapeFunctionTypes::Real Real;
    typedef typename LSShapeFunctionTypes::VReal VReal;
    typedef typename LSShapeFunctionTypes::VecVReal VecVReal;
    typedef typename LSShapeFunctionTypes::Ref Ref;
    typedef typename LSShapeFunctionTypes::VRef VRef;
    typedef typename LSShapeFunctionTypes::Weight Weight;
    typedef typename LSShapeFunctionTypes::VWeight VWeight;
    typedef typename LSShapeFunctionTypes::AffineWeight AffineWeight;
    typedef typename LSShapeFunctionTypes::VAffineWeight VAffineWeight;
    typedef typename LSShapeFunctionTypes::Coord Coord;                          ///< Spatial coordinates in world space
    typedef typename LSShapeFunctionTypes::VCoord VCoord;
    typedef typename LSShapeFunctionTypes::Gradient Gradient;                       ///< Gradient of a scalar value in world space
    typedef typename LSShapeFunctionTypes::VGradient VGradient;
    typedef typename LSShapeFunctionTypes::AffineGradient AffineGradient;
    typedef typename LSShapeFunctionTypes::VAffineGradient VAffineGradient;

    typedef typename LSShapeFunctionTypes::Hessian Hessian;                       ///< Hessian (second derivative) of a scalar value in world space
    typedef typename LSShapeFunctionTypes::VHessian VHessian;
    typedef typename LSShapeFunctionTypes::AffineHessian AffineHessian;
    typedef typename LSShapeFunctionTypes::VAffineHessian VAffineHessian;
    typedef typename LSShapeFunctionTypes::Cell Cell;
    typedef typename LSShapeFunctionTypes::VCell VCell;

    typedef typename LSShapeFunctionTypes::VecVRef VecVRef;
    typedef typename LSShapeFunctionTypes::VecVWeight VecVWeight;
    typedef typename LSShapeFunctionTypes::VecVAffineWeight VecVAffineWeight;
    typedef typename LSShapeFunctionTypes::VecVGradient VecVGradient;
    typedef typename LSShapeFunctionTypes::VecVAffineGradient VecVAffineGradient;
    typedef typename LSShapeFunctionTypes::VecVHessian VecVHessian;
    typedef typename LSShapeFunctionTypes::VecVAffineHessian VecVAffineHessian;
//	typedef ShapeFunctionInternalData<TShapeFunctionTypes> InternalData;
    //@}

    /** @name data */
    //@{
    Data<unsigned int > d_nbRef;      ///< maximum number of parents per child
    Data<unsigned int > d_nbPointParent; ///< number of parents which are points
    Data< VCoord > d_position;  ///< spatial coordinates of the parent nodes, all points then all affines
//    InternalData m_internalData;
    //@}

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

    virtual void init()
    {
        if(!d_position.isSet()) {
            serr << "Missing parent positions" << sendl;
            return;
        }
    }


    /// interpolate shape function values (and their first and second derivatives) at a given child position
    /// this function is typically used for collision and visual points
    virtual void computeShapeFunction(const Coord& childPosition,
                                      VRef& ref, VRef& affineRef,
                                      VWeight& w, VAffineWeight& affineW )=0;

    /// wrappers
    virtual void computeShapeFunction(const VCoord& childPosition,
                                      VecVRef& ref, VecVRef& affineRef,
                                      VecVWeight& w, VecVAffineWeight& affineW)
    {
        unsigned int nb = childPosition.size();
        ref.resize(nb);
        w.resize(nb);
        affineRef.resize(nb);
        affineW.resize(nb);

//        msg_info("BaseLSShapeFunction::computeShapeFunction()") << "Computing shapefunction for " << nb << " child positions.";

        // TODO openmp touch ?
        for(unsigned i=0; i<nb; i++)
        {
//            msg_info("BaseLSShapeFunction::computeShapeFunction()") << "  Iter #" << i << " / " << nb << ".";
            computeShapeFunction(childPosition[i],ref[i],affineRef[i],w[i],affineW[i]);
        }
    }


    /// used to make a partition of unity: $sum_i w_i + sum_j affineW_j(spatial_dimensions) =1$ and adjust derivatives accordingly
    void normalize(VWeight& w, VAffineWeight& affineW, VGradient* dw=nullptr, VAffineGradient* affineDw=nullptr)
    {
        unsigned int nbRef=w.size();
        unsigned int nbAffineRef=affineW.size();
        Weight sum_w=0;
        Gradient sum_dw; sum_dw.clear();
        AffineGradient sum_affineDw;

        // Compute norm
        for (unsigned int j = 0; j < nbRef; j++) sum_w += w[j];
        for (unsigned int j = 0; j < nbAffineRef; j++) sum_w += affineW[j][spatial_dimensions];
        if(dw && affineDw)
        {
            for (unsigned int j = 0; j < nbRef;       j++) sum_dw += (*dw)[j];
            for (unsigned int j = 0; j < nbAffineRef; j++) sum_affineDw += (*affineDw)[j];
        }

        // Normalize
        if(sum_w) {
            for (unsigned int j = 0; j < nbRef; j++) {
                Weight wn=w[j]/sum_w;
                if(dw) {
                    Gradient dwn=((*dw)[j] - sum_dw*wn)/sum_w;
                    (*dw)[j]=dwn;
                }
                w[j]=wn;
            }
            for (unsigned int j = 0; j < nbAffineRef; j++) {
                AffineWeight affineWn = affineW[j]/sum_w;
                if(dw) {
                    AffineGradient a,b,c;
                    c = a-b;
                    AffineGradient affineDwn;
                    for (std::size_t k=0; k<AffineGradient::size(); ++k) affineDwn[k] = ((*affineDw)[j][k] - sum_affineDw[k]*affineWn[k])/sum_w;
                    (*affineDw)[j]=affineDwn;
                }
                affineW[j]=affineWn;
            }
        }
    }


protected:
    BaseLSShapeFunction()
        : d_nbRef(initData(&d_nbRef,(unsigned int)4,"nbRef", "maximum number of parents per child"))
        , d_nbPointParent(initData(&d_nbPointParent,"nbPointParent", "number of point parents"))
        , d_position(initData(&d_position,"position", "position of parent nodes, point parent first, then aafine parents"))
    {
    }

    virtual ~BaseLSShapeFunction() {}

};


template <int spatial_dimensions_, class Real_>
struct LSShapeFunctionTypes
{
    typedef Real_ Real;
    typedef helper::vector<Real> VReal;
    typedef unsigned int Ref;
    typedef helper::vector<Ref> VRef;
    typedef Real Weight;                                               ///< scalar weight for point handle
    typedef helper::vector<Real> VWeight;
    typedef defaulttype::Vec<spatial_dimensions_+1,Real> AffineWeight; ///< homogeneous weight vector for affine handle
    typedef helper::vector<AffineWeight> VAffineWeight;
    typedef defaulttype::Vec<spatial_dimensions_,Real> Coord;          ///< Spatial coordinates in world space
    typedef helper::vector<Coord> VCoord;
    typedef defaulttype::Vec<spatial_dimensions_,Real> Gradient;       ///< Gradient of a point weight value in world space
    typedef helper::vector<Gradient> VGradient;
    /// Gradient of an affine handle weight value in world space
    typedef defaulttype::Vec<spatial_dimensions_+1, Real> AffineGradient;
    typedef helper::vector<AffineGradient> VAffineGradient;
    typedef defaulttype::Mat<spatial_dimensions_,spatial_dimensions_,Real> Hessian;    ///< Hessian (second derivative) of a scalar value in world space
    typedef helper::vector<Hessian> VHessian;
    typedef defaulttype::Mat<spatial_dimensions_,spatial_dimensions_,Real> AffineHessian;    ///< Hessian (second derivative) of a scalar value in world space
    typedef helper::vector<AffineHessian> VAffineHessian;
//    typedef int Cell;
//    typedef helper::vector<Cell> VCell;

    typedef helper::vector< helper::SVector<Ref> > VecVRef;
    typedef helper::vector< helper::SVector<Real> > VecVReal;
    typedef helper::vector< helper::SVector<Real> > VecVWeight;
    typedef helper::vector< helper::SVector< AffineWeight > > VecVAffineWeight;
    typedef helper::vector< helper::SVector<Gradient> > VecVGradient;
    typedef helper::vector< helper::SVector< AffineGradient > > VecVAffineGradient;
    typedef helper::vector< helper::SVector< Hessian > > VecVHessian;
    typedef helper::vector< helper::SVector< AffineHessian > > VecVAffineHessian;

//    typedef helper::vector< helper::SVector<Hessian> > VecVHessian;


    static const int spatial_dimensions=spatial_dimensions_ ;
    static const char* Name();




//    typedef helper::vector<Real> VReal;
//    typedef defaulttype::Mat<spatial_dimensions_,spatial_dimensions_,Real> Hessian;
//    typedef helper::vector<Hessian> VHessian;
    typedef int Cell;
    typedef helper::vector<Cell> VCell;

//    typedef helper::vector< helper::SVector<Real>> VecVReal;
//    typedef helper::vector< helper::SVector<Hessian>> VecVHessian;
};

#ifndef SOFA_FLOAT
typedef LSShapeFunctionTypes<3,double> LSShapeFunctiond;
typedef LSShapeFunctionTypes<2,double> LSShapeFunction2d;
template<> inline const char* LSShapeFunctiond::Name() { return "LSShapeFunctiond"; }
template<> inline const char* LSShapeFunction2d::Name() { return "LSShapeFunction2d"; }
#endif
#ifndef SOFA_DOUBLE
typedef LSShapeFunctionTypes<3,float>  LSShapeFunctionf;
typedef LSShapeFunctionTypes<2,float>  LSShapeFunction2f;
template<> inline const char* LSShapeFunctionf::Name() { return "LSShapeFunctionf"; }
template<> inline const char* LSShapeFunction2f::Name() { return "LSShapeFunction2f"; }
#endif

#ifdef SOFA_FLOAT
typedef LSShapeFunctionf LSShapeFunction;
typedef LSShapeFunction2f LSShapeFunction2;
#else
typedef LSShapeFunctiond LSShapeFunction;
typedef LSShapeFunction2d LSShapeFunction2;
#endif


}
}
}

#endif
