#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 bvp{ 
  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(utils::contains(Omega -> get_interior(), I -> first))
	f[I -> first] = I -> second;
      else if(utils::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 namespace linalg;
    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);

    bvp::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>
  error_handling::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 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();
    linalg::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>
  error_handling::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()
  {
    linalg::vector rhs(n+m);

    //Compute the RBF coefficients
    {
      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++){
	linalg::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 linalg::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;
  }
  


  //Instantiations
  using namespace rbf;
  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>;


}