#include <vector>
#include <set>

//debug
#include <iostream>

#include <utility>

#include "include/interpolator.hpp"
#include "include/rbf.hpp"
#include "include/func.hpp"
#include "include/error.hpp"
#include "include/utils.hpp"

#include <boost/functional/hash.hpp>
#include <boost/shared_ptr.hpp>

namespace kwantix{ 
using boost::shared_ptr;

template<typename RBF>
interpolator<RBF>::interpolator()
{
  initted = false;
}

template<typename RBF>
interpolator<RBF>::interpolator(shared_ptr<linear_BVP2> bvp)
{
  //Workaround because gdb can't break inside constructors. :-(
  init(bvp);
}

template<typename RBF>
interpolator<RBF>::interpolator(shared_ptr<domain> Omega, 
                                const map<point, double>& Xi)
{
  //Check that Xi matches the domain in size
  size_t Omega_size = Omega -> get_interior().size() 
    + Omega -> get_boundary().size();
  if(Xi.size() != Omega_size)
  {
    badArgument exc;
    if(Xi.size() < Omega_size)
      exc.reason = "Did not provide enough interpolation data for every " 
        "point in the given domain.";
    else
      exc.reason = "Provided more interpolation data than points in the " 
        "given domain.";
    exc.line = __LINE__;
    exc.file = __FILE__;
    throw exc;
  }

  //Create a trivial bvp taking into account the given domain.
  shared_ptr<Id_op> Id(new Id_op);
  shared_ptr<dirichlet_op> D(new dirichlet_op);
  map<point, double> f, g;

  //Extract f and g from given information
  for(map<point, double>::const_iterator I = Xi.begin();
      I != Xi.end(); I++)
  {
    if(kwantix::contains(Omega -> get_interior(), I -> first))
      f[I -> first] = I -> second;
    else if(kwantix::contains(Omega -> get_boundary(), I -> first))
      g[I -> first] = I -> second;
    else
    {
      badArgument exc;
      exc.reason = "The interpolation data contains points not in "
        "the given domain.";
      exc.line = __LINE__;
      exc.file = __FILE__;
      throw exc;
    }
  } 
    
  shared_ptr<linear_BVP2> new_bvp(new linear_BVP2(Omega, Id, D, f,g));
    
  interpolate(new_bvp);
}

template<typename RBF>
interpolator<RBF>::interpolator(const map<point, double>& Xi)
{
  interpolate(Xi);
}

template<typename RBF>
void interpolator<RBF>::interpolate(const map<point, double>& Xi)
{
    
  if(Xi.empty()){//Dude, wtf?
    badArgument exc;
    exc.reason = "Cannot interpolate if no data is given.";
    exc.line = __LINE__;
    exc.file = __FILE__;
    throw exc;
  }

  //Create a trivial bvp.
  shared_ptr<Id_op> Id(new Id_op);
  shared_ptr<dirichlet_op> D(new dirichlet_op);
  set<point> intr;
  set<point> bdry; //empty
  map<point, point> nrml; //empty
  map<point, double> g; //empty
    
  bool dim_set = false;
  size_t dimension = 0;
  for(map<point,double>::const_iterator i = Xi.begin(); i != Xi.end(); i++){
    if(!dim_set){
      dimension = (i -> first).size();
      dim_set = true;
    }
    else if(dimension != (i -> first).size()){
      badArgument exc;
      exc.reason = "Inconformant dimensions in interpolation data.";
      exc.line = __LINE__;
      exc.file = __FILE__;
      throw exc;
    }
    intr.insert( i->first);
  }
  shared_ptr<domain> Omega(new domain(dimension, intr,bdry,nrml));
  shared_ptr<linear_BVP2> bvp(new linear_BVP2(Omega, Id, D, Xi, g));
    
  init(bvp);
}

template<typename RBF>
void interpolator<RBF>::interpolate(shared_ptr<linear_BVP2> bvp)
{
  init(bvp);
}

template<typename RBF>
void interpolator<RBF>::interpolate(const interp_values& values)
{
  if(!initted){
    badArgument exc;
    exc.reason = "Cannot interpolate with other interpolator's "
      "values without defining the domain.";
    exc.line = __LINE__;
    exc.file = __FILE__;
    throw exc;
  }
  if(rbfs_hash != values.rbfs_hash){
    badArgument exc;
    exc.reason = "Cannot interpolate with values from incompatible "
      "interpolator." ;
    exc.line = __LINE__;
    exc.file = __FILE__;
    throw exc;
  }
  coeffs = M.inv(values.v);
   
  //Precompute the relevant values
  recompute_values_vec();
}

template<typename RBF>
void interpolator<RBF>::init(shared_ptr<linear_BVP2> bvp)
{
  thebvp = bvp;

  using std::set;
    
  shared_ptr<const domain> Omega = bvp -> get_domain();
  set<point> interior = Omega -> get_interior();
  set<point> boundary = Omega -> get_boundary();
  map<point, vector> normals = Omega -> get_normals();
  n = interior.size();
  m = boundary.size();
    
  vector temp(n+m);
  coeffs = temp;
  rbfs.reserve(n+m);

    
  RBF::set_dimension(Omega -> get_dimension());
    
  set<point>::iterator I;
  //Define all the rbfs...
  for(I = interior.begin(); I != interior.end(); I++){
    RBF r(*I);
    rbfs.push_back(r);
  }
  for(I = boundary.begin(); I != boundary.end(); I++){
    RBF r(*I);
    rbfs.push_back(r);
  }
    
  //Now define the matrix to be inverted...
  matrix Mtemp(n+m,n+m);
  M = Mtemp;
  shared_ptr<const linear_diff_op2> L = thebvp -> get_linear_diff_op2(); 

  shared_ptr<const bdry_diff_op> B = thebvp -> get_bdry_diff_op();

  I = interior.begin();
  for(size_t i = 1; i <= n; i++){
    for(size_t j = 1; j <= n+m; j++)
      M(i,j) = L -> at(rbfs[j-1], *I);
    I++;
  }
    
  map<point, vector>::iterator J;
  J = normals.begin();
  for(size_t i = n+1; i <= n+m; i++){
    for(size_t j = 1; j <= n+m; j++)
      M(i,j) = B -> at(rbfs[j-1], J->first, J->second);
    J++;
  }
   
  computecoeffs();
    
  ev_precomp = false;
  d1_precomp = false;
  d2_precomp = false;

  rbfs_hash = hash_value(rbfs);

  kwantix::normals nrmls_tmp(rbfs_hash, normals, n);  
  nrmls = nrmls_tmp;

  initted = true;
}

template<typename RBF>
size_t interpolator<RBF>::hash_value(const std::vector<RBF>& rbfs_in)
{
  size_t seed = 0;  

  for(size_t i = 0; i < rbfs_in.size(); i++)
    boost::hash_combine(seed,rbfs[i]);

  return seed;
}
  
// ******************** Evaluations, derivatives ********************
 
//  ---------- Precomputation ---------------------------
  
template<typename RBF>
void interpolator<RBF>::precompute_ev()
{
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }

