#ifndef RBFGENERATOR
#define RBFGENERATOR

#include "PointSet.h"
#include "EvaluationTreeD.h"
#include "BasisFunction.h"
#include "AxisKdTree.h"
#include "SVD.h"
#include "PBCG.h"

class RbfGenerator{
public:
  EvaluationTreeD* _func;
  float _smooth;
  float _error;
  float _diagonal;
  
private:
  AxisKdTree* tree;
  PointSet* ps;
  float support;
  BasisFunction** bfs;
  int bfN;
  int level;
  unsigned long total;

public:
  RbfGenerator(float base, float min[3], float max[3]){
    _func = new EvaluationTreeD(min, max);
    _func->out = base;
    _smooth = 0;
    _error = 0;
  }
  
  void addPointSet(PointSet* ps, float support, int level){
    this->ps = ps;
    this->support = support;
    this->level = level;
    
    if(_error != 0){
      reducePoints();
      ps = this->ps;
    }
    else{
      computePreviousValues(); 
    }
    
    tree = new AxisKdTree(ps);
    bfN = ps->_pointN;
    cout << bfN << " basis functions" << endl;
    bfs = new BasisFunction*[bfN];
    
    cout << "Local fitting" << endl;
    localFit();
    cout << "Global fitting" << endl;
    globalFit();
    
    delete tree;
    delete ps;
    
    _func->addBFS(bfs, bfN);
  }
  
  void computePreviousValues(){
    int N = ps->_pointN;
    
    float (*point)[3] = ps->_point;
    for(int i=0; i<N; i++){
      float* p = point[i];
      ps->setValue(i,  _func->value(p[0], p[1], p[2]));
    }
  }
  
  void reducePoints(){
    tree = new AxisKdTree(ps);
    
    int N = ps->_pointN;
    
    float* err = new float[N];
    
    float (*point)[3] = ps->_point;
    float (*normal)[3] = ps->_normal;
    
    int i;
    for(i=0; i<N; i++){
      float* p = point[i];
      float g[3];
      float f = _func->valueAndGradient(g, p[0], p[1], p[2]);
      ps->setValue(i, f);
      double len = sqrt(g[0]*g[0] + g[1]*g[1] + g[2]*g[2]);
      
      if((float)len != 0)
        err[i] = (float)(fabs(f)/len);
      else
        err[i] = (float)fabs(f);
    }
    
    bool* used = new bool[N];
    int count = 0;
    int listN, *list;
    for(i=0; i<N; i++){
      used[i] = false;
      tree->collectPointIndexInSphere(list, listN, point[i], support);
      for(int j=0; j<listN; j++){
        if(err[list[j]] > _error){
          used[i] = true;
          count++;
          break;
        }
      }
    }
    delete[] err;
    
    PointSet* qs = new PointSet;
    qs->setPointSize(count);
    count = 0;
    for(i=0; i<N; i++){
      if(used[i]){
        float* p = point[i];
        qs->setPoint(count, p[0], p[1], p[2]);
        p = normal[i];
        qs->setNormal(count, p[0], p[1], p[2]);
        qs->setValue(count, ps->_value[i]);
        count++;
      }
    }
    
    delete ps;
    delete tree;
    ps = qs;
    delete[] used;
  }
  
