#ifndef BASISFUNCTION
#define BASISFUNCTION

#include <math.h>

class BasisFunction{
  
public:
  float centerX, centerY, centerZ;
  float cXX, cYY, cZZ, cXY, cYZ, cZX, cX, cY, cZ, c0;
  float support;
  int level;
  
  double value(float x, float y, float z){
    float vx = x - centerX;
    float vy = y - centerY;
    float vz = z - centerZ;
    
    double d = vx*vx+vy*vy+vz*vz;
    if(d > support*support)
      return 0;
    double w = weight(sqrt(d), support);
    
    return w*poly(vx, vy, vz);
  }
  
  void gradient(double g[3], float x, float y, float z){
    float vx = x - centerX;
    float vy = y - centerY;
    float vz = z - centerZ;
    
    double d = sqrt(vx*vx+vy*vy+vz*vz);
    if(d > support){
      g[0] = g[1] = g[2] = 0;
      return;
    }
    double w = weight(d, support);
    double gw[3];
    weightG(gw, d, support, vx, vy, vz);
    
    double f = poly(vx, vy, vz);
    double gf[3];
    polyG(gf, vx, vy, vz);
    
    g[0] = gw[0]*f + w*gf[0];
    g[1] = gw[1]*f + w*gf[1];
    g[2] = gw[2]*f + w*gf[2];
  }
  
  void gradient2(double gg[3][3], float x, float y, float z){
    float vx = x - centerX;
    float vy = y - centerY;
    float vz = z - centerZ;
    
    double d = sqrt(vx*vx+vy*vy+vz*vz);
    if(d > support){
      gg[0][0] = gg[0][1] = gg[0][2] =
      gg[1][0] = gg[1][1] = gg[1][2] =
      gg[2][0] = gg[2][1] = gg[2][2] = 0;
      return;
    }
    
    double w = weight(d, support);
    double gw[3];
    weightG(gw, d, support, vx, vy, vz);
    double ggw[3][3];
    weightGG(ggw, d, support, vx, vy, vz);
    
    double f = poly(vx, vy, vz);
    double gf[3];
    polyG(gf, vx, vy, vz);
    double ggf[3][3];
    polyGG(ggf, vx, vy, vz);
    
    gg[0][0] = ggf[0][0]*w + 2.0*gf[0]*gw[0] + f*ggw[0][0];
    gg[0][1] = ggf[0][1]*w + gf[0]*gw[1] + gf[1]*gw[0] + f*ggw[0][1];
    gg[0][2] = ggf[0][2]*w + gf[0]*gw[2] + gf[2]*gw[0] + f*ggw[0][2];
    
    gg[1][0] = gg[0][1];
    gg[1][1] = ggf[1][1]*w + 2.0*gf[1]*gw[1] + f*ggw[1][1];
    gg[2][1] = ggf[1][2]*w + gf[1]*gw[2] + gf[2]*gw[1] + f*ggw[1][2];
    
    gg[2][0] = gg[0][2];
    gg[2][1] = gg[1][2];
    gg[2][2] = ggf[2][2]*w + 2.0*gf[2]*gw[2] + f*ggw[2][2];
  }
  
  double valueAndGradient(double g[3], float x, float y, float z){
    float vx = x - centerX;
    float vy = y - centerY;
    float vz = z - centerZ;
    
    double d = sqrt(vx*vx+vy*vy+vz*vz);
    if(d > support){
      g[0] = g[1] = g[2] = 0;
      return 0;
    }
    
    double w = weight(d, support);
    double gw[3];
    weightG(gw, d, support, vx, vy, vz);
    
    double f = poly(vx, vy, vz);
    double gf[3];
    polyG(gf, vx, vy, vz);
    
    g[0] = gw[0]*f + w*gf[0];
    g[1] = gw[1]*f + w*gf[1];
    g[2] = gw[2]*f + w*gf[2];
    
    return w*f;
  }
  