  if(!ev_precomp){
    std::vector<size_t> alpha; //empty vector
      
    //Precompute the RBFs at all points of the domain
    matrix phis(n+m,n+m);
    set<point>::iterator I;
    size_t i;
    for(I = thebvp -> get_domain () -> get_interior().begin(), i = 1; 
        i <= n; i++, I++)
      for(size_t j = 0; j < n+m; j++)
        phis(i,j+1) = rbfs[j].at(*I);
      
    for(I = thebvp -> get_domain() -> get_boundary().begin(), i=n+1; 
        i <= n+m; i++, I++)
      for(size_t j = 0; j < n+m; j++)
        phis(i,j+1) = rbfs[j].at(*I);
      
    precomp_rbfs[alpha] = phis;
    computecoeffs();
      
    ev_precomp = true;
  }
}

template<typename RBF>
void interpolator<RBF>::precompute_d1()
{
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }

  //Have the RBFs been precomputed yet?
  if(!d1_precomp){
    //No, so precompute the RBFs first derivatives at all points of
    //the domain.
    for(size_t k = 1; k <= thebvp -> get_domain() -> get_dimension(); 
        k++){
      std::vector<size_t> alpha(k); alpha[k-1]++;
      matrix phis(n+m,n+m);
      {
        set<point>::iterator I;
        size_t i;
        for(I = thebvp -> get_domain() -> get_interior().begin(), 
              i = 1; i <= n; i++, I++)
          for(size_t j = 0; j < n+m; j++)
            phis(i,j+1) = rbfs[j].d(*I,k);
      
        for(I = thebvp -> get_domain() -> get_boundary().begin(), 
              i=n+1; i <= n+m; i++, I++)
          for(size_t j = 0; j < n+m; j++)
            phis(i,j+1) = rbfs[j].d(*I,k);
      }

      precomp_rbfs[alpha] = phis;
      computecoeffs();
    }
  }
  d1_precomp = true;
}

