void createGrid();
void evaluateImplicitFunc();
void getLines();
bool callback_key_down(Viewer& viewer, unsigned char key, int modifiers);

Eigen::RowVector3d spatial_min, spatial_max;
int spatial_res;
std::vector<std::vector<int>>spatial_index;

int spatial_i(int x, int y, int z){
  return z+(y*spatial_res)+(x*spatial_res*spatial_res);
}

void initSpatialIndex(Eigen::RowVector3d min, Eigen::RowVector3d max, int resolution){
  spatial_min = min;
  spatial_max = max;
  spatial_res = resolution;
  int rres = 1+resolution;
  spatial_index = std::vector<std::vector<int>>(rres*rres*rres, std::vector<int>());
}

void registerPoint(Eigen::RowVector3d point, int value){
  Eigen::RowVector3d local = (point - spatial_min).array() / (spatial_max - spatial_min).array();
  int x = local[0]*spatial_res;
  int y = local[1]*spatial_res;
  int z = local[2]*spatial_res;
  spatial_index[spatial_i(x,y,z)].push_back(value);
}

std::vector<int> calcPointsInReach(Eigen::RowVector3d point, float radius){
  std::vector<int> vec;
  // Primitive full scan
  for(int i=P.rows(); i<constrained_points.rows(); ++i){
    float distance = (constrained_points.row(i) - point).norm();
    if(distance <= radius)
      vec.push_back(i);
  }
  return vec;
}

std::vector<int> getPointsInReach(Eigen::RowVector3d point, float radius){
  std::vector<int> vec;
  // Use spatial index localised scan
  Eigen::RowVector3d dim = spatial_max - spatial_min;
  Eigen::RowVector3d local = (point - spatial_min).array() / dim.array();
  int x = local[0]*spatial_res;
  int y = local[1]*spatial_res;
  int z = local[2]*spatial_res;
  int r = ceil(radius / dim.norm() * spatial_res)+1;
  
  for(int i=max(0,x-r); i<min(x+r,spatial_res); ++i){
    for(int j=max(0,y-r); j<min(y+r,spatial_res); ++j){
      for(int k=max(0,z-r); k<min(z+r,spatial_res); ++k){
        for(const auto& index: spatial_index[spatial_i(i,j,k)]){
          float distance = (constrained_points.row(index) - point).norm();
          if(distance <= radius)
            vec.push_back(index);
        }
      }
    }
  }
  return vec;
}