#ifndef RCROSS
#define RCROSS

#include "ImplicitFunction.h"

class RCross: public ImplicitFunction{
public:
  
  float a1, a2, a3, c;
  
  RCross(){
    a1 = 5.5f;
    a2 = 5.5f;
    a3 = 5.5f;
    c = 1.0f;
  }
  
  float value(float x, float y, float z){
    double xx = x*x;
    double yy = y*y;
    double zz = z*z;
    return (float)(xx*(xx+a1*yy) + yy*(yy+a2*zz) + zz*(zz+a3*xx) - c);
  }
  
  void gradient(float g[3], float x, float y, float z){
    double xx = x*x;
    double yy = y*y;
    double zz = z*z;
    
    g[0] = (float)(x*(4.0*xx + 2.0*(a1*yy + a3*zz)));
    g[1] = (float)(y*(4.0*yy + 2.0*(a2*zz + a1*xx)));
    g[2] = (float)(z*(4.0*zz + 2.0*(a3*xx + a2*yy)));
  }
  
  float valueAndGradient(float g[3], float x, float y, float z){
    double xx = x*x;
    double yy = y*y;
    double zz = z*z;
    
    g[0] = (float)(x*(4.0*xx + 2.0*(a1*yy + a3*zz)));
    g[1] = (float)(y*(4.0*yy + 2.0*(a2*zz + a1*xx)));
    g[2] = (float)(z*(4.0*zz + 2.0*(a3*xx + a2*yy)));

	return (float)(xx*(xx+a1*yy) + yy*(yy+a2*zz) + zz*(zz+a3*xx) - c);
  }

  double valueAndGradient1(double g[3], float x, float y, float z){
    double xx = x*x;
    double yy = y*y;
    double zz = z*z;
    
    g[0] = (x*(4.0*xx + 2.0*(a1*yy + a3*zz)));
    g[1] = (y*(4.0*yy + 2.0*(a2*zz + a1*xx)));
    g[2] = (z*(4.0*zz + 2.0*(a3*xx + a2*yy)));

	return (xx*(xx+a1*yy) + yy*(yy+a2*zz) + zz*(zz+a3*xx) - c);
  }
  
  //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 xx = x*x;
    double yy = y*y;
    double zz = z*z;
    
    g[0] = (x*(4.0*xx + 2.0*(a1*yy + a3*zz)));
    g[1] = (y*(4.0*yy + 2.0*(a2*zz + a1*xx)));
    g[2] = (z*(4.0*zz + 2.0*(a3*xx + a2*yy)));
    
    
    gg[0][0] = 12.0*xx + 2.0*(a1*yy + a3*zz);
    gg[0][1] = 4.0*a1*x*y;
    gg[0][2] = 4.0*a3*x*z;
    gg[1][1] = 12.0*yy + 2.0*(a2*zz + a1*xx);
    gg[1][2] = 4.0*a2*y*z;
    gg[2][2] = 12.0*zz + 2.0*(a3*xx + a2*yy);
    
    return (xx*(xx+a1*yy) + yy*(yy+a2*zz) + zz*(zz+a3*xx) - c);
  }

  double valueAndGradient123(double g1[3], double g2[3][3], double g3[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;
    
    return (xx*(xx+a1*yy) + yy*(yy+a2*zz) + zz*(zz+a3*xx) - c);
  }
  
  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);
  }
  
};

#endif