/*--------------------------------------------------------------------------*\
 |                                                                          |
 |  Copyright (C) 2018                                                      |
 |                                                                          |
 |         , __                 , __                                        |
 |        /|/  \               /|/  \                                       |
 |         | __/ _   ,_         | __/ _   ,_                                |
 |         |   \|/  /  |  |   | |   \|/  /  |  |   |                        |
 |         |(__/|__/   |_/ \_/|/|(__/|__/   |_/ \_/|/                       |
 |                           /|                   /|                        |
 |                           \|                   \|                        |
 |                                                                          |
 |      Paolo Bevilacqua and Enrico Bertolazzi                              |
 |                                                                          |
 |      (1) Dipartimento di Ingegneria e Scienza dell'Informazione          |
 |      (2) Dipartimento di Ingegneria Industriale                          |
 |                                                                          |
 |      Universita` degli Studi di Trento                                   |
 |      email: paolo.bevilacqua@unitn.it                                    |
 |      email: enrico.bertolazzi@unitn.it                                   |
 |                                                                          |
\*--------------------------------------------------------------------------*/

///
/// file: AABBtree.hh
///

#include "Clothoids.hh"

// Workaround for Visual Studio
#ifdef min
  #undef min
#endif

#ifdef max
  #undef max
#endif

#include <algorithm>

namespace G2lib {

  using std::abs;
  using std::min;
  using std::max;
  using std::numeric_limits;

  /*\
   |   ____  ____
   |  | __ )| __ )  _____  __
   |  |  _ \|  _ \ / _ \ \/ /
   |  | |_) | |_) | (_) >  <
   |  |____/|____/ \___/_/\_\
  \*/

