//----------------------------------------------------------------------------
//  EgalTech 2014-2014
//----------------------------------------------------------------------------
// File : PointGrid3d.cpp        Data : 15.05.14          Versione : 1.5e5
// Contenuto : Implementazione della classe PointGrid3d.
//             Indicizzazione spaziale di Point3d mediante hash grid.
//
//
// Modifiche : 15.05.14 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "\EgtDev\Include\EGkPointGrid3d.h"
#include <algorithm>
#include <utility>

using namespace std ;

//----------------------------------------------------------------------------
//    Struct IBox
//----------------------------------------------------------------------------
struct IBox {
   int nXmin, nXmax ;
   int nYmin, nYmax ;
   int nZmin, nZmax ;
   IBox( void) : nXmin( INT_MAX), nXmax( INT_MIN),
                  nYmin( INT_MAX), nYmax( INT_MIN),
                  nZmin( INT_MAX), nZmax( INT_MIN) {}
   void Reset( void)
            { nXmin = nYmin = nZmin = INT_MAX ;
               nXmax = nYmax = nZmax = INT_MIN ; }
   bool Encloses( int nX, int nY, int nZ)
            { return ( nX >= nXmin  &&  nX <= nXmax  &&
                        nY >= nYmin  &&  nY <= nYmax  &&
                        nZ >= nZmin  &&  nZ <= nZmax) ; }
} ;

//----------------------------------------------------------------------------
static const int N_SUBIBOX = 6 ;

//----------------------------------------------------------------------------
bool
SubtractIBox( const IBox& ibA, const IBox& ibB, IBox ibR[N_SUBIBOX])
{
  // ibR = ibA - ibB  (il risultato può essere formato da 6 box)
  // se ibA non include alcunchè
   if ( ibA.nXmin > ibA.nXmax  ||  ibA.nYmin > ibA.nYmax  ||  ibA.nZmin > ibA.nZmax) {
      for ( int i = 0 ; i < N_SUBIBOX ; ++ i)
         ibR[i].Reset() ;
      return true ;
   }
  // se ibB non include alcunchè
   if ( ibB.nXmin > ibB.nXmax  ||  ibB.nYmin > ibB.nYmax  ||  ibB.nZmin > ibB.nZmax) {
      ibR[0] = ibA ;
      for ( int i = 1 ; i < N_SUBIBOX ; ++ i)
         ibR[i].Reset() ;
      return true ;
   }
  // lungo asse X
   if ( ibA.nXmin < ibB.nXmin) {
      ibR[0].nXmin = ibA.nXmin ;
      ibR[0].nXmax = ibB.nXmin - 1 ;
      ibR[0].nYmin = ibA.nYmin ;
      ibR[0].nYmax = ibA.nYmax ;
      ibR[0].nZmin = ibA.nZmin ;
      ibR[0].nZmax = ibA.nZmax ;
   }
   else
      ibR[0].Reset() ;
   if ( ibA.nXmax > ibB.nXmax) {
      ibR[1].nXmin = ibB.nXmax + 1 ;
      ibR[1].nXmax = ibA.nXmax ;
      ibR[1].nYmin = ibA.nYmin ;
      ibR[1].nYmax = ibA.nYmax ;
      ibR[1].nZmin = ibA.nZmin ;
      ibR[1].nZmax = ibA.nZmax ;
   }
   else
      ibR[1].Reset() ;
  // lungo asse Y
   if ( ibA.nYmin < ibB.nYmin) {
      ibR[2].nXmin = ibB.nXmin ;
      ibR[2].nXmax = ibB.nXmax ;
      ibR[2].nYmin = ibA.nYmin ;
      ibR[2].nYmax = ibB.nYmin - 1 ;
      ibR[2].nZmin = ibA.nZmin ;
      ibR[2].nZmax = ibA.nZmax ;
   }
   else
      ibR[2].Reset() ;
   if ( ibA.nYmax > ibB.nYmax) {
      ibR[3].nXmin = ibB.nXmin ;
      ibR[3].nXmax = ibB.nXmax ;
      ibR[3].nYmin = ibB.nYmax + 1 ;
      ibR[3].nYmax = ibA.nYmax ;
      ibR[3].nZmin = ibA.nZmin ;
      ibR[3].nZmax = ibA.nZmax ;
   }
   else
      ibR[3].Reset() ;
  // lungo asse Z
   if ( ibA.nZmin < ibB.nZmin) {
      ibR[4].nXmin = ibB.nXmin ;
      ibR[4].nXmax = ibB.nXmax ;
      ibR[4].nYmin = ibB.nYmin ;
      ibR[4].nYmax = ibB.nYmax ;
      ibR[4].nZmin = ibA.nZmin ;
      ibR[4].nZmax = ibB.nZmin - 1 ;
   }
   else
      ibR[4].Reset() ;
   if ( ibA.nZmax > ibB.nZmax) {
      ibR[5].nXmin = ibB.nXmin ;
      ibR[5].nXmax = ibB.nXmax ;
      ibR[5].nYmin = ibB.nYmin ;
      ibR[5].nYmax = ibB.nYmax ;
      ibR[5].nZmin = ibB.nZmax + 1 ;
      ibR[5].nZmax = ibA.nZmax ;
   }
   else
      ibR[5].Reset() ;
   return true ;
}

