/*===========================================================================

 Copyright (C) 2007-2020 Yves Renard, Julien Pommier.

 This file is a part of GetFEM

 GetFEM  is  free software;  you  can  redistribute  it  and/or modify it
 under  the  terms  of the  GNU  Lesser General Public License as published
 by  the  Free Software Foundation;  either version 3 of the License,  or
 (at your option) any later version along with the GCC Runtime Library
 Exception either version 3.1 or (at your option) any later version.
 This program  is  distributed  in  the  hope  that it will be useful,  but
 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or  FITNESS  FOR  A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 License and GCC Runtime Library Exception for more details.
 You  should  have received a copy of the GNU Lesser General Public License
 along  with  this program;  if not, write to the Free Software Foundation,
 Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301, USA.

===========================================================================*/
#include "getfem/bgeot_rtree.h"
#include "getfem/bgeot_rtree.h"
#include "getfem/dal_bit_vector.h"
using std::endl; using std::cout; using std::cerr;
using std::ends; using std::cin;
using bgeot::base_node;
using bgeot::size_type;
using bgeot::dim_type;
using bgeot::rtree;

static bool quick = false;


static bool r1_ge_r2(const base_node& min1, const base_node& max1,
		     const base_node& min2, const base_node& max2) {
  for (size_type i=0; i < min1.size(); ++i) { 
    if (!(min1[i] <= min2[i] && max1[i] >= max2[i])) return false; 
  }
  return true;
}
static bool r1_inter_r2(const base_node& min1, const base_node& max1,
			const base_node& min2, const base_node& max2) {
  for (size_type i=0; i < min1.size(); ++i) 
    if (max1[i] < min2[i] || min1[i] > max2[i]) return false; 
  return true;
}

struct intersection_p {
  const base_node min,max;
  void print(std::ostream &o) { o << "intersects(" << min << ".." << max << ")"; }
  intersection_p(const base_node& min_, const base_node& max_) : min(min_), max(max_) {}
  bool operator()(const base_node& min2, const base_node& max2)
  { return r1_inter_r2(min,max,min2,max2); }
  bool accept(const base_node& min2, const base_node& max2) 
  { return operator()(min2,max2); }
};

struct contains_p {
  const base_node min,max;
  contains_p(const base_node& min_, const base_node& max_) : min(min_), max(max_) {}
  void print(std::ostream &o) { o << "contains(" << min << ".." << max << ")"; }
  bool operator()(const base_node& min2, const base_node& max2)
  { return r1_ge_r2(min2,max2,min,max); }
  bool accept(const base_node& min2, const base_node& max2) 
  { return operator()(min2,max2); }
};

struct contained_p {
  const base_node min,max;
  void print(std::ostream &o) { o << "contained(" << min << ".." << max << ")"; }
  contained_p(const base_node& min_, const base_node& max_) : min(min_), max(max_) {}
  bool accept(const base_node& min2, const base_node& max2)
  { return r1_inter_r2(min,max,min2,max2); }
  bool operator()(const base_node& min2, const base_node& max2) 
  { return r1_ge_r2(min,max,min2,max2); }
};

struct has_point_p {
  const base_node P;
  void print(std::ostream &o) { o << "has_point(" << P << ")"; }
  has_point_p(const base_node& P_) : P(P_) {}
  bool operator()(const base_node& min2, const base_node& max2) {
    for (size_type i=0; i < P.size(); ++i) 
      if (P[i] < min2[i] || P[i] > max2[i]) return false;
    return true;
  }
  bool accept(const base_node& min2, const base_node& max2) 
  { return operator()(min2,max2); }
};
  