  void localFit(){
    int i,j;
    total = 0;
	float (*point)[3] = ps->_point;
    float (*normal)[3] = ps->_normal;
	for(i=0; i<bfN; i++){
      float *c = point[i];
      float *n = normal[i];
	  BasisFunction* bf = bfs[i] = new BasisFunction;
      bf->centerX = c[0];
      bf->centerY = c[1];
      bf->centerZ = c[2];
      bf->cX = -n[0];
      bf->cY = -n[1];
      bf->cZ = -n[2];
      bf->c0 = 0;
      bf->support = support;
      bf->level = level;
      bf->cXX = bf->cYY = bf->cZZ = bf->cXY = bf->cYZ = bf->cZX = 0;
	}
	return;

    //For SVD
    float **A = new float*[7];
    float b[7];
    float w[7];
    float x[7];
    float **v = new float*[7];
    for(i=1; i<7; i++){
      A[i] = new float[7];
      v[i] = new float[7];
    }
    
    int listN, *list;
    
    
    //float (*full_point)[3] = _full_ps->_point;
    //float *value =  _full_ps->_value;
    
    for(i=0; i<bfN; i++){
      float *c = point[i];
      float *n = normal[i];
      
      BasisFunction* bf = bfs[i] = new BasisFunction;
      bf->centerX = c[0];
      bf->centerY = c[1];
      bf->centerZ = c[2];
      bf->cX = n[0];
      bf->cY = n[1];
      bf->cZ = n[2];
      bf->c0 = 0;
      bf->support = support;
      bf->level = level;
      
      if(n[0]*n[0] + n[1]*n[1] + n[2]*n[2] < 0.000000001){ // || listN < 7){
        bf->cXX = bf->cYY = bf->cZZ = bf->cXY = bf->cYZ = bf->cZX = 0;
        continue;
      }
      
      float s = support;
      while(true){
        tree->collectPointIndexInSphere(list, listN, c, s);
        //total += listN;
        if(listN > 5)
          break;
        s += 0.1f*support;
      }
	  total += listN;
      
      //local coordinate system
      float t1[3], t2[3];
      if(fabs(n[0]) < fabs(n[1])){
        double l = sqrt(n[1]*n[1] + n[2]*n[2]);
        t1[0] = 0;
        t1[1] = -(float)(n[2]/l);
        t1[2] = (float)(n[1]/l);
      }
      else{
        double l = sqrt(n[0]*n[0] + n[2]*n[2]);
        t1[0] = (float)(n[2]/l);
        t1[1] = 0;
        t1[2] = -(float)(n[0]/l);
      }
      t2[0] = n[1]*t1[2] - n[2]*t1[1];
      t2[1] = n[2]*t1[0] - n[0]*t1[2];
      t2[2] = n[0]*t1[1] - n[1]*t1[0];
      
      //construct matrix
      for(j=1; j<7; j++){
        A[j][1] = A[j][2] = A[j][3] = A[j][4] = A[j][5] = A[j][6] = 0;
        b[j] = 0;
      }
      for(j=0; j<listN; j++){
        int in = list[j];
        float* p = point[in];
        
        float vx = p[0] - c[0];
        float vy = p[1] - c[1];
        float vz = p[2] - c[2];
        float W = (float)BasisFunction::weight(sqrt(vx*vx+vy*vy+vz*vz), s);
        
        float u = t1[0]*vx + t1[1]*vy + t1[2]*vz;
        float v = t2[0]*vx + t2[1]*vy + t2[2]*vz;
        float gW = W*(n[0]*vx + n[1]*vy + n[2]*vz);
        
        float tmp[6];
        tmp[0] = W*u*u;
        tmp[1] = W*u*v;
        tmp[2] = W*v*v;
        tmp[3] = W*u;
        tmp[4] = W*v;
        tmp[5] = W;
        
        for(int k=0; k<6; k++){
          for(int l=0; l<6; l++)
            A[k+1][l+1] += tmp[k]*tmp[l];
          b[k+1] += tmp[k]*gW;
        }
      }
      
      //solve SVD
      int k;
      SVD::svdcmp(A, 6, 6, w, v);
      float wmax=0.0f;
      for (k=1;k<=6;k++)
        if (fabs(w[k]) > wmax) wmax=(float)fabs(w[k]);
      
      float cuu, cuv, cvv, cu, cv, c0;
      if(wmax < 0.000000000001f){
        cuu = cuv = cvv = cu = cv = c0 = 0;
      }
      else{
	float wmin=wmax*0.001f;
	for (k=1;k<7;k++){
          if (fabs(w[k]) < wmin) 
            w[k]=0.0;
        }
        SVD::svbksb(A, w, v, 6, 6, b, x);
        
        cuu = x[1];
        cuv = x[2];
        cvv = x[3];
        cu = x[4];
        cv = x[5];
        c0 = x[6];
      }
      
      
      //convert into world coordinates (u, v, w) -> (x, y, z)
      bf->cXX = -(cuu*t1[0]*t1[0] + cvv*t2[0]*t2[0] + cuv*t1[0]*t2[0]);
      bf->cYY = -(cuu*t1[1]*t1[1] + cvv*t2[1]*t2[1] + cuv*t1[1]*t2[1]);
      bf->cZZ = -(cuu*t1[2]*t1[2] + cvv*t2[2]*t2[2] + cuv*t1[2]*t2[2]);
      
      bf->cXY = -(2.0f*(cuu*t1[0]*t1[1] + cvv*t2[0]*t2[1]) + cuv*(t1[0]*t2[1] + t1[1]*t2[0]));
      bf->cYZ = -(2.0f*(cuu*t1[1]*t1[2] + cvv*t2[1]*t2[2]) + cuv*(t1[1]*t2[2] + t1[2]*t2[1]));
      bf->cZX = -(2.0f*(cuu*t1[2]*t1[0] + cvv*t2[2]*t2[0]) + cuv*(t1[2]*t2[0] + t1[0]*t2[2]));
      
      bf->cX = n[0] - cu*t1[0] - cv*t2[0];
      bf->cY = n[1] - cu*t1[1] - cv*t2[1];
      bf->cZ = n[2] - cu*t1[2] - cv*t2[2];
      
      bf->c0 = -c0;
    }
    
    //free
    for(i=1; i<7; i++){
      delete[] A[i];
      delete[] v[i];
    }
    delete[] v;
    delete[] A;

	/*
    //For SVD
    float **A = new float*[4];
    float b[4];
    float w[4];
    float x[4];
    float **v = new float*[4];
    for(i=1; i<4; i++){
      A[i] = new float[4];
      v[i] = new float[4];
    }
    
    int listN, *list;
    float (*point)[3] = ps->_point;
    float (*normal)[3] = ps->_normal;
    for(i=0; i<bfN; i++){
      float *c = point[i];
      float *n = normal[i];
      
      BasisFunction* bf = bfs[i] = new BasisFunction;
      bf->centerX = c[0];
      bf->centerY = c[1];
      bf->centerZ = c[2];
      bf->cX = n[0];
      bf->cY = n[1];
      bf->cZ = n[2];
      bf->c0 = 0;
      bf->support = support;
      bf->level = level;
      
      tree->collectPointIndexInSphere(list, listN, c, support);
      total += listN;
      
      if(n[0]*n[0] + n[1]*n[1] + n[2]*n[2] < 0.000000001 || listN < 3){
        bf->cXX = bf->cYY = bf->cZZ = bf->cXY = bf->cYZ = bf->cZX = 0;
        continue;
      }
      
      //local coordinate system
      float t1[3], t2[3];
      if(fabs(n[0]) < fabs(n[1])){
        double l = sqrt(n[1]*n[1] + n[2]*n[2]);
        t1[0] = 0;
        t1[1] = -(float)(n[2]/l);
        t1[2] = (float)(n[1]/l);
      }
      else{
        double l = sqrt(n[0]*n[0] + n[2]*n[2]);
        t1[0] = (float)(n[2]/l);
        t1[1] = 0;
        t1[2] = -(float)(n[0]/l);
      }
      t2[0] = n[1]*t1[2] - n[2]*t1[1];
      t2[1] = n[2]*t1[0] - n[0]*t1[2];
      t2[2] = n[0]*t1[1] - n[1]*t1[0];
      
      //construct matrix
      for(j=1; j<4; j++){
        A[j][1] = A[j][2] = A[j][3] = 0;
        b[j] = 0;
      }
      for(j=0; j<listN; j++){
        int in = list[j];
        if(i == in)
          continue;
        float* p = point[in];
        
        float vx = p[0] - c[0];
        float vy = p[1] - c[1];
        float vz = p[2] - c[2];
        float W = (float)BasisFunction::weight(sqrt(vx*vx+vy*vy+vz*vz), support);
        
        float u = t1[0]*vx + t1[1]*vy + t1[2]*vz;
        float v = t2[0]*vx + t2[1]*vy + t2[2]*vz;
        float gW = W*(n[0]*vx + n[1]*vy + n[2]*vz);
        
        float tmp[3];
        tmp[0] = W*u*u;
        tmp[1] = W*u*v;
        tmp[2] = W*v*v;
        
        for(int k=0; k<3; k++){
          for(int l=0; l<3; l++)
            A[k+1][l+1] += tmp[k]*tmp[l];
          b[k+1] += tmp[k]*gW;
        }
      }
      
      //solve SVD
      int k;
      SVD::svdcmp(A, 3, 3, w, v);
      float wmax=0.0f;
      for (k=1;k<=3;k++)
        if (fabs(w[k]) > wmax) wmax=(float)fabs(w[k]);
      
      float cuu, cuv, cvv;
      if(wmax < 0.000000000001f){
        cuu = cuv = cvv = 0;
      }
      else{
	float wmin=wmax*0.01f;
	for (k=1;k<=3;k++){
          if (fabs(w[k]) < wmin) 
            w[k]=0.0;
        }
        SVD::svbksb(A, w, v, 3, 3, b, x);
        
        cuu = x[1];
        cuv = x[2];
        cvv = x[3];
      }
      
      
      //convert into world coordinates (u, v, w) -> (x, y, z)
      bf->cXX = -(cuu*t1[0]*t1[0] + cvv*t2[0]*t2[0] + cuv*t1[0]*t2[0]);
      bf->cYY = -(cuu*t1[1]*t1[1] + cvv*t2[1]*t2[1] + cuv*t1[1]*t2[1]);
      bf->cZZ = -(cuu*t1[2]*t1[2] + cvv*t2[2]*t2[2] + cuv*t1[2]*t2[2]);
      
      bf->cXY = -(2.0f*(cuu*t1[0]*t1[1] + cvv*t2[0]*t2[1]) + cuv*(t1[0]*t2[1] + t1[1]*t2[0]));
      bf->cYZ = -(2.0f*(cuu*t1[1]*t1[2] + cvv*t2[1]*t2[2]) + cuv*(t1[1]*t2[2] + t1[2]*t2[1]));
      bf->cZX = -(2.0f*(cuu*t1[2]*t1[0] + cvv*t2[2]*t2[0]) + cuv*(t1[2]*t2[0] + t1[0]*t2[2]));
    }
    
    //free
    for(i=1; i<4; i++){
      delete[] A[i];
      delete[] v[i];
    }
    delete[] v;
    delete[] A;*/
  }
  
