#ifndef EVALUATIONTREED
#define EVALUATIONTREED

#include "BasisFunction.h"
#include "ImplicitFunction.h"
#include <stdio.h>
#include <iostream.h>
#include "jacobi.h"

//Basis functions can be add dinamically. (slightly slower than the static version)
//This is used for multi-level fitting.
class EvaluationTreeD : public ImplicitFunction{
private:
  class BFlist{
  public:
    BasisFunction* bf;
    BFlist* next;
    
    BFlist(BasisFunction* bf){
      this->bf = bf;
      next = NULL;
    }
    
    ~BFlist(){
      delete bf;
      if(next!=NULL)
        delete next;
    }
  };
  
  
private:
  class EvaluationCellD{
  private:
    BFlist* bfl;
    float cx, cy, cz;
    float size;
    float call;
    
    EvaluationCellD** child;
    
  public:
    EvaluationCellD(float cx, float cy, float cz, float size){
      bfl = NULL;
      
      this->cx = cx;
      this->cy = cy;
      this->cz = cz;
      this->size = size;
      
      child = NULL;
    }
  
    ~EvaluationCellD(){
      delete bfl;
      if(child != NULL){
        for(int i=0; i<8; i++){
          if(child[i] != NULL)
            delete child[i];
        }
        delete child;
      }
    }
    
    double value(float x, float y, float z){
      /*
      if(cx+call < x || cx-call > x ||
         cy+call < y || cy-call > y ||
         cz+call < z || cz-call > z)
        return 0;*/
      
      double f = 0;
      if(child != NULL){
        float minB[3] = {x-call, y-call, z-call};
        float maxB[3] = {x+call, y+call, z+call};
        for(int i=0; i<8; i++){
          EvaluationCellD* c = child[i];
          if(c != NULL && c->touch(minB, maxB))
            f += c->value(x, y, z);
        }
      }
      
      for(BFlist* c=bfl; c!=NULL; c=c->next){
        f += c->bf->value(x, y, z);
      }
      
      return f;
    }
    
    void gradient(double g[3], float x, float y, float z){
      if(child != NULL){
        float minB[3] = {x-call, y-call, z-call};
        float maxB[3] = {x+call, y+call, z+call};
        for(int i=0; i<8; i++){
          EvaluationCellD* c = child[i];
          if(c != NULL && c->touch(minB, maxB))
            c->gradient(g, x, y, z);
        }
      }
      
      for(BFlist* c=bfl; c!=NULL; c=c->next){
        double g1[3];
        c->bf->gradient(g1, x, y, z);
        g[0] += g1[0];
        g[1] += g1[1];
        g[2] += g1[2];
      }
    }
    
    double valueAndGradient(double g[3], float x, float y, float z){
      double f = 0;
      if(child != NULL){
        float minB[3] = {x-call, y-call, z-call};
        float maxB[3] = {x+call, y+call, z+call};
        for(int i=0; i<8; i++){
          EvaluationCellD* c = child[i];
          if(c != NULL && c->touch(minB, maxB))
            f += c->valueAndGradient(g, x, y, z);
        }
      }
      
      for(BFlist* c=bfl; c!=NULL; c=c->next){
        double g1[3];
        f += c->bf->valueAndGradient(g1, x, y, z);
        g[0] += g1[0];
        g[1] += g1[1];
        g[2] += g1[2];
      }
      return f;
    }
    
    void gradient2(double gg[3][3], float x, float y, float z){
      if(child != NULL){
        float minB[3] = {x-call, y-call, z-call};
        float maxB[3] = {x+call, y+call, z+call};
        for(int i=0; i<8; i++){
          EvaluationCellD* c = child[i];
          if(c != NULL && c->touch(minB, maxB))
            c->gradient2(gg, x, y, z);
        }
      }
      
      for(BFlist* c=bfl; c!=NULL; c=c->next){
        double gg1[3][3];
        c->bf->gradient2(gg1, x, y, z);
        gg[0][0] += gg1[0][0];
        gg[0][1] += gg1[0][1];
        gg[0][2] += gg1[0][2];
        
        gg[1][0] += gg1[1][0];
        gg[1][1] += gg1[1][1];
        gg[1][2] += gg1[1][2];
        
        gg[2][0] += gg1[2][0];
        gg[2][1] += gg1[2][1];
        gg[2][2] += gg1[2][2];
      }
    }
    