template<typename RBF>
void interpolator<RBF>::precompute_d2()
{
  if(!initted)
    throw not_initted(__LINE__, __FILE__);
  if(!d2_precomp){
    size_t dim = thebvp -> get_domain() -> get_dimension();
    //precompute all second derivatives
    for(size_t k1 = 1; k1 <= dim; k1++){
      for(size_t k2 = k1; k2 <= dim; k2++){
        std::vector<size_t> alpha(k1>k2?k1:k2); alpha[k1-1]++; alpha[k2-1]++;
        matrix phis(n+m,n+m);
        set<point>::iterator I;
        size_t i;
        for(I = thebvp -> get_domain () -> get_interior().begin(), i = 1; 
            i <= n; i++, I++)
          for(size_t j = 0; j < n+m; j++)
            phis(i,j+1) = rbfs[j].d2(*I,k1,k2);
      
        for(I = thebvp -> get_domain() -> get_boundary().begin(), i=n+1; 
            i <= n+m; i++, I++)
          for(size_t j = 0; j < n+m; j++)
            phis(i,j+1) = rbfs[j].d2(*I,k1,k2);
      
        precomp_rbfs[alpha] = phis;
        computecoeffs();	  
      }
    }
  }
  d2_precomp = true;
}

// ----------------- Point evaluation ---------------------------

template<typename RBF>
double interpolator<RBF>::operator()(const point& p) const
{
  return at(p);
}

template<typename RBF>
double interpolator<RBF>::at(const point& p) const
{
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
  std::vector<size_t> alpha; //empty vector

  //Are we evaluating at a precomputed point in the domain?
  if(ev_precomp and 
     precomp_values[alpha].find(p) != precomp_values[alpha].end())
    return precomp_values[alpha][p];
     
  //Else, must compute the value
  double result = 0;
  for(size_t i = 1; i <= coeffs.size(); i++)
    result += coeffs(i)*rbfs[i-1].at(p);
        
  return result;
}

template<typename RBF>
double interpolator<RBF>::d(const point& p, size_t k) const{
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
  std::vector<size_t> alpha(k); alpha[k-1]++;
    
  //Are we evaluating at a precomputed point in the domain?
  if(d1_precomp and 
     precomp_values[alpha].find(p) != precomp_values[alpha].end())
    return precomp_values[alpha][p];
    
  //Else, must compute the value
  double result = 0;
  for(size_t i = 1; i <= coeffs.size(); i++)
    result += coeffs(i)*rbfs[i-1].d(p,k);
   
  return result;
}

template<typename RBF>
double interpolator<RBF>::d2(const point& p, size_t k1, size_t k2) const{
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
  std::vector<size_t> alpha(k1>k2?k1:k2); alpha[k1-1]++; alpha[k2-1]++;
    
  //Are we evaluating at a precomputed point in the domain?
  if(d2_precomp and 
     precomp_values[alpha].find(p) != precomp_values[alpha].end())
    return precomp_values[alpha][p];
    
  //Else, must compute the value
  double result = 0;
  for(size_t i = 1; i <= coeffs.size(); i++)
    result += coeffs(i)*rbfs[i-1].d2(p,k1,k2);

  return result;
}

// ------------------- Whole domain evaluations ----------------

template<typename RBF>
kwantix::badArgument
interpolator<RBF>::not_precomputed(int line, string file) const
{
  badArgument exc;
  exc.reason = "Cannot do whole domain evaluations without "
    "precomputed RBFs.";
  exc.line = line;
  exc.file = file;
  return exc;
}

template<typename RBF>
interp_values interpolator<RBF>::operator()() const
{
  return at();
}
  
template<typename RBF>
interp_values interpolator<RBF>::at() const
{
  if(!ev_precomp)
    throw not_precomputed(__LINE__, __FILE__);
     
  std::vector<size_t> alpha;
  return interp_values(precomp_values_vec[alpha],rbfs_hash,n,m);
}
   
