// based on Eigen unsupported bvh KDTree

#pragma once
/** \file */

namespace hgeom {
namespace bvh {

////////////////////////////////////////////////////////////////////////
//////////////////////////// Intersector ///////////////////////////////
////////////////////////////////////////////////////////////////////////

namespace internal {

#ifndef EIGEN_PARSED_BY_DOXYGEN
template <typename BVH, typename Intersector>
bool intersect_helper(const BVH &tree, Intersector &intersector,
                      typename BVH::Index root) {
  typedef typename BVH::Index Index;
  typedef typename BVH::VolumeIterator VolIter;
  typedef typename BVH::ObjectIterator ObjIter;

  VolIter vBegin = VolIter(), vEnd = VolIter();
  ObjIter oBegin = ObjIter(), oEnd = ObjIter();

  std::vector<Index> todo(1, root);

  while (!todo.empty()) {
    tree.getChildren(todo.back(), vBegin, vEnd, oBegin, oEnd);
    todo.pop_back();

    for (; vBegin != vEnd; ++vBegin) // go through child volumes
      if (intersector.intersectVolume(tree.getVolume(*vBegin)))
        todo.push_back(*vBegin);

    for (; oBegin != oEnd; ++oBegin) // go through child objects
      if (intersector.intersectObject(*oBegin))
        return true; // intersector said to stop query
  }
  return false;
}
#endif // not EIGEN_PARSED_BY_DOXYGEN

template <typename Volume1, typename Object1, typename Object2,
          typename Intersector>
struct intersector_helper1 {
  intersector_helper1(const Object2 &inStored, Intersector &in)
      : stored(inStored), intersector(in) {}
  bool intersectVolume(const Volume1 &vol) {
    return intersector.intersectVolumeObject(vol, stored);
  }
  bool intersectObject(const Object1 &obj) {
    return intersector.intersectObjectObject(obj, stored);
  }
  Object2 stored;
  Intersector &intersector;

private:
  intersector_helper1 &operator=(const intersector_helper1 &);
};

template <typename Volume2, typename Object2, typename Object1,
          typename Intersector>
struct intersector_helper2 {
  intersector_helper2(const Object1 &inStored, Intersector &in)
      : stored(inStored), intersector(in) {}
  bool intersectVolume(const Volume2 &vol) {
    return intersector.intersectObjectVolume(stored, vol);
  }
  bool intersectObject(const Object2 &obj) {
    return intersector.intersectObjectObject(stored, obj);
  }
  Object1 stored;
  Intersector &intersector;

private:
  intersector_helper2 &operator=(const intersector_helper2 &);
};

} // namespace internal

/**  Given a BVH, runs the query encapsulated by \a intersector.
  *  The Intersector type must provide the following members: \code
     bool intersectVolume(const BVH::Volume &volume) //returns true if volume
  intersects the query
     bool intersectObject(const BVH::Object &object) //returns true if the
  search should terminate immediately
  \endcode
  */
template <typename BVH, typename Intersector>
void BVIntersect(const BVH &tree, Intersector &intersector) {
  internal::intersect_helper(tree, intersector, tree.getRootIndex());
}

/**  Given two BVH's, runs the query on their Cartesian product encapsulated by
  \a intersector.
  *  The Intersector type must provide the following members: \code
     bool intersectVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2)
  //returns true if product of volumes intersects the query
     bool intersectVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2)
  //returns true if the volume-object product intersects the query
     bool intersectObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2)
  //returns true if the volume-object product intersects the query
     bool intersectObjectObject(const BVH1::Object &o1, const BVH2::Object &o2)
  //returns true if the search should terminate immediately
  \endcode
  */
template <typename BVH1, typename BVH2, typename Intersector>
void BVIntersect(
    const BVH1 &tree1, const BVH2 &tree2,
    Intersector &intersector) // TODO: tandem descent when it makes sense
{
  typedef typename BVH1::Index Index1;
  typedef typename BVH2::Index Index2;
  typedef internal::intersector_helper1<typename BVH1::Volume,
                                        typename BVH1::Object,
                                        typename BVH2::Object, Intersector>
      Helper1;
  typedef internal::intersector_helper2<typename BVH2::Volume,
                                        typename BVH2::Object,
                                        typename BVH1::Object, Intersector>
      Helper2;
  typedef typename BVH1::VolumeIterator VolIter1;
  typedef typename BVH1::ObjectIterator ObjIter1;
  typedef typename BVH2::VolumeIterator VolIter2;
  typedef typename BVH2::ObjectIterator ObjIter2;

  VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
  ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
  VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
  ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();

  std::vector<std::pair<Index1, Index2>> todo(
      1, std::make_pair(tree1.getRootIndex(), tree2.getRootIndex()));

  while (!todo.empty()) {
    tree1.getChildren(todo.back().first, vBegin1, vEnd1, oBegin1, oEnd1);
    tree2.getChildren(todo.back().second, vBegin2, vEnd2, oBegin2, oEnd2);
    todo.pop_back();

    for (; vBegin1 != vEnd1;
         ++vBegin1) { // go through child volumes of first tree
      const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
      for (vCur2 = vBegin2; vCur2 != vEnd2;
           ++vCur2) { // go through child volumes of second tree
        if (intersector.intersectVolumeVolume(vol1, tree2.getVolume(*vCur2)))
          todo.push_back(std::make_pair(*vBegin1, *vCur2));
      }

      for (oCur2 = oBegin2; oCur2 != oEnd2;
           ++oCur2) { // go through child objects of second tree
        Helper1 helper(*oCur2, intersector);
        if (internal::intersect_helper(tree1, helper, *vBegin1))
          return; // intersector said to stop query
      }
    }

    for (; oBegin1 != oEnd1;
         ++oBegin1) { // go through child objects of first tree
      for (vCur2 = vBegin2; vCur2 != vEnd2;
           ++vCur2) { // go through child volumes of second tree
        Helper2 helper(*oBegin1, intersector);
        if (internal::intersect_helper(tree2, helper, *vCur2))
          return; // intersector said to stop query
      }

      for (oCur2 = oBegin2; oCur2 != oEnd2;
           ++oCur2) { // go through child objects of second tree
        if (intersector.intersectObjectObject(*oBegin1, *oCur2))
          return; // intersector said to stop query
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////
///////////////////////////// Minimizer ////////////////////////////////
////////////////////////////////////////////////////////////////////////

namespace internal {

#ifndef EIGEN_PARSED_BY_DOXYGEN
template <typename BVH, typename Minimizer>
typename Minimizer::Scalar
minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root,
                typename Minimizer::Scalar minimum) {
  typedef typename Minimizer::Scalar Scalar;
  typedef typename BVH::Index Index;
  typedef std::pair<Scalar, Index> QueueElement; // first element is priority
  typedef typename BVH::VolumeIterator VolIter;
  typedef typename BVH::ObjectIterator ObjIter;

  VolIter vBegin = VolIter(), vEnd = VolIter();
  ObjIter oBegin = ObjIter(), oEnd = ObjIter();
  std::priority_queue<QueueElement, std::vector<QueueElement>,
                      std::greater<QueueElement>>
      todo; // smallest is at the top

  todo.push(std::make_pair(Scalar(), root));

  while (!todo.empty()) {
    tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd);
    todo.pop();

    for (; oBegin != oEnd; ++oBegin) { // go through child objects
      minimum = (std::min)(minimum, minimizer.minimumOnObject(*oBegin));
    }

    for (; vBegin != vEnd; ++vBegin) { // go through child volumes
      Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin));
      if (val < minimum)
        todo.push(std::make_pair(val, *vBegin));
    }
  }

  return minimum;
}
#endif // not EIGEN_PARSED_BY_DOXYGEN

template <typename Volume1, typename Object1, typename Object2,
          typename Minimizer>
struct minimizer_helper1 {
  typedef typename Minimizer::Scalar Scalar;
  minimizer_helper1(const Object2 &inStored, Minimizer &m)
      : stored(inStored), minimizer(m) {}
  Scalar minimumOnVolume(const Volume1 &vol) {
    return minimizer.minimumOnVolumeObject(vol, stored);
  }
  Scalar minimumOnObject(const Object1 &obj) {
    return minimizer.minimumOnObjectObject(obj, stored);
  }
  Object2 stored;
  Minimizer &minimizer;

private:
  minimizer_helper1 &operator=(const minimizer_helper1 &);
};

template <typename Volume2, typename Object2, typename Object1,
          typename Minimizer>
struct minimizer_helper2 {
  typedef typename Minimizer::Scalar Scalar;
  minimizer_helper2(const Object1 &inStored, Minimizer &m)
      : stored(inStored), minimizer(m) {}
  Scalar minimumOnVolume(const Volume2 &vol) {
    return minimizer.minimumOnObjectVolume(stored, vol);
  }
  Scalar minimumOnObject(const Object2 &obj) {
    return minimizer.minimumOnObjectObject(stored, obj);
  }
  Object1 stored;
  Minimizer &minimizer;

private:
  minimizer_helper2 &operator=(const minimizer_helper2 &);
};

} // end namespace internal

/**  Given a BVH, runs the query encapsulated by \a minimizer.
  *  \returns the minimum value.
  *  The Minimizer type must provide the following members: \code
     typedef Scalar //the numeric type of what is being minimized--not
  necessarily the Scalar type of the BVH (if it has one)
     Scalar minimumOnVolume(const BVH::Volume &volume)
     Scalar minimumOnObject(const BVH::Object &object)
  \endcode
  */
template <typename BVH, typename Minimizer>
typename Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer) {
  return internal::minimize_helper(
      tree, minimizer, tree.getRootIndex(),
      std::numeric_limits<typename Minimizer::Scalar>::max());
}

/**  Given two BVH's, runs the query on their cartesian product encapsulated by
  \a minimizer.
  *  \returns the minimum value.
  *  The Minimizer type must provide the following members: \code
     typedef Scalar //the numeric type of what is being minimized--not
  necessarily the Scalar type of the BVH (if it has one)
     Scalar minimumOnVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume
  &v2)
     Scalar minimumOnVolumeObject(const BVH1::Volume &v1, const BVH2::Object
  &o2)
     Scalar minimumOnObjectVolume(const BVH1::Object &o1, const BVH2::Volume
  &v2)
     Scalar minimumOnObjectObject(const BVH1::Object &o1, const BVH2::Object
  &o2)
  \endcode
  */
template <typename BVH1, typename BVH2, typename Minimizer>
typename Minimizer::Scalar BVMinimize(const BVH1 &tree1, const BVH2 &tree2,
                                      Minimizer &minimizer) {
  typedef typename Minimizer::Scalar Scalar;
  typedef typename BVH1::Index Index1;
  typedef typename BVH2::Index Index2;
  typedef internal::minimizer_helper1<typename BVH1::Volume,
                                      typename BVH1::Object,
                                      typename BVH2::Object, Minimizer>
      Helper1;
  typedef internal::minimizer_helper2<typename BVH2::Volume,
                                      typename BVH2::Object,
                                      typename BVH1::Object, Minimizer>
      Helper2;
  typedef std::pair<Scalar, std::pair<Index1, Index2>>
      QueueElement; // first element is priority
  typedef typename BVH1::VolumeIterator VolIter1;
  typedef typename BVH1::ObjectIterator ObjIter1;
  typedef typename BVH2::VolumeIterator VolIter2;
  typedef typename BVH2::ObjectIterator ObjIter2;

  VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
  ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
  VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
  ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
  std::priority_queue<QueueElement, std::vector<QueueElement>,
                      std::greater<QueueElement>>
      todo; // smallest is at the top

  Scalar minimum = (std::numeric_limits<Scalar>::max)();
  todo.push(std::make_pair(
      Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())));

