/*===========================================================================
 
 Copyright (C) 2000-2012 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"

namespace bgeot {
  struct rtree_elt_base {
    enum { RECTS_PER_LEAF=8 };
    bool isleaf_;
    bool isleaf() const { return isleaf_; }
    base_node rmin, rmax;
    rtree_elt_base(bool leaf, const base_node& rmin_, const base_node& rmax_) 
      : isleaf_(leaf), rmin(rmin_), rmax(rmax_) {}
  };
  
  struct rtree_node : public rtree_elt_base {
    rtree_elt_base *left, *right;
    rtree_node(const base_node& bmin, const base_node& bmax, 
	       rtree_elt_base *left_, rtree_elt_base *right_) 
      : rtree_elt_base(false, bmin, bmax), left(left_), right(right_) {}
  };

  struct rtree_leaf : public rtree_elt_base {
    rtree::pbox_cont lst;
    rtree_leaf(const base_node& bmin, const base_node& bmax, 
	       rtree::pbox_cont& lst_)
      : rtree_elt_base(true, bmin, bmax) { lst.swap(lst_); }
  };

  /* enlarge box to hold [a..b] */
  static void update_box(base_node& bmin, base_node& bmax, const base_node& a, const base_node& b) {
    base_node::iterator itmin=bmin.begin(), itmax=bmax.begin();
    for (size_type i=0; i < a.size(); ++i) {
      itmin[i] = std::min(itmin[i], a.at(i));
      itmax[i] = std::max(itmax[i], b.at(i));
    }
  }

  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;
  }

  /* some predicates for searches */
  struct intersection_p {
    const base_node 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); }
  };

  /* match boxes containing [min..max] */
  struct contains_p {
    const base_node min,max;
    contains_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_ge_r2(min2,max2,min,max); }
    bool accept(const base_node& min2, const base_node& max2) 
    { return r1_inter_r2(min,max,min2,max2); }
  };

  /* match boxes contained in [min..max] */
  struct contained_p {
    const base_node 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;
    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 find_matching_boxes_(rtree_elt_base *n, rtree::pbox_set& boxlst,
				   Predicate p) {
    //cout << "find_matching_boxes_: " << n->rmin << ".." << n->rmax << "\n";
    if (n->isleaf()) {
      //cout << "find_matching_boxes_ in leaf\n";
      const rtree_leaf *rl = static_cast<rtree_leaf*>(n);
      for (rtree::pbox_cont::const_iterator it = rl->lst.begin(); it != rl->lst.end(); ++it) {
	//cout << "  ->match(" << (*it)->id << "=" << (*it)->min << "," << (*it)->max << " -> " << p((*it)->min, (*it)->max) << "\n";
	if (p((*it)->min, (*it)->max)) { boxlst.insert(*it); }
      }
    } else {
      //cout << "find_matching_boxes_ in branch\n";
      const rtree_node *rn = static_cast<rtree_node*>(n);
      if (p.accept(rn->left->rmin,rn->left->rmax)) 
	bgeot::find_matching_boxes_(rn->left, boxlst, p);
      if (p.accept(rn->right->rmin,rn->right->rmax)) 
	bgeot::find_matching_boxes_(rn->right, boxlst, p);
    }
  }

  void rtree::find_intersecting_boxes(const base_node& bmin, const base_node& bmax, pbox_set& boxlst) {
    boxlst.clear(); if (!root) build_tree(); 
    if (root) find_matching_boxes_(root, boxlst, intersection_p(bmin,bmax));
    //cout << "find_intersecting_boxes : found " << boxlst.size() << " matches\n";
  }

  void rtree::find_containing_boxes(const base_node& bmin, const base_node& bmax, pbox_set& boxlst) {
    boxlst.clear(); if (!root) build_tree();
    if (root) find_matching_boxes_(root, boxlst, contains_p(bmin,bmax));
  }

  void rtree::find_contained_boxes(const base_node& bmin, const base_node& bmax, pbox_set& boxlst) {
    boxlst.clear(); if (!root) build_tree();
    if (root) find_matching_boxes_(root, boxlst, contained_p(bmin,bmax));
    //cout << "find_matching_boxes : found " << boxlst.size() << " matches\n";
  }

  void rtree::find_boxes_at_point(const base_node& P, pbox_set& boxlst) {
    boxlst.clear(); if (!root) build_tree();
    if (root) find_matching_boxes_(root, boxlst, has_point_p(P));
  }

  /* 
     try to split at the approximate center of the box. Could be much more
     sophisticated 
  */
  static bool split_test(const rtree::pbox_cont& b, 
			 const base_node& bmin, const base_node& bmax, 
			 unsigned dir, scalar_type& split_v) {
    scalar_type v = bmin[dir] + (bmax[dir] - bmin[dir])/2; split_v = v;
    size_type cnt = 0;
    //cout << "[enter]Split_test: dir=" << dir << ", split_v=" << v << ", bmin=" << bmin << ", bmax=" << bmax << "\n";
    for (rtree::pbox_cont::const_iterator it = b.begin(); it != b.end(); ++it) {
      if ((*it)->max[dir] < v) {
	if (cnt == 0) split_v = (*it)->max[dir]; 
	else split_v = std::max((*it)->max[dir],split_v);
	cnt++; 
      }
    }
    //cout << "[exit] Split_test cnt = " << cnt << ", b.size()=" << b.size() << ", split_v=" << split_v << "\n";
    return (cnt > 0 && cnt < b.size());
  }

  /* 
     there are many flavors of rtree ... this one is more or less a quadtree
     where splitting does not occurs at predefined positions (hence the split_test
     function above). Regions of the tree do not overlap (box are splitted).
  */
  static rtree_elt_base* build_tree_(rtree::pbox_cont b, 
				     const base_node& bmin, const base_node& bmax,
				     unsigned last_dir) {
    size_type N=bmin.size();
    scalar_type split_v(0); 
    unsigned split_dir = unsigned((last_dir+1)%N);
    //cout << " build_tree_ [b.size=" << b.size() << "], bmin=" << bmin << ", bmax=" << bmax << "\n";
    bool split_ok = false;
    if (b.size() > rtree_elt_base::RECTS_PER_LEAF) {
      for (size_type itry=0; itry < N; ++itry) {
	//cout << "split_test: dir=" << split_dir << "\n";
	if (split_test(b, bmin, bmax, split_dir, split_v)) { split_ok = true; break; }
	split_dir = unsigned((split_dir+1)%N);
      }
      //if (!split_ok && b.size() > rtree_elt_base::RECTS_PER_LEAF*2) cout << "FAILED TO SPLIT ...\n";
    }
    if (split_ok) {
      size_type cnt1=0,cnt2=0;
      //cout << "splitting with v=" << split_v << "\n";
      for (rtree::pbox_cont::const_iterator it = b.begin(); it != b.end(); ++it) {
	//cout << " . test box" << (*it)->min[split_dir] << ".." << (*it)->max[split_dir] << "\n";
	if ((*it)->min[split_dir] < split_v) cnt1++; 
	if ((*it)->max[split_dir] > split_v) cnt2++;
      }
      //cout << "  -> left : " << cnt1 << " boxes, right : " << cnt2 << " boxes\n";
      assert(cnt1); assert(cnt2);
      GMM_ASSERT1(cnt1+cnt2 >= b.size(), "internal error");
      rtree::pbox_cont v1(cnt1), v2(cnt2);
      base_node bmin1(bmax), bmax1(bmin); 
      base_node bmin2(bmax), bmax2(bmin);
      cnt1 = cnt2 = 0;
      for (rtree::pbox_cont::const_iterator it = b.begin(); it != b.end(); ++it) {
	if ((*it)->min[split_dir] < split_v) {
	  v1[cnt1++] = *it; 
	  update_box(bmin1,bmax1,(*it)->min,(*it)->max);
	  //cout << "update_box bmin1=" << bmin1 << ", bmax1=" << bmax1 << "\n";
	}
	if ((*it)->max[split_dir] > split_v) {
	  v2[cnt2++] = *it;
	  update_box(bmin2,bmax2,(*it)->min,(*it)->max);
	}
      }
      for (size_type k=0; k < N; ++k) {
	bmin1[k] = std::max(bmin1[k],bmin[k]);
	bmax1[k] = std::min(bmax1[k],bmax[k]);
	bmin2[k] = std::max(bmin2[k],bmin[k]);
	bmax2[k] = std::min(bmax2[k],bmax[k]);
      }
      bmax1[split_dir] = std::min(bmax1[split_dir], split_v);
      bmin2[split_dir] = std::max(bmin2[split_dir], split_v);
      assert(cnt1 == v1.size()); assert(cnt2 == v2.size());
      return new rtree_node(bmin,bmax, 
			    build_tree_(v1, bmin1, bmax1, split_dir),
			    build_tree_(v2, bmin2, bmax2, split_dir));
    } else {
      return new rtree_leaf(bmin,bmax,b);
    }
  }

 void rtree::build_tree() {
   //cout << "build tree\n";
   if (boxes.size() == 0) return;
   assert(root == 0);
   pbox_cont b(boxes.size());
   pbox_cont::iterator b_it = b.begin();
   base_node bmin(boxes.front().min), bmax(boxes.front().max);
   for (box_cont::const_iterator it=boxes.begin(); it != boxes.end(); ++it) {
     update_box(bmin,bmax,(*it).min,(*it).max);
     *b_it++ = &(*it);
   }
   root = build_tree_(b,bmin,bmax,0);
 }
  
  static void destroy_tree_(rtree_elt_base *n) {
    if (n->isleaf()) delete static_cast<rtree_leaf*>(n);
    else {
      const rtree_node *rn = static_cast<rtree_node*>(n);
      if (rn->left) { destroy_tree_(rn->left); }
      if (rn->right) { destroy_tree_(rn->right); }
      delete rn;
    }
  }

  void rtree::destroy_tree() {
    if (root) { destroy_tree_(root); }
    root = 0;
  }

  static void dump_tree_(rtree_elt_base *p, int level, size_type& count) {
    if (!p) return;
    cout << level << "|";
    for (int i=0; i < level; ++i) cout << "  ";
    cout << "span=" << p->rmin << ".." << p->rmax << " ";
    if (p->isleaf()) {
      rtree_leaf *rl = static_cast<rtree_leaf*>(p);
      cout << "Leaf [" << rl->lst.size() << " elts] = ";
      for (size_type i=0; i < rl->lst.size(); ++i) cout << " " << rl->lst[i]->id; cout << "\n";
      count += rl->lst.size();
    } else {
      cout << "Node\n";
      const rtree_node *rn = static_cast<rtree_node*>(p);
      if (rn->left) { dump_tree_(rn->left, level+1, count); }
      if (rn->right) { dump_tree_(rn->right, level+1, count); }
    }
  }

  void rtree::dump() {
    cout << "tree dump follows\n";
    if (!root) build_tree();
    size_type count = 0;
    dump_tree_(root, 0, count);
    cout << " --- end of tree dump, nb of rectangles: " << boxes.size() 
	 << ", rectangle ref in tree: " << count << "\n";
  }
}