template<typename RBF>
interp_values interpolator<RBF>::d(size_t k) const
{
  if(!d1_precomp)
    throw not_precomputed(__LINE__, __FILE__);
    
  std::vector<size_t> alpha(k); alpha[k-1]++;
  return interp_values(precomp_values_vec[alpha],rbfs_hash,n,m);
}

template<typename RBF>
interp_values interpolator<RBF>::d2(size_t k1, size_t k2) const
{
  if(!d2_precomp)
    throw not_precomputed(__LINE__, __FILE__);
    
  std::vector<size_t> alpha(k1>k2?k1:k2); alpha[k1-1]++; alpha[k2-1]++;
  return interp_values(precomp_values_vec[alpha],rbfs_hash,n,m);
}

//***************** linear operations ***************************

template<typename RBF>
interpolator<RBF> interpolator<RBF>::
operator+(const interpolator<RBF>& u) const
{    
  if(this -> rbfs_hash != u.rbfs_hash){
    badArgument exc;
    exc.reason = 
      "Cannot add interpolators on different domains (interior and boundary must match)."; 
    exc.line = __LINE__;
    exc.file = __FILE__;
    throw exc;	
  }
    
  interpolator<RBF> out = *this;
  out.coeffs = (this -> coeffs) + u.coeffs;
  out.recompute_values_vec();
       
  return out;
}

template<typename RBF>
interpolator<RBF> interpolator<RBF>::
operator-(const interpolator<RBF>& u) const
{    
  if(this -> rbfs_hash != u.rbfs_hash){
    badArgument exc;
    exc.reason = 
      "Cannot subtract interpolators on different domains (interior and boundary must match)."; 
    exc.line = __LINE__;
    exc.file = __FILE__;
    throw exc;	
  }

  interpolator<RBF> out = *this;
  out.coeffs = (this -> coeffs) - u.coeffs;
  out.recompute_values_vec();
       
  return out;
}
  
template<typename RBF>
interpolator<RBF> interpolator<RBF>::operator*(double a) const
{
  interpolator<RBF> u = *this;
  u.coeffs = (this -> coeffs)*a;
  u.recompute_values_vec();
  return u;
}

template<typename RBF>
interpolator<RBF> interpolator<RBF>::operator/(double a) const
{
  interpolator<RBF> u = *this;
  u.coeffs = (this -> coeffs)*(1/a);
  u.recompute_values_vec();
  return u;
}

template<typename RBF>
normals interpolator<RBF>::get_normals() const{
  if(!initted){
    throw badArgument("Interpolator not initialised, so there are "
                      "no normals!",__FILE__,__LINE__);
  }
  return nrmls;
}
  
//******************* data manipulation **************************

template<typename RBF>
void interpolator<RBF>::set_f(const intr_values& f){
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }

  std::vector<size_t> alpha;
  slice s(1,n);
  precomp_values_vec[alpha](s) = f.v;
  //precomp_values[alpha] = valsvec2map(precomp_values_vec[alpha]);

  computecoeffs(true);
}

template<typename RBF>
void interpolator<RBF>::set_g(const bdry_values& g){
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
    
  std::vector<size_t> alpha;
  slice s(n+1,n+m);
  precomp_values_vec[alpha](s) = g.v;
  //precomp_values[alpha] = valsvec2map(precomp_values_vec[alpha]);
    
  computecoeffs(true);
}

template<typename RBF>
void interpolator<RBF>::set_f(const realfunc& f){
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
  thebvp -> set_f(f);
  computecoeffs();
}

template<typename RBF>
void interpolator<RBF>::set_g(const realfunc& g)
{
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
  thebvp -> set_g(g);
  computecoeffs();
}

template<typename RBF>
void interpolator<RBF>::set_f(const map<point, double>& f)
{
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
  thebvp -> set_f(f);
  computecoeffs();
}

template<typename RBF>
void interpolator<RBF>::set_g(const map<point, double>& g)
{
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
  thebvp -> set_g(g);
  computecoeffs();
}

template<typename RBF>
void interpolator<RBF>::set_bdry_values(const bdry_values& b_new){
  if(!initted){
    throw not_initted(__LINE__, __FILE__);
  }
  std::vector<size_t> alpha; //empty, corresponds to evaluation
  if(precomp_values_vec.find(alpha) == precomp_values_vec.end())
    at();
  kwantix::vector rhs = precomp_values_vec[alpha];
  slice s1(1,n), s2(n+1,n+m);
  rhs(s2) = b_new.v;
  coeffs = precomp_rbfs[alpha].inv(rhs);
    
  recompute_values_vec();
}