    double valueAndGradient12(double g[3], double gg[3][3],
                              float x, float y, float z){
      double f = 0;
      if(child != NULL){
        float minB[3] = {x-call, y-call, z-call};
        float maxB[3] = {x+call, y+call, z+call};
        for(int i=0; i<8; i++){
          EvaluationCellD* c = child[i];
          if(c != NULL && c->touch(minB, maxB))
            f += c->valueAndGradient12(g, gg, x, y, z);
        }
      }
      
      for(BFlist* c=bfl; c!=NULL; c=c->next){
        double g1[3], gg1[3][3];
        f += c->bf->valueAndGradient12(g1, gg1, x, y, z);
        
        g[0] += g1[0];
        g[1] += g1[1];
        g[2] += g1[2];
        
        gg[0][0] += gg1[0][0];
        gg[0][1] += gg1[0][1];
        gg[0][2] += gg1[0][2];
        gg[1][1] += gg1[1][1];
        gg[1][2] += gg1[1][2];
        gg[2][2] += gg1[2][2];
      }
      return f;
    }
    
    double valueAndGradient123(double g[3], double gg[3][3], double ggg[3][3][3],
                               float x, float y, float z){
      double f = 0;
      if(child != NULL){
        float minB[3] = {x-call, y-call, z-call};
        float maxB[3] = {x+call, y+call, z+call};
        for(int i=0; i<8; i++){
          EvaluationCellD* c = child[i];
          if(c != NULL && c->touch(minB, maxB))
            f += c->valueAndGradient123(g, gg, ggg, x, y, z);
        }
      }
      
      for(BFlist* c=bfl; c!=NULL; c=c->next){
        double g1[3], gg1[3][3], ggg1[3][3][3];
        f += c->bf->valueAndGradient123(g1, gg1, ggg1, x, y, z);
        
        g[0] += g1[0];
        g[1] += g1[1];
        g[2] += g1[2];
        
        gg[0][0] += gg1[0][0];
        gg[0][1] += gg1[0][1];
        gg[0][2] += gg1[0][2];
        gg[1][1] += gg1[1][1];
        gg[1][2] += gg1[1][2];
        gg[2][2] += gg1[2][2];
        
        ggg[0][0][0] += ggg1[0][0][0];
        ggg[0][0][1] += ggg1[0][0][1];
        ggg[0][0][2] += ggg1[0][0][2];
        ggg[0][1][1] += ggg1[0][1][1];
        ggg[0][1][2] += ggg1[0][1][2];
        ggg[0][2][2] += ggg1[0][2][2];
        ggg[1][1][1] += ggg1[1][1][1];
        ggg[1][1][2] += ggg1[1][1][2];
        ggg[1][2][2] += ggg1[1][2][2];
        ggg[2][2][2] += ggg1[2][2][2];
      }
      return f;
    }
    