  void globalFit(){
    int i,j;
    float (*point)[3] = ps->_point;
    
    total = 0;
    int listN, *list;
	for(i=0; i<bfN; i++){
      float *c = point[i];
      tree->collectPointIndexInSphere(list, listN, c, support);
	  total += listN;
	}


    cout << "Non-zero element in the matrix; " << total << endl;
    
    PBCG solver;
    double *sa = solver.sa = new double[total+2];
    unsigned long *ija = solver.ija = new unsigned long[total+2];
    double *b = new double[bfN+1];
    double* sol = new double[bfN+1];
    ija[1]=bfN+2;
    int kk=bfN+1;
    
    for(i=0; i<bfN; i++){
	  sa[i+1] = BasisFunction::weight(0, support) + _smooth;
      b[i+1] = sol[i+1] = -ps->_value[i];
      
      float *c = point[i];
      tree->collectPointIndexInSphere(list, listN, c, support);
      int* t = new int[listN-1];
      int m = 0;
      for(j=0; j<listN; j++){
        if(list[j] == i)
          continue;
        t[m++] = list[j];
      }
      
      for(j=0; j<m; j++){
        for(int k=j+1; k<m; k++){
          if(t[j] > t[k]){
            int tmp = t[j];
            t[j] = t[k];
            t[k] = tmp;
          }
        }
      }
      
      for(j=0; j<m; j++){
        int k = t[j];
        
        float *q = point[k];
        float vx = c[0] - q[0];
        float vy = c[1] - q[1];
        float vz = c[2] - q[2];
        
        double w = BasisFunction::weight(sqrt(vx*vx+vy*vy+vz*vz), support);
        b[i+1] -= w*bfs[k]->poly(vx, vy, vz);
        
        kk++;
        sa[kk] = w;
        ija[kk] = k+1;
      }
      ija[i+2]=kk+1;
      
      if(m != 0)
        delete t;
    }
    
    //sovle 
    int iter;
    double err;
    solver.diagonal = _diagonal;
    solver.linbcg((unsigned long)bfN, b, sol, 1, 1e-5*_diagonal, 1000*level, &iter, &err);
    delete[] b;
    
    for(i=0; i<bfN; i++){
      bfs[i]->c0 += (float)sol[i+1];
    }
    delete[] sol;
  }
};

#endif