template <typename Predicate>
static void brute_force_check(const std::vector<base_node>& rmin,
		       const std::vector<base_node>& rmax,			 
		       std::vector<size_type>& pbset, Predicate p) {    
  //cout << "brute_force_check("; p.print(cout); cout << ")\n";
  dal::bit_vector iset; 
  for (size_type i=0; i < rmin.size(); ++i) {
    if (p(rmin[i],rmax[i])) { 
      //cout << "brute_force_check: found match " << i << ":" << rmin[i] << "," << rmax[i] << "\n";
      iset.add(i);
    }
  }
  //cout << "brute force: iset = " << iset << "\n";
  dal::bit_vector bv; bv.merge_from(pbset);
  //cout << "      rtree: iset = " << bv << "\n";
  for (size_type i=0; i < pbset.size(); ++i) {
    assert(iset[pbset[i]]);
    iset[pbset[i]] = false;
  }
  if (iset.card()) { 
    cout << "the rtree found " << bv << " but failed to find " << iset << "\n";
    assert(iset.card() == 0);
  }
}

static void verify(const std::vector<base_node>& rmin, const std::vector<base_node>& rmax, bgeot::rtree& tree) {
  size_type N=rmin.front().size();
  std::vector<size_type> pbset;
  //tree.dump();
  base_node extent(rmin[0].size()); 
  for (size_type i=0; i < rmin.size(); ++i)
    for (size_type k=0; k < N; ++k) { 
      assert(rmax[i][k]-rmin[i][k] >= 0.);
      extent[k] = std::max(extent[k], rmax[i][k]-rmin[i][k]);
    }

  tree.build_tree();

  for (size_type i=0; i < 100; ++i) {
    base_node min(N), max(N);
    for (size_type k=0; k < N; ++k) { min[k] = gmm::random(double()*1.3); max[k] = min[k]+gmm::random()*0.1; }
    tree.find_containing_boxes(min,max,pbset);
    brute_force_check(rmin,rmax,pbset,contains_p(min,max));

    tree.find_intersecting_boxes(min,max,pbset);
    brute_force_check(rmin,rmax,pbset,intersection_p(min,max));

    tree.find_contained_boxes(min,max,pbset);
    brute_force_check(rmin,rmax,pbset,contained_p(min,max));

    tree.find_boxes_at_point(min,pbset);
    brute_force_check(rmin,rmax,pbset,has_point_p(min));

    tree.find_boxes_at_point(max,pbset);
    brute_force_check(rmin,rmax,pbset,has_point_p(max));
  }
  for (size_type i=0; i < rmin.size(); ++i) {
    base_node min2(rmin[i]); for (size_type k=0; k < N; ++k) { min2[k] -= extent[k]*gmm::random()*0.1; }
    base_node max2(rmax[i]); for (size_type k=0; k < N; ++k) { max2[k] += extent[k]*gmm::random()*0.1; }
    base_node min3(rmin[i]); for (size_type k=0; k < N; ++k) { min3[k] += extent[k]*gmm::random()*0.001; }
    base_node max3(rmax[i]); for (size_type k=0; k < N; ++k) { max3[k] -= extent[k]*gmm::random()*0.00001; }
    tree.find_boxes_at_point(rmin[i],pbset);
    assert(std::find(pbset.begin(), pbset.end(), i) != pbset.end());
    tree.find_boxes_at_point(rmax[i],pbset);
    assert(std::find(pbset.begin(), pbset.end(), i) != pbset.end());
    tree.find_contained_boxes(min2,max2,pbset);
    assert(pbset.size() >= 1);
    assert(std::find(pbset.begin(), pbset.end(), i) != pbset.end());
    //cout << "i=" << i << " find_contained_boxes " << rmin[i] << rmax[i] << "\n";
    tree.find_contained_boxes(rmin[i],rmax[i],pbset);
    assert(pbset.size() >= 1);
    assert(std::find(pbset.begin(), pbset.end(), i) != pbset.end());
    tree.find_containing_boxes(rmin[i],rmax[i],pbset);
    assert(pbset.size() >= 1);
    assert(std::find(pbset.begin(), pbset.end(), i) != pbset.end());
    { 
      contains_p p(min3,max3);
      tree.find_containing_boxes(min3,max3,pbset);
      //cout << "i=" << i << ", " << rmin[i] << rmax[i] << ", min3=" << min3 << ", max3=" << max3 << "\n";
      if (p(rmin[i],rmax[i])) {
	assert(pbset.size() >= 1);
	assert(std::find(pbset.begin(), pbset.end(), i) != pbset.end());
      } else {
	assert(std::find(pbset.begin(), pbset.end(), i) == pbset.end());
      }
    }
    //cout << "i=" << i << " find_intersecting_boxes " << rmin[i] << rmax[i] << ", " << min2 << max3 << "\n";
    tree.find_intersecting_boxes(min2,max3,pbset);
    assert(pbset.size() >= 1);
    assert(std::find(pbset.begin(), pbset.end(), i) != pbset.end());
  }
}