  while (!todo.empty()) {
    tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1);
    tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2);
    todo.pop();

    for (; oBegin1 != oEnd1;
         ++oBegin1) { // go through child objects of first tree
      for (oCur2 = oBegin2; oCur2 != oEnd2;
           ++oCur2) { // go through child objects of second tree
        minimum = (std::min)(minimum,
                             minimizer.minimumOnObjectObject(*oBegin1, *oCur2));
      }

      for (vCur2 = vBegin2; vCur2 != vEnd2;
           ++vCur2) { // go through child volumes of second tree
        Helper2 helper(*oBegin1, minimizer);
        minimum = (std::min)(
            minimum, internal::minimize_helper(tree2, helper, *vCur2, minimum));
      }
    }

    for (; vBegin1 != vEnd1;
         ++vBegin1) { // go through child volumes of first tree
      const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);

      for (oCur2 = oBegin2; oCur2 != oEnd2;
           ++oCur2) { // go through child objects of second tree
        Helper1 helper(*oCur2, minimizer);
        minimum = (std::min)(minimum, internal::minimize_helper(
                                          tree1, helper, *vBegin1, minimum));
      }

      for (vCur2 = vBegin2; vCur2 != vEnd2;
           ++vCur2) { // go through child volumes of second tree
        Scalar val =
            minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2));
        if (val < minimum)
          todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2)));
      }
    }
  }
  return minimum;
}

