#ifndef RSIX
#define RSIX

#include "ImplicitFunction.h"

class RSix: public ImplicitFunction{
public:
  
  float a, b, c, d;
  
  RSix(){
    a = 1;
    b = 10.0f;
    c = 0;
    d = 1.0f;
  }
  
  float value(float x, float y, float z){
    double x2 = x*x;
    double y2 = y*y;
    double z2 = z*z;
    double x4 = x2*x2;
    double y4 = y2*y2;
    double z4 = z2*z2;
    double x6 = x4*x2;
    double y6 = y4*y2;
    double z6 = z4*z2;
    
    return (float)(a*(x6 + y6 + z6) + b*(x4*y2 + y4*z2 + z4*x2) - d);
  }
  
  void gradient(float g[3], float x, float y, float z){
    double x2 = x*x;
    double y2 = y*y;
    double z2 = z*z;
    double x4 = x2*x2;
    double y4 = y2*y2;
    double z4 = z2*z2;
    double x5 = x4*x;
    double y5 = y4*y;
    double z5 = z4*z;
    g[0] = (float)(6.0*a*x4*x + b*(4.0*x2*x*y2 + 2.0*z4*x));
    g[1] = (float)(6.0*a*y4*y + b*(4.0*y2*y*z2 + 2.0*x4*y));
    g[2] = (float)(6.0*a*z4*z + b*(4.0*z2*z*x2 + 2.0*y4*z));
  }
  
  float valueAndGradient(float g[3], float x, float y, float z){
    double x2 = x*x;
    double y2 = y*y;
    double z2 = z*z;
    double x4 = x2*x2;
    double y4 = y2*y2;
    double z4 = z2*z2;
    double x6 = x4*x2;
    double y6 = y4*y2;
    double z6 = z4*z2;
    
    g[0] = (float)(6.0*a*x4*x + b*(4.0*x2*x*y2 + 2.0*z4*x));
    g[1] = (float)(6.0*a*y4*y + b*(4.0*y2*y*z2 + 2.0*x4*y));
    g[2] = (float)(6.0*a*z4*z + b*(4.0*z2*z*x2 + 2.0*y4*z));
    
    return (float)(a*(x6 + y6 + z6) + b*(x4*y2 + y4*z2 + z4*x2) - d);
  }

  double valueAndGradient1(double g[3], float x, float y, float z){
    double x2 = x*x;
    double y2 = y*y;
    double z2 = z*z;
    double x4 = x2*x2;
    double y4 = y2*y2;
    double z4 = z2*z2;
    double x6 = x4*x2;
    double y6 = y4*y2;
    double z6 = z4*z2;
    
    g[0] = 6.0*a*x4*x + b*(4.0*x2*x*y2 + 2.0*z4*x);
    g[1] = 6.0*a*y4*y + b*(4.0*y2*y*z2 + 2.0*x4*y);
    g[2] = 6.0*a*z4*z + b*(4.0*z2*z*x2 + 2.0*y4*z);
    
    return a*(x6 + y6 + z6) + b*(x4*y2 + y4*z2 + z4*x2) - d;
  }
  
  //Used for curvature estimations.
  //Double precision is required for more accuarte estimations.
  double valueAndGradient12(double g[3], double gg[3][3],
                            float x, float y, float z){
    double x2 = x*x;
    double y2 = y*y;
    double z2 = z*z;
    double x3 = x2*x;
    double y3 = y2*y;
    double z3 = z2*z;
    double x4 = x2*x2;
    double y4 = y2*y2;
    double z4 = z2*z2;
    double x6 = x4*x2;
    double y6 = y4*y2;
    double z6 = z4*z2;
    
    g[0] = 6.0*a*x4*x + b*(4.0*x2*x*y2 + 2.0*z4*x);
    g[1] = 6.0*a*y4*y + b*(4.0*y2*y*z2 + 2.0*x4*y);
    g[2] = 6.0*a*z4*z + b*(4.0*z2*z*x2 + 2.0*y4*z);
    
    gg[0][0] = a*30.0*x4 + b*(12.0*x2*y2 + 2.0*z4);
    gg[0][1] = b*8.0*x3*y;
    gg[0][2] = b*8.0*z3*x;
    gg[1][1] = a*30.0*y4 + b*(12.0*y2*z2 + 2.0*x4);
    gg[1][2] = b*8.0*y3*z;
    gg[2][2] = a*30.0*z4 + b*(12.0*z2*x2 + 2.0*y4);
    
    return a*(x6 + y6 + z6) + b*(x4*y2 + y4*z2 + z4*x2) - d;
  }
  