    double valueAndGradient1234(double g[3], double gg[3][3],
                                double ggg[3][3][3], double gggg[3][3][3][3],
                                float x, float y, float z){
      double f = 0;
      if(child != NULL){
        float minB[3] = {x-call, y-call, z-call};
        float maxB[3] = {x+call, y+call, z+call};
        for(int i=0; i<8; i++){
          EvaluationCellD* c = child[i];
          if(c != NULL && c->touch(minB, maxB))
            f += c->valueAndGradient1234(g, gg, ggg, gggg, x, y, z);
        }
      }
      
      for(BFlist* c=bfl; c!=NULL; c=c->next){
        double g1[3], gg1[3][3], ggg1[3][3][3], gggg1[3][3][3][3];
        f += c->bf->valueAndGradient1234(g1, gg1, ggg1, gggg1, x, y, z);
        
        g[0] += g1[0];
        g[1] += g1[1];
        g[2] += g1[2];
        
        gg[0][0] += gg1[0][0];
        gg[0][1] += gg1[0][1];
        gg[0][2] += gg1[0][2];
        gg[1][1] += gg1[1][1];
        gg[1][2] += gg1[1][2];
        gg[2][2] += gg1[2][2];
        
        ggg[0][0][0] += ggg1[0][0][0];
        ggg[0][0][1] += ggg1[0][0][1];
        ggg[0][0][2] += ggg1[0][0][2];
        ggg[0][1][1] += ggg1[0][1][1];
        ggg[0][1][2] += ggg1[0][1][2];
        ggg[0][2][2] += ggg1[0][2][2];
        ggg[1][1][1] += ggg1[1][1][1];
        ggg[1][1][2] += ggg1[1][1][2];
        ggg[1][2][2] += ggg1[1][2][2];
        ggg[2][2][2] += ggg1[2][2][2];
        
        gggg[0][0][0][0] += gggg1[0][0][0][0];
        gggg[0][0][0][1] += gggg1[0][0][0][1];
        gggg[0][0][0][2] += gggg1[0][0][0][2];
        gggg[0][0][1][1] += gggg1[0][0][1][1];
        gggg[0][0][1][2] += gggg1[0][0][1][2];
        gggg[0][0][2][2] += gggg1[0][0][2][2];
        gggg[0][1][1][1] += gggg1[0][1][1][1];
        gggg[0][1][1][2] += gggg1[0][1][1][2];
        gggg[0][1][2][2] += gggg1[0][1][2][2];
        gggg[0][2][2][2] += gggg1[0][2][2][2];
        gggg[1][1][1][1] += gggg1[1][1][1][1];
        gggg[1][1][1][2] += gggg1[1][1][1][2];
        gggg[1][1][2][2] += gggg1[1][1][2][2];
        gggg[1][2][2][2] += gggg1[1][2][2][2];
        gggg[2][2][2][2] += gggg1[2][2][2][2];
      }
      return f;
    }
    
    void computeCallSize(){
      
      call = 0.25f*size;
      /*
      call = 0.5f*size;
      for(BFlist* c=bfl; c!=NULL; c=c->next){
        BasisFunction* bf = c->bf;
        float s = bf->support;
        float ox = bf->centerX;
        float oy = bf->centerY;
        float oz = bf->centerZ;
        
        if( (ox+s) - (cx+size) > call)
          call = (ox+s) - (cx+size);
        if( (cx-size) - (ox-s) > call)
          call = (cx-size) - (ox-s);
        
        if( (oy+s) - (cy+size) > call)
          call = (oy+s) - (cy+size);
        if( (cy-size) - (oy-s) > call)
          call = (cy-size) - (oy-s);
        
        if( (oz+s) - (cz+size) > call)
          call = (oz+s) - (cz+size);
        if( (cz-size) - (oz-s) > call)
          call = (cz-size) - (oz-s);
      }
      call += size;*/
      
      if(child != NULL){
        for(int i=0; i<8; i++){
          if(child[i] == NULL)
            continue;
          
          child[i]->computeCallSize();
          
          for(BFlist* c=child[i]->bfl; c!=NULL; c=c->next){
            float r = c->bf->support;
            if(call < r)
              call = r;
          }
        }
      }
    }
    
    void addBF(BasisFunction* bf){
      float r = bf->support;
      if(2*r > size){
        if(bfl == NULL){
          bfl = new BFlist(bf);
        }
        else{
          BFlist* h = new BFlist(bf);
          h->next = bfl;
          bfl = h;
        }
      }
      else{
        if(child==NULL){
          child = new EvaluationCellD*[8];
          for(int i=0; i<8; i++)
            child[i] = NULL;
        }
        
        int index =0;
        if(bf->centerX > cx)
          index += 1;
        if(bf->centerY > cy)
          index += 2;
        if(bf->centerZ > cz)
          index += 4;
        
        if(child[index] == NULL){
          float x = cx;
          float y = cy;
          float z = cz;
          float s = 0.5f*size;
          if(index%2 == 0)
            x -= s;
          else
            x += s;
          if((index/2)%2 == 0)
            y -= s;
          else
            y += s;
          if(index < 4)
            z -= s;
          else
            z += s;
          
          child[index] = new EvaluationCellD(x, y, z, s);
        }
        
        child[index]->addBF(bf);
      }
    }
    
