#ifndef EVALUATIONTREED
#define EVALUATIONTREED

#include "BasisFunction.h"
#include "ImplicitFunction.h"
#include <stdio.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){
      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][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];
      }
      return f;
    }
    
    void computeCallSize(){
      call = 0.25f*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){
    for(int i=0; i<bfN; i++){
      if(bfs[i]->support == 0){
        delete bfs[i];
        continue;
      }
      root->addBF(bfs[i]);
    }
    delete[] bfs;
    
    root->computeCallSize();
  }
  
  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){
    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;
    }
  }
  
  //Original code from Prof. Alexander G. Belyaev
  //assumed f=0, maybe
  void curvatureHK(double &H, double &K, float x, float y, float z){
    double pd1[3], pd2[3][3];
    pd1[0] = pd1[1] = pd1[2] =
    pd2[0][0] = pd2[0][1] = pd2[0][2] =
    pd2[1][0] = pd2[1][1] = pd2[1][2] =
    pd2[2][0] = pd2[2][1] = pd2[2][2] = 0;
    double f = root->valueAndGradient12(pd1, pd2, x, y, z);
    
    /* temporary derivatives*/
    double fxfx = pd1[0]*pd1[0];
    double fxfy = pd1[0]*pd1[1];
    double fxfz = pd1[0]*pd1[2];
    double fyfy = pd1[1]*pd1[1];
    double fyfz = pd1[1]*pd1[2];
    double fzfz = pd1[2]*pd1[2];
    
    double fxx = pd2[0][0];
    double fxy = pd2[0][1];
    double fxz = pd2[0][2];
    double fyy = pd2[1][1];
    double fyz = pd2[1][2];
    double fzz = pd2[2][2];
    
    double g2 = pd1[0]*pd1[0]+pd1[1]*pd1[1]+pd1[2]*pd1[2];
    double g1 = sqrt(g2);
    double g3 = g2*g1;
    double g4 = g2*g2;

    /* mean and gaussian curvatures */
    H = (fxx*(fyfy+fzfz) + fyy*(fxfx+fzfz) + fzz*(fxfx+fyfy)
		- 2*(fxy*fxfy+fxz*fxfz+fyz*fyfz))/2;
    H /= g3;
    
    K = fxfx*(fyy*fzz-fyz*fyz)
      + fyfy*(fxx*fzz-fxz*fxz)
        + fzfz*(fxx*fyy-fxy*fxy)
          + 2*(fxfy*(fxz*fyz-fxy*fzz)
               + fxfz*(fxy*fyz-fxz*fyy)
               + fyfz*(fxy*fxz-fxx*fyz));
    K /= g4; 
  }
  
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;
  }
};

#endif