  void
  BBox::join( vector<PtrBBox> const & bboxes ) {
    if ( bboxes.empty() ) {
      m_xmin = m_ymin = m_xmax = m_ymax = 0;
    } else {
      vector<PtrBBox>::const_iterator it = bboxes.begin();

      m_xmin = (*it)->m_xmin;
      m_ymin = (*it)->m_ymin;
      m_xmax = (*it)->m_xmax;
      m_ymax = (*it)->m_ymax;

      for ( ++it; it != bboxes.end(); ++it ) {
        BBox const & currBox = **it;
        if ( currBox.m_xmin < m_xmin ) m_xmin = currBox.m_xmin;
        if ( currBox.m_ymin < m_ymin ) m_ymin = currBox.m_ymin;
        if ( currBox.m_xmax > m_xmax ) m_xmax = currBox.m_xmax;
        if ( currBox.m_ymax > m_ymax ) m_ymax = currBox.m_ymax;
      }
    }
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  real_type
  BBox::distance( real_type x, real_type y ) const {
    /*\
     |
     |   6          7          8
     |       +-------------+
     |       |             |
     |   3   |      4      |   5
     |       |             |
     |       +-------------+
     |   0          1          2
     |
    \*/
    int_type icase = 4;
    if      ( x < m_xmin ) icase = 3;
    else if ( x > m_xmax ) icase = 5;
    if      ( y < m_ymin ) icase -= 3;
    else if ( y > m_ymax ) icase += 3;
    real_type dst = 0;
    switch ( icase ) {
      case 0: dst = hypot( x-m_xmin, y-m_ymin); break;
      case 1: dst = m_ymin-y;                   break;
      case 2: dst = hypot( x-m_xmax, y-m_ymin); break;
      case 3: dst = m_xmin-x;                   break;
      case 4:                                   break;
      case 5: dst = x-m_xmax;                   break;
      case 6: dst = hypot( x-m_xmin, y-m_ymax); break;
      case 7: dst = y-m_ymax;                   break;
      case 8: dst = hypot( x-m_xmax, y-m_ymax); break;
    }
    return dst;
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  real_type
  BBox::maxDistance( real_type x, real_type y ) const {
    real_type dx = max( abs(x-m_xmin), abs(x-m_xmax) );
    real_type dy = max( abs(y-m_ymin), abs(y-m_ymax) );
    return hypot(dx,dy);
  }

  /*\
   |      _        _    ____  ____  _
   |     / \      / \  | __ )| __ )| |_ _ __ ___  ___
   |    / _ \    / _ \ |  _ \|  _ \| __| '__/ _ \/ _ \
   |   / ___ \  / ___ \| |_) | |_) | |_| | |  __/  __/
   |  /_/   \_\/_/   \_\____/|____/ \__|_|  \___|\___|
  \*/

  #ifdef G2LIB_USE_CXX11

  AABBtree::AABBtree() {
    pBBox.reset();
    children.clear();
  }

  AABBtree::~AABBtree() {
    pBBox.reset();
    children.clear();
  }

  void
  AABBtree::clear() {
    pBBox.reset();
    children.clear();
  }

  bool
  AABBtree::empty() const {
    return children.empty() && !pBBox;
  }

  #else

  AABBtree::AABBtree()
  : pBBox(nullptr)
  {
    children.clear();
  }

  AABBtree::~AABBtree() {
    if ( pBBox != nullptr ) {
      delete pBBox;
      pBBox = nullptr;
    }
    children.clear();
  }

  void
  AABBtree::clear() {
    if ( pBBox != nullptr ) {
      delete pBBox;
      pBBox = nullptr;
    }
    children.clear();
  }

  bool
  AABBtree::empty() const {
    return children.empty() && pBBox == nullptr;
  }

  #endif

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  void
  AABBtree::build( vector<PtrBBox> const & bboxes ) {
    clear();

    if ( bboxes.empty() ) return;

    size_t size = bboxes.size();

    if ( size == 1 ) {
      this -> pBBox = bboxes.front();
      return;
    }

    ///pBBox = make_shared<BBox>( bboxes, 0, 0 );
    #ifdef G2LIB_USE_CXX11
    pBBox = shared_ptr<BBox>( new BBox(bboxes, 0, 0) );
    #else
    if ( pBBox != nullptr ) { delete pBBox; pBBox = nullptr; }
    pBBox = new BBox( bboxes, 0, 0 );
    #endif

    real_type xmin = pBBox -> Xmin();
    real_type ymin = pBBox -> Ymin();
    real_type xmax = pBBox -> Xmax();
    real_type ymax = pBBox -> Ymax();

    vector<PtrBBox> posBoxes;
    vector<PtrBBox> negBoxes;

    if ( (ymax - ymin) > (xmax - xmin) ) {
      real_type cutPos = (ymax + ymin)/2;
      vector<PtrBBox>::const_iterator it;
      for ( it = bboxes.begin(); it != bboxes.end(); ++it ) {
        real_type ymid = ( (*it) -> Ymin() + (*it) -> Ymax() ) / 2;
        if ( ymid > cutPos ) posBoxes.push_back(*it);
        else                 negBoxes.push_back(*it);
      }
    } else {
      real_type cutPos = (xmax + xmin)/2;
      vector<PtrBBox>::const_iterator it;
      for ( it = bboxes.begin(); it != bboxes.end(); ++it ) {
        real_type xmid = ( (*it) -> Xmin() + (*it) -> Xmax() ) / 2;
        if ( xmid > cutPos ) posBoxes.push_back(*it);
        else                 negBoxes.push_back(*it);
      }
    }

    if ( negBoxes.empty() ) {
      vector<PtrBBox>::iterator midIdx;
      midIdx = posBoxes.begin() + posBoxes.size()/2;
      negBoxes.insert( negBoxes.end(), midIdx, posBoxes.end() );
      posBoxes.erase( midIdx, posBoxes.end() );
    } else if ( posBoxes.empty() ) {
      vector<PtrBBox>::iterator midIdx;
      midIdx = negBoxes.begin() + negBoxes.size()/2;
      posBoxes.insert( posBoxes.end(), midIdx, negBoxes.end() );
      negBoxes.erase( midIdx, negBoxes.end() );
    }

    #ifdef G2LIB_USE_CXX11
    PtrAABB neg = make_shared<AABBtree>();
    PtrAABB pos = make_shared<AABBtree>();
    #else
    PtrAABB neg = new AABBtree();
    PtrAABB pos = new AABBtree();
    #endif

    neg->build(negBoxes);
    if (!neg->empty()) children.push_back(neg);

    pos->build(posBoxes);
    if (!pos->empty()) children.push_back(pos);
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  void
  AABBtree::print( ostream_type & stream, int level ) const {
    if ( empty() ) {
      stream << "[EMPTY AABB tree]\n";
    } else {
      fmt::print( stream,
        "BBOX xmin={:<12.4)} ymin={:<12.4)} xmax={:<12.4)} ymax={:<12.4)} level={}\n",
        pBBox->m_xmin, pBBox->m_ymin, pBBox->m_xmax, pBBox->m_ymax
      );
      vector<PtrAABB>::const_iterator it;
      for ( it = children.begin(); it != children.end(); ++it )
        (*it)->print( stream, level+1 );
    }
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  void
  AABBtree::intersect(
    AABBtree const & tree,
    VecPairPtrBBox & intersectionList,
    bool             swap_tree
  ) const {

    // check bbox with
    if ( !tree.pBBox->collision(*pBBox) ) return;

    int icase = (children.empty() ? 0 : 1) +
                (tree.children.empty()? 0 : 2);

    switch ( icase ) {
    case 0: // both leaf
      if ( swap_tree )
        intersectionList.push_back( PairPtrBBox(tree.pBBox,pBBox ) );
      else
        intersectionList.push_back( PairPtrBBox(pBBox,tree.pBBox ) );
      break;
    case 1: // first is a tree, second is a leaf
      { vector<PtrAABB>::const_iterator it;
        for ( it = children.begin(); it != children.end(); ++it )
          tree.intersect( **it, intersectionList, !swap_tree );
      }
      break;
    case 2: // first leaf, second is a tree
      { vector<PtrAABB>::const_iterator it;
        for ( it = tree.children.begin(); it != tree.children.end(); ++it )
          this->intersect( **it, intersectionList, swap_tree );
      }
      break;
    case 3: // first is a tree, second is a tree
      { vector<PtrAABB>::const_iterator c1;
        vector<PtrAABB>::const_iterator c2;
        for ( c1 = children.begin(); c1 != children.end(); ++c1 )
          for ( c2 = tree.children.begin(); c2 != tree.children.end(); ++c2 )
            (*c1)->intersect( **c2, intersectionList, swap_tree );
      }
      break;
    }
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  real_type
  AABBtree::min_maxdist(
    real_type        x,
    real_type        y,
    AABBtree const & tree,
    real_type        mmDist
  ) {

    vector<PtrAABB> const & children = tree.children;

    if ( children.empty() ) {
      real_type dst = tree.pBBox->maxDistance( x, y );
      return min( dst, mmDist );
    }

    real_type dmin = tree.pBBox->distance( x, y );
    if ( dmin > mmDist ) return mmDist;

    // check bbox with
    vector<PtrAABB>::const_iterator it;
    for ( it = children.begin(); it != children.end(); ++it )
      mmDist = min_maxdist( x, y, **it, mmDist );

    return mmDist;
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  void
  AABBtree::min_maxdist_select(
    real_type        x,
    real_type        y,
    real_type        mmDist,
    AABBtree const & tree,
    VecPtrBBox     & candidateList
  ) {
    vector<PtrAABB> const & children = tree.children;
    real_type dst = tree.pBBox->distance( x, y );
    if ( dst <= mmDist ) {
      if ( children.empty() ) {
        candidateList.push_back( tree.pBBox );
      } else {
        // check bbox with
        vector<PtrAABB>::const_iterator it;
        for ( it = children.begin(); it != children.end(); ++it )
          min_maxdist_select( x, y, mmDist, **it, candidateList );
      }
    }
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  void
  AABBtree::min_distance(
    real_type    x,
    real_type    y,
    VecPtrBBox & candidateList
  ) const {
    real_type mmDist = min_maxdist(
      x, y, *this, numeric_limits<real_type>::infinity()
    );
    min_maxdist_select( x, y, mmDist, *this, candidateList );
  }

}

///
/// eof: AABBtree.cc
///