    void countBfList(int &count){
      for(BFlist* c=bfl; c!=NULL; c=c->next)
        count++;
      
      if(child != NULL){
        for(int i=0; i<8; i++){
          if(child[i] == NULL)
            continue;
          
          child[i]->countBfList(count);
        }
      }
    }
    
    void catBfList(BasisFunction** bf_array, int &count){
      for(BFlist* c=bfl; c!=NULL; c=c->next)
        bf_array[count++] = c->bf;
      
      if(child != NULL){
        for(int i=0; i<8; i++){
          if(child[i] == NULL)
            continue;
          
          child[i]->catBfList(bf_array, count);
        }
      }
    }
    
  private:
    bool inline touch(float minB[3], float maxB[3]){
      if(maxB[0] < cx-size || maxB[1] < cy-size || maxB[2] < cz-size ||
         minB[0] > cx+size || minB[1] > cy+size || minB[2] > cz+size)
        return false;
      else
        return true;
    }
  };
  
private:
  EvaluationCellD* root;
  
public:
  float max[3], min[3];
  float out;
  
  EvaluationTreeD(float min[3], float max[3]){
    this->min[0] = min[0];
    this->min[1] = min[1];
    this->min[2] = min[2];
    
    this->max[0] = max[0];
    this->max[1] = max[1];
    this->max[2] = max[2];
    
    float mx = 0.5f*(min[0] + max[0]);
    float my = 0.5f*(min[1] + max[1]);
    float mz = 0.5f*(min[2] + max[2]);
    float s = max[0] - min[0];
    if(s < max[1] - min[1])
      s = max[1] - min[1];
    if(s < max[2] - min[2])
      s = max[2] - min[2];
    s *= 0.5f;
    
    root = new EvaluationCellD(mx, my, mz, s);
    
    printf("Octree construction is Fnished.\n\n");
    
    out = -10;
  }
  
  EvaluationTreeD(BasisFunction** bfs, int bfN, float min[3], float max[3]){ 
    this->min[0] = min[0];
    this->min[1] = min[1];
    this->min[2] = min[2];
    
    this->max[0] = max[0];
    this->max[1] = max[1];
    this->max[2] = max[2];
    
    float mx = 0.5f*(min[0] + max[0]);
    float my = 0.5f*(min[1] + max[1]);
    float mz = 0.5f*(min[2] + max[2]);
    float s = max[0] - min[0];
    if(s < max[1] - min[1])
      s = max[1] - min[1];
    if(s < max[2] - min[2])
      s = max[2] - min[2];
    s *= 0.5f;
    
    printf("Octree construction is started.\n");
    
    root = new EvaluationCellD(mx, my, mz, s);
    addBFS(bfs, bfN);
    
    printf("Octree construction is Fnished.\n\n");
    
    out = -10;
  }
  
  ~EvaluationTreeD(){
  }
  
  void addBFS(BasisFunction** bfs, int bfN){
	seed[0] = bfs[0]->centerX;
	seed[1] = bfs[0]->centerY;
	seed[2] = bfs[0]->centerZ;
    for(int i=0; i<bfN; i++){
      if(bfs[i]->support == 0){
        delete bfs[i];
        continue;
      }
      root->addBF(bfs[i]);
    }
    delete[] bfs;
    
    root->computeCallSize();

    
  }
  
  float seed[3];
  void getBFS(BasisFunction** &bfs, int &bfN){
    bfN = 0;
    root->countBfList(bfN);
    bfs = new BasisFunction*[bfN];
    bfN = 0;
    root->catBfList(bfs, bfN);
  }
  
  float value(float x, float y, float z){
    /*
    double Kmax, Kmin, Rmax, Rmin, Dmax, Dmin, Tmax[3], Tmin[3];
    curvatureDerivative(Kmax, Kmin, Rmax, Rmin,
                        Dmax, Dmin, Tmax, Tmin, x, y, z);
    return Rmax*Rmin;*/
    
    if(isIn(x,y,z))
      return (float)root->value(x, y, z)+out;
    else
      return out;
  }
  