  double valueAndGradient123(double g1[3], double g2[3][3], double g3[3][3][3], 
                             float x, float y, float z){
    double x2 = x*x;
    double y2 = y*y;
    double z2 = z*z;
    double x3 = x2*x;
    double y3 = y2*y;
    double z3 = z2*z;
    double x4 = x2*x2;
    double y4 = y2*y2;
    double z4 = z2*z2;
    double x6 = x4*x2;
    double y6 = y4*y2;
    double z6 = z4*z2;
    
    g1[0] = 6.0*a*x4*x + b*(4.0*x2*x*y2 + 2.0*z4*x);
    g1[1] = 6.0*a*y4*y + b*(4.0*y2*y*z2 + 2.0*x4*y);
    g1[2] = 6.0*a*z4*z + b*(4.0*z2*z*x2 + 2.0*y4*z);
    
    g2[0][0] = a*30.0*x4 + b*(12.0*x2*y2 + 2.0*z4);
    g2[0][1] = b*8.0*x3*y;
    g2[0][2] = b*8.0*z3*x;
    g2[1][1] = a*30.0*y4 + b*(12.0*y2*z2 + 2.0*x4);
    g2[1][2] = b*8.0*y3*z;
    g2[2][2] = a*30.0*z4 + b*(12.0*z2*x2 + 2.0*y4);
    
    g3[0][0][0] = a*120.0*x3 + b*24.0*x*y2;
    g3[0][0][1] = b*24.0*x2*y;
    g3[0][0][2] = b*8.0*z3;
    g3[0][1][1] = b*8.0*x3;
    g3[0][1][2] = 0;
    g3[0][2][2] = b*24.0*z2*x;
    g3[1][1][1] = a*120.0*y3 + b*24.0*y*z2;
    g3[1][1][2] = b*24.0*y2*z;
    g3[1][2][2] = b*8.0*y3;
    g3[2][2][2] = a*120.0*z3 + b*24.0*z*x2;
    
    return a*(x6 + y6 + z6) + b*(x4*y2 + y4*z2 + z4*x2) - d;
  }
  
  double valueAndGradient1234(double g1[3], double g2[3][3],
                              double g3[3][3][3], double g4[3][3][3][3],
                              float x, float y, float z){
    /*
    double xx = x*x;
    double yy = y*y;
    double zz = z*z;
    
    g1[0] = (x*(4.0*xx + 2.0*(a1*yy + a3*zz)));
    g1[1] = (y*(4.0*yy + 2.0*(a2*zz + a1*xx)));
    g1[2] = (z*(4.0*zz + 2.0*(a3*xx + a2*yy)));
    
    
    g2[0][0] = 12.0*xx + 2.0*(a1*yy + a3*zz);
    g2[0][1] = 4.0*a1*x*y;
    g2[0][2] = 4.0*a3*x*z;
    g2[1][1] = 12.0*yy + 2.0*(a2*zz + a1*xx);
    g2[1][2] = 4.0*a2*y*z;
    g2[2][2] = 12.0*zz + 2.0*(a3*xx + a2*yy);

    g3[0][0][0] = 24.0*x;
    g3[0][0][1] = 4.0*a1*y;
    g3[0][0][2] = 4.0*a3*z;
    g3[0][1][1] = 4.0*a1*x;
    g3[0][1][2] = 0;
    g3[0][2][2] = 4.0*a3*x;
    g3[1][1][1] = 24.0*y;
    g3[1][1][2] = 4.0*a2*z;
    g3[1][2][2] = 4.0*a2*y;
    g3[2][2][2] = 24.0*z;

	g4[0][0][0][0] = 24.0;
    g4[0][0][0][1] = 0.0;
    g4[0][0][0][2] = 0.0;
    g4[0][0][1][1] = 4.0*a1;
    g4[0][0][1][2] = 0.0;
    g4[0][0][2][2] = 4.0*a3;
    g4[0][1][1][1] = 0.0;
    g4[0][1][1][2] = 0.0;
    g4[0][1][2][2] = 0.0;
    g4[0][2][2][2] = 0.0;
    g4[1][1][1][1] = 24.0;
    g4[1][1][1][2] = 0.0;
    g4[1][1][2][2] = 4.0*a2;
    g4[1][2][2][2] = 0.0;
    g4[2][2][2][2] = 24.0;
	
    return (xx*(xx+a1*yy) + yy*(yy+a2*zz) + zz*(zz+a3*xx) - c);
    */
	  return 0;
  }
  
};

#endif