////////////////////////////////////////////////////////////////////////
///////////////////////// Floor Minimizer ////////////////////////////
////////////////////////////////////////////////////////////////////////
/*
namespace internal {

#ifndef EIGEN_PARSED_BY_DOXYGEN
template <typename BVH, typename Minimizer>
typename Minimizer::Scalar floormin_helper(const BVH &tree,
                                           Minimizer &minimizer,
                                           typename BVH::Index root,
                                           typename Minimizer::Scalar minimum,
                                           typename Minimizer::Scalar floor) {
  typedef typename Minimizer::Scalar Scalar;
  typedef typename BVH::Index Index;
  typedef std::pair<Scalar, Index> QueueElement;  // first element is priority
  typedef typename BVH::VolumeIterator VolIter;
  typedef typename BVH::ObjectIterator ObjIter;

  VolIter vBegin = VolIter(), vEnd = VolIter();
  ObjIter oBegin = ObjIter(), oEnd = ObjIter();
  std::priority_queue<QueueElement, std::vector<QueueElement>,
                      std::greater<QueueElement> >
      todo;  // smallest is at the top

  todo.push(std::make_pair(Scalar(), root));

  while (!todo.empty()) {
    tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd);
    todo.pop();

    for (; oBegin != oEnd; ++oBegin) {  // go through child objects
      minimum = (std::min)(minimum, minimizer.minimumOnObject(*oBegin));
      if (minimum <= floor) return minimum;
    }

    for (; vBegin != vEnd; ++vBegin) {  // go through child volumes
      Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin));
      if (val < minimum) todo.push(std::make_pair(val, *vBegin));
    }
  }

  return minimum;
}
#endif  // not EIGEN_PARSED_BY_DOXYGEN

template <typename Volume1, typename Object1, typename Object2,
          typename Minimizer>
struct floormin_helper1 {
  typedef typename Minimizer::Scalar Scalar;
  floormin_helper1(const Object2 &inStored, Minimizer &m)
      : stored(inStored), minimizer(m) {}
  Scalar minimumOnVolume(const Volume1 &vol) {
    return minimizer.minimumOnVolumeObject(vol, stored);
  }
  Scalar minimumOnObject(const Object1 &obj) {
    return minimizer.minimumOnObjectObject(obj, stored);
  }
  Object2 stored;
  Minimizer &minimizer;

 private:
  floormin_helper1 &operator=(const floormin_helper1 &);
};

template <typename Volume2, typename Object2, typename Object1,
          typename Minimizer>
struct floormin_helper2 {
  typedef typename Minimizer::Scalar Scalar;
  floormin_helper2(const Object1 &inStored, Minimizer &m)
      : stored(inStored), minimizer(m) {}
  Scalar minimumOnVolume(const Volume2 &vol) {
    return minimizer.minimumOnObjectVolume(stored, vol);
  }
  Scalar minimumOnObject(const Object2 &obj) {
    return minimizer.minimumOnObjectObject(stored, obj);
  }
  Object1 stored;
  Minimizer &minimizer;

 private:
  floormin_helper2 &operator=(const floormin_helper2 &);
};

}  // end namespace internal

// *  Given a BVH, runs the query encapsulated by \a minimizer.
//   *  \returns the minimum value.
//   *  The Minimizer type must provide the following members: \code
//      typedef Scalar //the numeric type of what is being minimized--not
//   necessarily the Scalar type of the BVH (if it has one)
//      Scalar minimumOnVolume(const BVH::Volume &volume)
//      Scalar minimumOnObject(const BVH::Object &object)
//   \endcode

template <typename BVH, typename Minimizer>
typename Minimizer::Scalar BVFloorMinimize(
    const BVH &tree, Minimizer &minimizer,
    typename Minimizer::Scalar floor =
        -std::numeric_limits<typename Minimizer::Scalar>::max()) {
  return internal::floormin_helper(
      tree, minimizer, tree.getRootIndex(),
      (std::numeric_limits<typename Minimizer::Scalar>::max)(), floor);
}

// Given two BVH's, runs the query on their cartesian product encapsulated by
// \a minimizer.
//   \returns the minimum value.
//   The Minimizer type must provide the following members: \code
// typedef Scalar //the numeric type of what is being minimized--not
// necessarily the Scalar type of the BVH (if it has one)
// Scalar minimumOnVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume
// &v2) Scalar minimumOnVolumeObject(const BVH1::Volume &v1, const BVH2::Object
// &o2)
// Scalar minimumOnObjectVolume(const BVH1::Object &o1, const BVH2::Volume
// &v2)
// Scalar minimumOnObjectObject(const BVH1::Object &o1, const BVH2::Object
// &o2)
// \endcode

template <typename BVH1, typename BVH2, typename Minimizer>
typename Minimizer::Scalar BVFloorMinimize(
    const BVH1 &tree1, const BVH2 &tree2, Minimizer &minimizer,
    typename Minimizer::Scalar floor =
        -std::numeric_limits<typename Minimizer::Scalar>::max()) {
  typedef typename Minimizer::Scalar Scalar;
  typedef typename BVH1::Index Index1;
  typedef typename BVH2::Index Index2;
  typedef internal::floormin_helper1<typename BVH1::Volume,
                                     typename BVH1::Object,
                                     typename BVH2::Object, Minimizer>
      Helper1;
  typedef internal::floormin_helper2<typename BVH2::Volume,
                                     typename BVH2::Object,
                                     typename BVH1::Object, Minimizer>
      Helper2;
  typedef std::pair<Scalar, std::pair<Index1, Index2> >
      QueueElement;  // first element is priority
  typedef typename BVH1::VolumeIterator VolIter1;
  typedef typename BVH1::ObjectIterator ObjIter1;
  typedef typename BVH2::VolumeIterator VolIter2;
  typedef typename BVH2::ObjectIterator ObjIter2;

  VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
  ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
  VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
  ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
  std::priority_queue<QueueElement, std::vector<QueueElement>,
                      std::greater<QueueElement> >
      todo;  // smallest is at the top

  Scalar minimum = (std::numeric_limits<Scalar>::max)();
  todo.push(std::make_pair(
      Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())));

  while (!todo.empty()) {
    tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1);
    tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2);
    todo.pop();

    for (; oBegin1 != oEnd1;
         ++oBegin1) {  // go through child objects of first tree
      for (oCur2 = oBegin2; oCur2 != oEnd2;
           ++oCur2) {  // go through child objects of second tree
        minimum = (std::min)(minimum,
                             minimizer.minimumOnObjectObject(*oBegin1, *oCur2));
        if (minimum <= floor) return minimum;
      }

      for (vCur2 = vBegin2; vCur2 != vEnd2;
           ++vCur2) {  // go through child volumes of second tree
        Helper2 helper(*oBegin1, minimizer);
        minimum = (std::min)(
            minimum,
            internal::floormin_helper(tree2, helper, *vCur2, minimum, floor));
        if (minimum <= floor) return minimum;
      }
    }

    for (; vBegin1 != vEnd1;
         ++vBegin1) {  // go through child volumes of first tree
      const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);

      for (oCur2 = oBegin2; oCur2 != oEnd2;
           ++oCur2) {  // go through child objects of second tree
        Helper1 helper(*oCur2, minimizer);
        minimum = (std::min)(
            minimum,
            internal::floormin_helper(tree1, helper, *vBegin1, minimum, floor));
        if (minimum <= floor) return minimum;
      }

      for (vCur2 = vBegin2; vCur2 != vEnd2;
           ++vCur2) {  // go through child volumes of second tree
        Scalar val =
            minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2));
        if (val < minimum)
          todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2)));
      }
    }
  }
  return minimum;
}
*/

} // namespace bvh
} // namespace hgeom