  void gradient(float g[3], float x, float y, float z){
    if(isIn(x,y,z)){
      double g1[3];
      g1[0] = g1[1] = g1[2] = 0;
      root->gradient(g1, x, y, z);
      g[0] = (float)g1[0];
      g[1] = (float)g1[1];
      g[2] = (float)g1[2];
    }
    else
      g[0] = g[1] = g[2] = 0;
  }
  
  float valueAndGradient(float g[3], float x, float y, float z){
    if(isIn(x,y,z)){
      double g1[3];
      g1[0] = g1[1] = g1[2] = 0;
      double f = root->valueAndGradient(g1, x, y, z);
      g[0] = (float)g1[0];
      g[1] = (float)g1[1];
      g[2] = (float)g1[2];
      return (float)(f+out);
    }
    else{
      g[0] = g[1] = g[2] = 0;
      return out;
    }
  }
  
  double valueAndGradient1(double g[3], float x, float y, float z){
    g[0] = g[1] = g[2] = 0;
    return root->valueAndGradient(g, x, y, z) + out;
  }
  
  double valueAndGradient12(double g[3], double gg[3][3],
                            float x, float y, float z){
    initPd12(g, gg);
    return root->valueAndGradient12(g, gg, x, y, z) + out;
  }
  
  double valueAndGradient123(double g1[3], double g2[3][3],
                             double g3[3][3][3],
                             float x, float y, float z){
    initPd123(g1, g2, g3);
    return root->valueAndGradient123(g1, g2, g3, x, y, z) + out;
  }
  
  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){
    initPd1234(g1, g2, g3, g4);
    return root->valueAndGradient1234(g1, g2, g3, g4, x, y, z) + out;
  }
  
private:
  bool inline isIn(float x, float y, float z){
    if(x < min[0] || y < min[1] || z < min[2] ||
       x > max[0] || y > max[1] || z > max[2])
      return false;
    else
      return true;
  }
  
  inline void initPd12(double pd1[3], double pd2[3][3]){
    pd1[0] = pd1[1] = pd1[2] = 
    
    pd2[0][0] = pd2[0][1] = pd2[0][2] =
    pd2[1][1] = pd2[1][2] = pd2[2][2] = 0;
  }
  
  inline void initPd123(double pd1[3], double pd2[3][3],
                        double pd3[3][3][3]){
    pd1[0] = pd1[1] = pd1[2] = 
    
    pd2[0][0] = pd2[0][1] = pd2[0][2] =
    pd2[1][1] = pd2[1][2] = pd2[2][2] = 
    
    
    pd3[0][0][0] = pd3[0][0][1] = pd3[0][0][2] =
    pd3[0][1][1] = pd3[0][1][2] = pd3[0][2][2] =
    pd3[1][1][1] = pd3[1][1][2] = pd3[1][2][2] =
    pd3[2][2][2] = 0;
  }
  
  inline void initPd1234(double pd1[3], double pd2[3][3],
                         double pd3[3][3][3], double pd4[3][3][3][3]){
    pd1[0] = pd1[1] = pd1[2] = 
    
    pd2[0][0] = pd2[0][1] = pd2[0][2] =
    pd2[1][1] = pd2[1][2] = pd2[2][2] = 
    
    
    pd3[0][0][0] = pd3[0][0][1] = pd3[0][0][2] =
    pd3[0][1][1] = pd3[0][1][2] = pd3[0][2][2] =
    pd3[1][1][1] = pd3[1][1][2] = pd3[1][2][2] =
    pd3[2][2][2] = 
    
    pd4[0][0][0][0] = pd4[0][0][0][1] = pd4[0][0][0][2] =
    pd4[0][0][1][1] = pd4[0][0][1][2] = pd4[0][0][2][2] =
    pd4[0][1][1][1] = pd4[0][1][1][2] = pd4[0][1][2][2] =
    pd4[0][2][2][2] =
    pd4[1][1][1][1] = pd4[1][1][1][2] = pd4[1][1][2][2] =
    pd4[1][2][2][2] = pd4[2][2][2][2] = 0;
  }
};

#endif