//----------------------------------------------------------------------------
//    Class PointGrid3d
//----------------------------------------------------------------------------
bool
PointGrid3d::Init( int nBuckets, double dCellDim)
{
   m_dCellDim = max( dCellDim, EPS_SMALL) ;
   m_dInvCellDim = 1 / m_dCellDim ;
   m_BBox.Reset() ;
   m_MMap.clear() ;
   try { m_MMap.rehash( nBuckets) ;}
   catch(...) { return false ;}
   return true ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::InsertPoint( const Point3d& ptP, int nId)
{
  // inserimento nel map
   int nKey = PointHash( Get1dCellNbr( ptP.x), Get1dCellNbr( ptP.y), Get1dCellNbr( ptP.z)) ;
   m_MMap.emplace( nKey, make_pair( ptP, nId)) ;
  // aggiornamento del bbox complessivo
   m_BBox.Add( ptP) ;
   return true ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::RemovePoint( const Point3d& ptP, int nId)
{
  // recupero gli elementi con la chiave
   int nKey = PointHash( Get1dCellNbr( ptP.x), Get1dCellNbr( ptP.y), Get1dCellNbr( ptP.z)) ;
   IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( nKey) ;
  // cerco quello con l'Id voluto
   for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
     // se coincidono punto e Id lo cancello
      if ( SqDist( (*MMrange.first).second.first, ptP) < ( EPS_SMALL * EPS_SMALL)  &&
           abs( (*MMrange.first).second.second) == abs( nId)) {
         m_MMap.erase( MMrange.first) ;
         return true ;
      }
   }

   return false ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::Find( const Point3d& ptTest, double dTol, INTVECTOR& vnIds)
{
  // pulisco il risultato
   vnIds.clear() ;
  // determino il range di celle sui tre assi
   IBox iBox ;
   if ( ! Get3dRangeNbr( ptTest, dTol, iBox))
      return false ;
  // ciclo su tutte le celle del range
   for ( int i = iBox.nXmin ; i <= iBox.nXmax ; ++ i) {
      for ( int j = iBox.nYmin ; j <= iBox.nYmax ; ++ j) {
         for ( int k = iBox.nZmin ; k <= iBox.nZmax ; ++ k) {
            IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
            for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
               if ( SqDist( (*MMrange.first).second.first, ptTest) < ( dTol * dTol))
                  vnIds.push_back( (*MMrange.first).second.second) ;
            }
         }
      }
   }

   return ( vnIds.size() > 0) ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::Find( const BBox3d& b3Test, INTVECTOR& vnIds)
{
  // pulisco il risultato
   vnIds.clear() ;
  // determino il range di celle sui tre assi
   IBox iBox ;
   if ( ! Get3dRangeNbr( b3Test, iBox))
      return false ;
  // ciclo su tutte le celle del range
   for ( int i = iBox.nXmin ; i <= iBox.nXmax ; ++ i) {
      for ( int j = iBox.nYmin ; j <= iBox.nYmax ; ++ j) {
         for ( int k = iBox.nZmin ; k <= iBox.nZmax ; ++ k) {
            IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
            for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
               if ( b3Test.Encloses( (*MMrange.first).second.first))
                  vnIds.push_back( (*MMrange.first).second.second) ;
            }
         }
      }
   }

   return ( vnIds.size() > 0) ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::Find( const Point3d& ptTest, double dTol, int& nId)
{
  // determino il range di celle sui tre assi
   IBox iBox ;
   if ( ! Get3dRangeNbr( ptTest, dTol, iBox))
      return false ;
  // ciclo su tutte le celle del range
   for ( int i = iBox.nXmin ; i <= iBox.nXmax ; ++ i) {
      for ( int j = iBox.nYmin ; j <= iBox.nYmax ; ++ j) {
         for ( int k = iBox.nZmin ; k <= iBox.nZmax ; ++ k) {
            IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
            for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
               if ( SqDist( (*MMrange.first).second.first, ptTest) < ( dTol * dTol)) {
                  nId = (*MMrange.first).second.second ;
                  return true ;
               }
            }
         }
      }
   }

   return false ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::FindNearest( const Point3d& ptTest, INTVECTOR& vnIds)
{
   const double NEAR_TOL = 100 * EPS_SMALL ;
  // pulisco il risultato
   vnIds.clear() ;
  // raggio di ricerca
   double dRad = max( m_BBox.DistFromPoint( ptTest), NEAR_TOL) ;
  // delta di incremento per raggio di ricerca
   const int N_CELL = 10 ;
   double dDelta = N_CELL * m_dCellDim ;
  // massimo numero di step di ricerca basato su raggio box di ingombro
   double dBoxRad ;
   if ( ! m_BBox.GetRadius( dBoxRad))
      return false ;
   int nMaxSteps = int( 2 * ceil( dBoxRad / dDelta)) ;
   nMaxSteps = max( nMaxSteps, 1) ;
  // ciclo di ricerca con raggio crescente
   IBox iBoxPrev ;
   IBox ibR[N_SUBIBOX] ;
   bool bFound = false ;
   double dMinDist ;
   double dSqMinDist ;
   for ( int m = 0 ; m < nMaxSteps ; ++ m) {
     // determino il range di celle sui tre assi
      IBox iBox ;
      if ( ! Get3dRangeNbr( ptTest, dRad, iBox))
         continue ;
     // tolgo il range precedente
      SubtractIBox( iBox, iBoxPrev, ibR) ;
     // ciclo su tutte le celle dei range rimanenti
      for ( int l = 0 ; l < N_SUBIBOX ; ++ l) {
         for ( int i = ibR[l].nXmin ; i <= ibR[l].nXmax ; ++ i) {
            for ( int j = ibR[l].nYmin ; j <= ibR[l].nYmax ; ++ j) {
               for ( int k = ibR[l].nZmin ; k <= ibR[l].nZmax ; ++ k) {
                 // ciclo sui punti della cella
                  IPNTI_UMMAP_CRANGE MMrange = m_MMap.equal_range( PointHash( i, j, k)) ;
                  for ( ; MMrange.first != MMrange.second ; ++ MMrange.first) {
                    // se distanza inferiore al minimo, aggiorno...
                     double dSqDist = SqDist( (*MMrange.first).second.first, ptTest) ;
                    // altro punto con la stessa minima distanza già trovata
                     if ( bFound  &&  fabs( dSqDist - dSqMinDist) < 2 * dMinDist * NEAR_TOL + NEAR_TOL * NEAR_TOL) {
                       // inserisco il punto nel vettore dei risultati
                        vnIds.push_back( (*MMrange.first).second.second) ;
                     }
                    // primo punto o punto con minima distanza più bassa
                     else if ( ! bFound  ||  dSqDist < dSqMinDist) {
                        bFound = true ;
                       // aggiorno i minimi 
                        dSqMinDist = dSqDist ;
                        dMinDist = sqrt( dSqMinDist) ;
                       // pulisco il vettore dei risultati ed inserisco questo
                        vnIds.clear() ;
                        vnIds.push_back( (*MMrange.first).second.second) ;
                     }
                  }
               }
            }
         }
      }
     // se trovato, inutile continuare perchè ci si allontanerà
      if ( bFound)
         return true ;
     // incremento il raggio di ricerca
      dRad += dDelta ;
     // salvo box di precedente ricerca
      iBoxPrev = iBox ;
   }

   return bFound ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::First( int& nId)
{
  // se vuoto
   if ( m_MMap.empty())
      return false ;
  // recupero il primo
   nId = (*m_MMap.begin()).second.second ;
   return true ;
}

//----------------------------------------------------------------------------
int
PointGrid3d::Get1dCellNbr( double dCoord)
{
   return static_cast<int>( floor( dCoord * m_dInvCellDim)) ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::Get3dRangeNbr( const BBox3d& b3Test, IBox& iBox)
{
  // calcolo intersezione tra box
   BBox3d b3Int ;
   if ( ! m_BBox.FindIntersection( b3Test, b3Int))
      return false ;
  // ricavo gli indici sui tre assi degli estremi del box
   iBox.nXmin = Get1dCellNbr( b3Int.GetMin().x) ;
   iBox.nYmin = Get1dCellNbr( b3Int.GetMin().y) ;
   iBox.nZmin = Get1dCellNbr( b3Int.GetMin().z) ;
   iBox.nXmax = Get1dCellNbr( b3Int.GetMax().x) ;
   iBox.nYmax = Get1dCellNbr( b3Int.GetMax().y) ;
   iBox.nZmax = Get1dCellNbr( b3Int.GetMax().z) ;
   return true ;
}

//----------------------------------------------------------------------------
bool
PointGrid3d::Get3dRangeNbr( const Point3d& ptTest, double dTol, IBox& iBox)
{
  // calcolo intersezione tra box
   BBox3d b3Int ;
   if ( ! m_BBox.FindIntersection( BBox3d( ptTest, dTol), b3Int))
      return false ;
  // ricavo gli indici sui tre assi degli estremi del box
   iBox.nXmin = Get1dCellNbr( b3Int.GetMin().x) ;
   iBox.nYmin = Get1dCellNbr( b3Int.GetMin().y) ;
   iBox.nZmin = Get1dCellNbr( b3Int.GetMin().z) ;
   iBox.nXmax = Get1dCellNbr( b3Int.GetMax().x) ;
   iBox.nYmax = Get1dCellNbr( b3Int.GetMax().y) ;
   iBox.nZmax = Get1dCellNbr( b3Int.GetMax().z) ;
   return true ;
}

//----------------------------------------------------------------------------
int
PointGrid3d::PointHash( int nX, int nY, int nZ)
{
   return ( nX * 73856093 ^ nY * 19349653 ^ nZ * 83492791) ;
}