namespace getfem {
  class convex_locator : public context_dependencies {
    public:
      explicit convex_locator(const mesh &me);
      convex_locator();
      virtual ~convex_locator();
      void init_with_mesh(const mesh &me);
      void update_from_context(void) const;
      bool find_a_convex(base_node pt, size_type &cv, base_node *ptr=NULL);
      size_type find_all_convexes(base_node pt, dal::bit_vector &bv);
    protected:
      void build_rtree();
      const mesh *linked_mesh_;
      bgeot::rtree boxtree;
      size_type cv_stored;
      bgeot::rtree::pbox_set boxlst;
      bgeot::geotrans_inv_convex gic;
  };
  
void convex_locator::init_with_mesh(const mesh &me) {
  GMM_ASSERT1(linked_mesh_ == 0, "Mesh level set already initialized");
  linked_mesh_ = &me;
  this->add_dependency(me);
  this->build_rtree();
}

convex_locator::convex_locator ( const getfem::mesh& me ) {
  linked_mesh_ = 0;
  init_with_mesh(me);
}

convex_locator::convex_locator() {
  linked_mesh_ = 0;
}

convex_locator::~convex_locator() {
}


bool convex_locator::find_a_convex ( base_node pt, size_type& cv, base_node* ptr ) {
  GMM_ASSERT1(linked_mesh_ != 0, "Uninitialized convex_locator");
  
  context_check();

  base_node refpt(pt.size());
  if (NULL == ptr) {
    ptr = &refpt;
  }
  bool gt_invertible;
  if (cv_stored != size_type(-1) && gic.invert(pt, *ptr, gt_invertible)) {
    cv = cv_stored;
    if (gt_invertible) {
      return true;
    }
  }
  boxtree.find_boxes_at_point(pt, boxlst);
  bgeot::rtree::pbox_set::const_iterator it = boxlst.begin(),ite = boxlst.end();
  for (; it != ite; ++it) {
    gic = bgeot::geotrans_inv_convex(linked_mesh_->convex((*it)->id),
         linked_mesh_->trans_of_convex((*it)->id));
    cv_stored = (*it)->id;
    if (gic.invert(pt, *ptr, gt_invertible)) {
        cv = (*it)->id;
        return true;
    }
  }
  return false;
}

size_type convex_locator::find_all_convexes( base_node pt, dal::bit_vector &bv) {
  GMM_ASSERT1(linked_mesh_ != 0, "Uninitialized convex_locator");
  
  context_check();

  size_type counter=0;
  base_node refpt(pt.size());
  bool gt_invertible;
  boxtree.find_boxes_at_point(pt, boxlst);
  bgeot::rtree::pbox_set::const_iterator it = boxlst.begin(),ite = boxlst.end();
  for (; it != ite; ++it) {
    gic = bgeot::geotrans_inv_convex(linked_mesh_->convex((*it)->id),
         linked_mesh_->trans_of_convex((*it)->id));
    cv_stored = (*it)->id;
    if (gic.invert(pt, refpt, gt_invertible)) {
        bv.add((*it)->id);
        counter++;
    }
  }
  return counter;
}

void convex_locator::update_from_context ( void ) const {
  convex_locator *self = const_cast<convex_locator*>(this);
  self->cv_stored = size_type(-1);
  self->build_rtree();
}


void convex_locator::build_rtree () {
  GMM_ASSERT1(linked_mesh_ != 0, "Uninitialized convex_locator");
  base_node min, max;
  scalar_type EPS=1E-13;
  cv_stored = size_type(-1);
  boxtree.clear();
  for (dal::bv_visitor cv(linked_mesh_->convex_index()); !cv.finished(); ++cv) {
    bounding_box(min, max, linked_mesh_->points_of_convex(cv),
                 linked_mesh_->trans_of_convex(cv));
    for (unsigned k=0; k < min.size(); ++k) { min[k]-=EPS; max[k]+=EPS; }
    boxtree.add_box(min, max, cv);
  }
}

} /* namespace getfem */