template<typename RBF>
void interpolator<RBF>::recompute_values_vec(){
  typename 
    map<std::vector<size_t>, matrix>::const_iterator I;
  for(I = precomp_rbfs.begin(); I != precomp_rbfs.end(); I++)
    precomp_values_vec[I -> first] = (I -> second)*coeffs;
      
  //Pointwise map evaluations we will not compute again.
  precomp_values.clear();
}

template<typename RBF>
kwantix::badArgument
interpolator<RBF>::not_initted(int line, string file) const
{
  badArgument exc;
  exc.reason = 
    "Interpolator can't interpolate without initialisation data.";
  exc.line = line;
  exc.file = file;
  return exc;
}

template<typename RBF>
void interpolator<RBF>::computecoeffs(bool rhs_defined)
{
  kwantix::vector rhs(n+m);

  //Compute the RBF coefficients
  if(rhs_defined){
    std::vector<size_t> alpha;
    rhs = precomp_values_vec[alpha];
  }
  else
  {
    map<point, double>::const_iterator I;

    I = (thebvp -> get_f()).begin();
    for(size_t i = 1; i <= n; i++){
      rhs(i) = I->second;
      I++;
    }
    I = (thebvp -> get_g()).begin();   
    for(size_t i = n+1; i <= n+m; i++){
      rhs(i) = I->second;
      I++;
    }
  }
  coeffs = M.inv(rhs);
    
  //Precompute the values for the derivatives and evaluations that
  //have already been computed.
  {				
    typename map<std::vector<size_t>, matrix>::const_iterator I;
      
    for(I = precomp_rbfs.begin(); I != precomp_rbfs.end(); I++){
      kwantix::vector vals = (I->second)*coeffs;
      precomp_values_vec[I -> first] = vals;
      precomp_values[I -> first] = valsvec2map(vals);
    }
  }
}

template<typename RBF>
map<point, double> interpolator<RBF>::
valsvec2map(const kwantix::vector& values) const
{
  map<point, double> out;
  {
    set<point>::const_iterator I;
    size_t i;
    for(I = thebvp -> get_domain() -> get_interior().begin(), i = 1;
        i <= n; I++, i++)
      out[*I] = values(i);
      
    for(I = thebvp -> get_domain() -> get_boundary().begin(), i = n+1;
        i <= n+m; I++, i++)
      out[*I] = values(i);
  }
  return out;
}
  
//****************** I/O *************************************
  
template<typename RBF>
void interpolator<RBF>::into_os(std::ostream& os) const
{
  kwantix::vector values;
  size_t i;
  set<point>::const_iterator I;
    
  //If we already have precomputed the values...
  std::vector<size_t> alpha;
  if(precomp_values_vec.count(alpha)){
    values = precomp_values_vec[alpha];
  }
  else{
    kwantix::vector tmp(n+m);
    for(i = 1, I=thebvp->get_domain()->get_interior().begin() ; 
        i<=n; i++, I++)
    {
      tmp(i) = at(*I);
    }

    for(i = n+1, I=thebvp->get_domain()->get_boundary().begin() ; 
        i<=n+m; i++, I++)
    {
      tmp(i) = at(*I);
    }

    values = tmp;
  }
    
  for(i = 1, I=thebvp->get_domain()->get_interior().begin() ; 
      i<=n; i++, I++)
  {
    os << *I << " " << values(i) << std::endl;
  }

  for(i = n+1, I=thebvp->get_domain()->get_boundary().begin() ; 
      i<=n+m; i++, I++)
  {
    os << *I << " " << values(i) << std::endl;
  }
 
}

//Instantiations
template class interpolator<piecewise_polynomial>;
template class interpolator<thin_plate_spline>;
template class interpolator<multiquadric>;
template class interpolator<inverse_multiquadric>;
template class interpolator<inverse_quadratic>;
template class interpolator<gaussian>;


}

/*! \mainpage Outline of the RBF-DDM code

  Thank you for your interest in my code. Above you can browse the
  structure of my code. You can also read here a technical design
  document that also outlines certain difficulties I have been facing
  with it:

  http://inversethought.com/jordi/design.pdf
*/