  double valueAndGradient12(double g[3], double gg[3][3], float x, float y, float z){
    float vx = x - centerX;
    float vy = y - centerY;
    float vz = z - centerZ;
    
    double d = sqrt(vx*vx+vy*vy+vz*vz);
    if(d > support){
      g[0] = g[1] = g[2] = 
      gg[0][0] = gg[0][1] = gg[0][2] =
      gg[1][0] = gg[1][1] = gg[1][2] =
      gg[2][0] = gg[2][1] = gg[2][2] = 0;
      return 0;
    }
    else if(d == 0){
      g[0] = g[1] = g[2] = 
      gg[0][0] = gg[0][1] = gg[0][2] =
      gg[1][0] = gg[1][1] = gg[1][2] =
      gg[2][0] = gg[2][1] = gg[2][2] = 0;
      return poly(vx, vy, vz);
    }
    
    double w = weight(d, support);
	
    double gw[3];
    weightG(gw, d, support, vx, vy, vz);
    double ggw[3][3];
    weightGG(ggw, d, support, vx, vy, vz);
    
    double f = poly(vx, vy, vz);
    double gf[3];
    polyG(gf, vx, vy, vz);
    double ggf[3][3];
    polyGG(ggf, vx, vy, vz);
    
	
    gg[0][0] = ggf[0][0]*w + 2.0*gf[0]*gw[0] + f*ggw[0][0];
    gg[0][1] = ggf[0][1]*w + gf[0]*gw[1] + gf[1]*gw[0] + f*ggw[0][1];
    gg[0][2] = ggf[0][2]*w + gf[0]*gw[2] + gf[2]*gw[0] + f*ggw[0][2];
    
    gg[1][0] = gg[0][1];
    gg[1][1] = ggf[1][1]*w + 2.0*gf[1]*gw[1] + f*ggw[1][1];
    gg[1][2] = ggf[1][2]*w + gf[1]*gw[2] + gf[2]*gw[1] + f*ggw[1][2];
    
    gg[2][0] = gg[0][2];
    gg[2][1] = gg[1][2];
    gg[2][2] = ggf[2][2]*w + 2.0*gf[2]*gw[2] + f*ggw[2][2];
    
    g[0] = gw[0]*f + w*gf[0];
    g[1] = gw[1]*f + w*gf[1];
    g[2] = gw[2]*f + w*gf[2];
    
    return w*f;
  }
  
  inline double poly(float vx, float vy, float vz){
    return (cXX*vx+cXY*vy+cX)*vx +
           (cYY*vy+cYZ*vz+cY)*vy +
           (cZZ*vz+cZX*vx+cZ)*vz + c0;
  }
  
  inline void polyG(double g[3], float vx, float vy, float vz){
    g[0] = 2.0*cXX*vx + cXY*vy + cZX*vz + cX;
    g[1] = 2.0*cYY*vy + cYZ*vz + cXY*vx + cY;
    g[2] = 2.0*cZZ*vz + cZX*vx + cYZ*vy + cZ;
  }
  
  inline void polyGG(double gg[3][3], float vx, float vy, float vz){
    gg[0][0] = 2.0*cXX;
    gg[0][1] = gg[1][0] = cXY;
    gg[0][2] = gg[2][0] = cZX;
    
    gg[1][1] = 2.0*cYY;
    gg[2][1] = gg[1][2] = cYZ;
    
    gg[2][2] = 2.0*cZZ;
  }
  
  
  static inline double weight(double d, float R){
    if(R < d){
      return 0.0;
    }
    //wendrand's functions
    else{
      d /= R;
      return pow(1.0-d, 6.0)*(35.0*d*d + 18.0*d + 3.0);
      //return pow(1.0-d, 4.0)*(4.0*d+1.0);
    }
  }
  
  static inline void weightG(double g[3],
                             double d, float R, float x, float y, float z){
    if(R < d){
      g[0] = g[1] = g[2] = 0;
    }
    else{
      double t = -20.0*pow(1.0-d/R, 3.0)/(R*R);
      
      g[0] = t*x;
      g[1] = t*y;
      g[2] = t*z;
    }
  }
  
  static inline void weightGG(double gg[3][3],
                              double d, float R, float x, float y, float z){
    if(R < d || d == 0){
      gg[0][0] = gg[0][1] = gg[0][2] =
      gg[1][0] = gg[1][1] = gg[1][2] =
      gg[2][0] = gg[2][1] = gg[2][2] = 0;
    }
    else{
      double d1 = d/R;
      double dw = -20.0*d1*pow(1.0-d1, 3.0);
      double ddw = 20.0*(1.0-d1)*(1.0-d1)*(4.0*d1-1);
      
      double Rd = R*d;
      double dx = x/Rd;
      double dy = y/Rd;
      double dz = z/Rd;
      
      double Rd3 = R*d*d*d;
      double dxx = (y*y+z*z)/Rd3;
      double dxy = -x*y/Rd3;
      double dzx = -z*x/Rd3;
      double dyy = (z*z+x*x)/Rd3;
      double dyz = -y*z/Rd3;
      double dzz = (x*x+y*y)/Rd3;

      gg[0][0] = ddw*dx*dx + dw*dxx;
      gg[0][1] = gg[1][0] = ddw*dx*dy + dw*dxy;
      gg[0][2] = gg[2][0] = ddw*dx*dz + dw*dzx;
      
      gg[1][1] = ddw*dy*dy + dw*dyy;
      gg[1][2] = gg[2][1] = ddw*dy*dz + dw*dyz;
      
      gg[2][2] = ddw*dz*dz + dw*dzz;
    }
  }
};

#endif