static void check_tree() {
  bgeot::rtree tree;
  tree.add_box(base_node(1.0,0.),base_node(1.5,0.));
  tree.add_box(base_node(2.0,0.),base_node(3.0,0.),2);
  tree.add_box(base_node(1.5,0.),base_node(2.2,0.),1);
  std::vector<size_type> pi;
  tree.build_tree();
  tree.find_boxes_at_point(base_node(2.8,0.),pi);
  tree.dump();
  assert(pi.size()==1 && pi[0] == 2);  
  tree.clear();
  std::vector<base_node> rmin, rmax;
  cout << "1D checks\n";
  for (int C=5; C < 15; ++C) {
    static double dec[] = {.1, .5, .7423, .99, 1.,1.0001,1.25,1.5,2.,4.,4.1};
    for (size_type d=0; d < sizeof(dec)/sizeof(dec[0]); ++d) {
      //cout << "C=" << C << ",  DEC=" << dec[d] << "\n";
      for (int i=-C; i < C-1; ++i) {
	base_node a(2),b(2); a[0] = i/double(C); b[0]=(i+dec[d])/double(C);
	rmin.push_back(a); rmax.push_back(b);
	tree.add_box(rmin.back(),rmax.back());
      }
      verify(rmin, rmax, tree);
      tree.clear(); rmin.clear(); rmax.clear();
    }
  }

  cout << "2D random check\n";
  for (size_type i=0; i < 600; ++i) {
    rmin.push_back(base_node(gmm::random(double()), gmm::random(double())));
    rmax.push_back(rmin.back() + base_node(1.+gmm::random(), 1.+gmm::random())/10.);
    tree.add_box(rmin.back(),rmax.back());
  }
  verify(rmin, rmax, tree);

  cout << "2D/1D random check\n";
  tree.clear(); rmin.clear(); rmax.clear();
  for (size_type i=0; i < 600; ++i) {
    rmin.push_back(base_node(double(i)/10000.,0.));
    rmax.push_back(base_node(double(i+1)/10000.,0.));
    tree.add_box(rmin.back(),rmax.back());
  }
  verify(rmin, rmax, tree);

  cout << "3D/2D random check\n";
  tree.clear(); rmin.clear(); rmax.clear();
  for (size_type i=0; i < 600; ++i) {
    rmin.push_back(base_node(gmm::random(double()), 0, gmm::random(double())));
    rmax.push_back(rmin.back() + base_node(.1+gmm::random(), 0.1, .1+gmm::random())/10.);
    tree.add_box(rmin.back(),rmax.back());
  }
  verify(rmin, rmax, tree);
  cout << "\nthe rtree is ok!\n";
}

int main(int argc, char **argv) {
  if (argc == 2 && strcmp(argv[1],"-quick")==0) quick = true;
  try {
    check_tree();
    /*if (!quick)
      speed_test(3,300000,20000);
      else speed_test(2,10000,100);*/
  } GMM_STANDARD_CATCH_ERROR;
  return 0;  
}