#include <vector>
#include <iostream>
#include <map>
#include <cmath>
#include "protos_lapack.hpp"
#include "RBF_functions.hpp"
using namespace std;
using namespace lapack_c;
#ifndef RBFInterp__hpp
#define RBFInterp__hpp

//-------------------------------------------------------------------------
// Interpolation rbf.
//-------------------------------------------------------------------------
template<typename real,class Radial>  class RBFInterp{
  // on met tout dans une classe paramétrée par le type de flottants,
  // et par Radial qui est le type (cllase) de fonction d'interpolation.
  double * matrix , *b; // pour le système linaéire
  real * image; // l'"image"
  real eps;
  int imgsize_x,imgsize_y; // tailles
  // work sert à lapack (tableau de travail).
  double rcond,innorm; double *work; bool rcondok;
  
public:
  // Ce qui définit les points de mesure
  // (x,y) -> valeur
  // NB:
  // Il ne faut pas d'ex-aequo pour (x,y), d'où l'utilisation d'une map
  // pour décrire les noeuds de l'interpolation.
  //Clé de la map: paire d'entiers (ordonnée lexicographiquement)
  // Les clés sont uniques.
  typedef std::map<pair<int,int>,real> PointValue;

  // consructeur:
  RBFInterp(real epsilon):eps(epsilon){}
  //changer epsilon
  void set_epsilon(real epsilon){eps=epsilon;}
  // Calcule les RBF.
  void build(PointValue& values,bool comp_cond= false)
  // comp_cond : on calcule ou non le conditionnement L\infinity de
  // la matrice.
  {
    Radial  R(eps);  // fontion d'interpolation, à eps fixé.
    int np= values.size();
   
    matrix = new double[np*np];
    b=  new double[np];

    // construire le système linéaire (on n'utilise pas la symétrie de la
    // matrice).

    // 1) matrice:
    auto  indix= [np](int i, int j){return i*np+j;};//lambda function.
    int i=0;
    for(typename PointValue::const_iterator I=values.begin();
	I!=values.end();I++) //parcourir la map. Boucle sur les fonctions
      {
	auto v= I->first; //la fonction est centrée en ce point (paire i,j)
	int j=i;
	for(typename PointValue::const_iterator J=I;J!=values.end();++J)
	  // boucle sur les points
	  {
	    //calculer la distance r entre le point I et le point J
	    // et y applique la fonction radiale ->s.
	   
	    auto s = R(r(v,J->first));
	    matrix[indix(i,j)]=s;
	    if(i!=j)//matrice symétrique
	      matrix[indix(j,i)] =s;
	    ++j;
	  }
	++i;
      }
   
    //2) second membre.
    i=0;
    for(typename PointValue::iterator I= values.begin();I!=values.end();I++)
      b[i++]=I->second;

    // resolution (lapack + blas).
    int* ipiv= new int[np];
    int nrhs=1,lda=np,ldb=np,info;
    char trans='N'; char norm='I';

    //
    rcondok =  comp_cond;
    if(comp_cond)
      {
	// norme oo de la matrice:
	
	work= new double[4*np];
	// norme infinie de la matrice:
	innorm= dlange_(&norm,&np,&np,matrix,&lda,work);
      }
    // factorisation PLU:
    dgetrf_(&np,&np,matrix,&lda,ipiv,&info);
    if(info != 0) throw  LapackException(info);
    if(comp_cond)
      {
	int *iwork=new int[np];
	dgecon_(&norm,&np,matrix,&lda,&innorm,&rcond,work,iwork,&info);
	rcond= 1.0/rcond;
	if(info != 0) throw  LapackException(info);
	delete[] work; delete[] iwork;
      }
    // résolution du système factorisé:
    dgetrs_(&trans,&np,&nrhs,matrix,&lda,ipiv,b,&ldb,&info);
    if(info != 0) throw  LapackException(info);
    //
    delete[] matrix;
    delete[] ipiv;
  }
 
  real test(const PointValue& interpvalues,const PointValue& testvalues)
  {
    // Estimer l'erreur.
    
    // interpvalues: les noeuds d'interpolation
    // testvalues  : les noeuds test.
     Radial  R(eps);
    
     real ret=0.0;
     // boucle sur les noeuds test :
     for(typename PointValue::const_iterator K=testvalues.begin();
	 K!=testvalues.end();K++)
       {
	 int k=0;
	 real u=0.0;
	 for(typename PointValue::const_iterator L=interpvalues.begin();
	     L!=interpvalues.end();L++)
	   {
	     auto dist= r(K->first,L->first);
	     u+=b[k++]*R(dist);
	   }
	 ret=max(ret,abs(K->second-u));
       }
     return ret;
  }
  real Interpolator(PointValue& values,int nx,int i,int j,Radial R)
  {
    real s=0.0;
    int k=0;
    for(typename PointValue::const_iterator K=values.begin();
	K!=values.end();K++)
      s+=b[k++]* R(r(K->first,make_pair(i,j)));
    return s;
  }
  // interpolation sur une image nx par ny
  void Interp(PointValue& values,real *image,int nx,int ny,int increment=1)
  {
    Radial  R(eps);
    auto  indix= [nx](int i, int j){return i*nx+j;};

    #pragma omp parallel for
      for(int i=0;i<ny;i+=increment)
	for(int j=0;j<nx;j+=increment)
	{
	  // real s=0.0;
	  // int k=0;
	  // for(typename PointValue::const_iterator K=values.begin();
	  //     K!=values.end();K++)
	  //   s+=b[k++]* R(r(K->first,make_pair(i,j)));
	  //image[indix(i,j)] = s;
	  image[indix(i,j)] =  Interpolator(values,nx,i,j,R);
	}
      if(increment == 2)
	{
	  for(int i=ny-3;i<ny;i++)
	    for(int j=0;j<nx;j++)
	      image[indix(i,j)] =  Interpolator(values,nx,i,j,R);
	  for(int j=ny-3;j<nx;j++)
	    for(int i=0;i<ny;i++)
	      image[indix(i,j)] =  Interpolator(values,nx,i,j,R);
	}
	
  }
  ~RBFInterp()
  {
    delete[] b;
   
  }
  double cond() const
  {
    // accesseur pour le conditionnement
    if(rcondok)
      return rcond;
    else
      throw "rcond pas calculé";
  }
 double norme_matrice() const
  {
    if(rcondok)
      return innorm;
    else
      throw "rcond pas calculé";
  } 
private:
  // distance euclidienne de 2 points .
  inline float  r(pair<int,int> x1,pair<int,int> x2){return sqrt(r2(x1,x2));}
  // distance euclidienne de 2 points au carré.
  inline float  r2(pair<int,int> x1,pair<int,int> x2)
  {return pow(get<0>(x1)-get<0>(x2),2) + pow(get<1>(x1)-get<1>(x2),2);}
};
  
#endif