#include "OctSampler.h"

void OctSamplerCell::split(){
  int s[9];
  s[0] = _start;
  s[8] = _end;
  s[4] = split1D(s[0], s[8], 2);
  s[2] = split1D(s[0], s[4], 1);
  s[6] = split1D(s[4], s[8], 1);
  s[1] = split1D(s[0], s[2], 0);
  s[3] = split1D(s[2], s[4], 0);
  s[5] = split1D(s[4], s[6], 0);
  s[7] = split1D(s[6], s[8], 0);
  
  float sx = 0.5f*_tree->sizeX(_level);
  float sy = 0.5f*_tree->sizeY(_level);
  float sz = 0.5f*_tree->sizeZ(_level);
  
  _sub_cell = new OctSamplerCell*[8];
  for(int i=0; i<8; i++){
    if(s[i+1] - s[i] > 0){
      //center of i-th subcell
      float x, y, z;
      
      if(i%2 == 0) x = _cx - sx;
      else         x = _cx + sx;
      
      if(i%4 < 2)  y = _cy - sy;
      else         y = _cy + sy;
      
      if(i < 4)    z = _cz - sz;
      else         z = _cz + sz;
      
      _sub_cell[i] = new OctSamplerCell(_tree, x, y, z, _level+1, s[i], s[i+1]);
    }
    else
      _sub_cell[i] = NULL;
  }
}

int OctSamplerCell::split1D(int s, int e, int index){
  int i = s;
  int j = e-1;
  float (*point)[3] = _tree->_ps->_point;
  float m;
  if(index == 0)
    m = _cx;
  else if(index == 1)
    m = _cy;
  else
    m = _cz;
  while(i <= j){
    if(point[i][index] < m)
      i++;
    else
      _tree->_ps->swapIndex(i, j--);
  }
  return i;
}

void OctSamplerCell::averagedLeafSize(double& total, int max_point){
  if(_end - _start <= max_point){
    float sx = _tree->sizeX(_level);
    float sy = _tree->sizeY(_level);
    float sz = _tree->sizeZ(_level);
    total += (_end-_start)*sqrt(sx*sx + sy*sy + sz*sz);
  }
  else{
    if(_sub_cell == NULL)
      split();
    for(int i=0; i<8; i++)
      if(_sub_cell[i] != NULL)
        _sub_cell[i]->averagedLeafSize(total, max_point);
  }
}

void OctSamplerCell::countPoints(int& count, int target_level){
  if(_level == target_level || _end - _start < 2)
    count++;
  else{
    if(_sub_cell == NULL)
      split();
    for(int i=0; i<8; i++)
      if(_sub_cell[i] != NULL)
        _sub_cell[i]->countPoints(count, target_level);
  }
}

void OctSamplerCell::collectPoints(PointSet* qs, int& count, int target_level){
  if(_level == target_level || _end - _start < 2){
    float p[3];
    
    _tree->_ps->centroid(p, _start, _end);
    qs->setPoint(count, p[0], p[1], p[2]);
    
    _tree->_ps->averagedNormal(p, _start, _end);
    qs->setNormal(count, p[0], p[1], p[2]);
    count++;
  }
  else{
    if(_sub_cell == NULL)
      split();
    for(int i=0; i<8; i++)
      if(_sub_cell[i] != NULL)
        _sub_cell[i]->collectPoints(qs, count, target_level);
  }
}