// Copyright (C) 2016 EDF
// All Rights Reserved
// This code is published under the GNU Lesser General Public License (GNU LGPL)
#ifndef LINEARINTERPOLATOR_H
#define LINEARINTERPOLATOR_H
#include <vector>
#include <Eigen/Dense>
#include "StOpt/core/grids/FullGrid.h"
#include "StOpt/core/utils/toDouble.h"

/** \file LinearInterpolator.h
 *  \brief Defines a linear interpolator on a full grid
 * \author Xavier Warin
 */
namespace StOpt
{

/// \class LinearInterpolator LinearInterpolator.h
/// Linear interpolation object for full grid
template<typename T>
class LinearInterpolator : public Interpolator<T>
{
private :

    int m_nbWeigth ; ///< storing \f$ 2^N \f$ where \f$ N \f$ is the dimension of the problem
    Eigen::Array< std::pair<T, int>, Eigen::Dynamic, 1  > m_weightAndPoints ; ///< For interpolation stores the weights and the points number in the mesh

public :

    /** \brief Default constructor
     */
    LinearInterpolator() {}

    /** \brief Constructor
     *  \param p_grid   is the grid used to interpolate
     *  \param p_point  is the coordinate of the points used for interpolatation
     */
    LinearInterpolator(const FullGrid    *p_grid, const Eigen::Array<T, Eigen::Dynamic, 1> &p_point):
        m_nbWeigth(0x01 << p_point.size()), m_weightAndPoints(m_nbWeigth)
  {
    using StOpt::toDouble;

    Eigen::ArrayXd pointDouble(p_point.size());
    for (int i = 0; i < p_point.size(); ++i)
      {
        pointDouble(i) = StOpt::toDouble(p_point(i));
      }
    // coordinate min
    Eigen::ArrayXi  coordmin = p_grid->lowerPositionCoord(pointDouble);
    Eigen::ArrayXd  meshSize = p_grid->getMeshSize(coordmin);
    // get back real coordinates
    Eigen::ArrayXd xCoord = p_grid->getCoordinateFromIntCoord(coordmin);
    // weight in each direction
    Eigen::Array<T, Eigen::Dynamic, 1> weightPerDim(p_point.size());
    for (int id = 0; id < p_point.size(); ++id)
      {
        // weights have to be positive : so force in case is nearly 0 (rounding error)
        // they have to be below 1
        weightPerDim(id) = (p_point(id) - T(xCoord(id))) / T(meshSize(id));
        if (weightPerDim(id) < T(0.)) weightPerDim(id) = T(0.);
        if (weightPerDim(id) > T(1.)) weightPerDim(id) = T(1.);
      }
    Eigen::ArrayXi iCoord(p_point.size()) ;
    // iterate on all vertex of the hypercube
    for (int j = 0 ; j < m_nbWeigth ; ++j)
      {
        unsigned int ires = j ;
        T weightLocal  = T(1.) ;
        for (int id = p_point.size() - 1 ; id >= 0  ; --id)
          {
            unsigned int idec = (ires >> id) ;
            iCoord(id) = coordmin(id) + idec;
            weightLocal *= (1 - weightPerDim(id)) * (1 - idec) + weightPerDim(id) * idec ;
            ires -= (idec << id);
          }
        m_weightAndPoints(j) = std::make_pair(weightLocal, p_grid->intCoordPerDimToGlobal(iCoord));
      }
    }

    /**  \brief  interpolate
     *  \param  p_dataValues   Values of the data on the grid
     *  \return interpolated value
     */
    inline T apply(const Eigen::Ref< const Eigen::Array<T, Eigen::Dynamic, 1> >   &p_dataValues) const override
    {
        T retInterp = T(0.);
        for (int i = 0; i < m_weightAndPoints.size(); ++i)
             retInterp  = retInterp + m_weightAndPoints(i).first *  p_dataValues(m_weightAndPoints(i).second);
        return retInterp;
    }

    /**  \brief  interpolate and use vectorization
     *  \param  p_dataValues   Values of the data on the grid. Interpolation is achieved for all values in the first dimension
     *  \return interpolated value
     */
    Eigen::Array<T, Eigen::Dynamic, 1> applyVec(const Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &p_dataValues) const override
    {
        Eigen::Array<T, Eigen::Dynamic, 1> retInterp(Eigen::Array<T, Eigen::Dynamic, 1>::Zero(p_dataValues.rows()));

        for (int i = 0; i < m_weightAndPoints.size(); ++i)
          // unroll for  types incompatible with egen
          for (int j =0 ; j <p_dataValues.rows(); ++j)
            retInterp(j)  = retInterp(j) +m_weightAndPoints(i).first *  p_dataValues(j,m_weightAndPoints(i).second);
        return retInterp;
    }

    /** \brief  Same as above but avoids copy for Numpy eigen mapping due to storage conventions
     *  \param  p_dataValues   Values of the data on the grid. Interpolation is achieved for all values in the first dimension
     *  \return interpolated value
     */
    inline Eigen::ArrayXd applyVecPy(Eigen::Ref< Eigen::ArrayXXd, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >  p_dataValues) const override
    {
        Eigen::ArrayXd retInterp(Eigen::ArrayXd::Zero(p_dataValues.rows()));
        using StOpt::toDouble;
        for (int i = 0; i < m_weightAndPoints.size(); ++i)
          retInterp  = retInterp + toDouble(m_weightAndPoints(i).first) *  p_dataValues.col(m_weightAndPoints(i).second);
        return retInterp;
    }  
};
}
#endif
