//----------------------------------------------------------------------------
//  EgalTech 2015-2022
//----------------------------------------------------------------------------
// File : IntersLineSurfTm.cpp      Data : 08.01.22         Versione : 2.4a3
// Contenuto : Implementazione della intersezione linea/superficie trimesh.
//
//
//
// Modifiche : 09.03.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "ProjPlane.h"
#include "IntersLineBox.h"
#include "/EgtDev/Include/EGkIntersLineTria.h"
#include "/EgtDev/Include/EGkIntersLineSurfTm.h"

using namespace std ;

//----------------------------------------------------------------------------
static void
UpdateInfoIntersLineSurfTm( const Point3d& ptL, const Vector3d& vtDir, double dLen,
                            int nStm, int nT, const Triangle3d& Tria, ILSIVECTOR& vInfo, bool bFinite)
{
   Point3d ptInt, ptInt2 ;
   int nRes = IntersLineTria( ptL, vtDir, dLen, Tria, ptInt, ptInt2, bFinite) ;
   if ( nRes == ILTT_IN  ||  nRes == ILTT_EDGE  ||  nRes == ILTT_VERT) {
      double dU = ( ptInt - ptL) * vtDir ;
      double dCosDN = vtDir * Tria.GetN() ;
      vInfo.emplace_back( nRes, dU, nStm, nT, dCosDN, ptInt) ;
   }
   else if ( nRes == ILTT_SEGM  ||  nRes == ILTT_SEGM_ON_EDGE) {
      double dU = ( ptInt - ptL) * vtDir ;
      double dU2 = ( ptInt2 - ptL) * vtDir ;
      double dCosDN = vtDir * Tria.GetN() ;
      vInfo.emplace_back( nRes, dU, dU2, nStm, nT, dCosDN, ptInt, ptInt2) ;
   }
}

//----------------------------------------------------------------------------
static void
OrderInfoIntersLineSurfTm( ILSIVECTOR& vInfo)
{
  // se non trovati, esco 
   if ( vInfo.empty())
      return ;
  // ordino il vettore delle intersezioni secondo il senso crescente del parametro di linea
   sort( vInfo.begin(), vInfo.end(),
            []( const IntLinStmInfo& a, const IntLinStmInfo& b)
               {  double dUa = ( ( a.nILTT == ILTT_SEGM  ||  a.nILTT == ILTT_SEGM_ON_EDGE) ? ( a.dU + a.dU2) / 2 : a.dU) ;
                  double dUb = ( ( b.nILTT == ILTT_SEGM  ||  b.nILTT == ILTT_SEGM_ON_EDGE) ? ( b.dU + b.dU2) / 2 : b.dU) ;
                  if ( abs( dUa - dUb) < EPS_SMALL)
                     return ( a.dCosDN < b.dCosDN) ;
                  return ( dUa < dUb) ; }) ;
}

//----------------------------------------------------------------------------
//  Intersezione di una linea con una superficie TriMesh
//----------------------------------------------------------------------------
bool
IntersLineSurfTm( const Point3d& ptL, const Vector3d& vtL, double dLen, const ISurfTriMesh& Stm,
                  ILSIVECTOR& vInfo, bool bFinite)
{
  // verifico linea
   Vector3d vtDir = vtL ;
   if ( ! vtDir.Normalize( EPS_ZERO))
      return false ;
  // verifico superficie
   if ( &Stm == nullptr)
      return false ;
  // verifico parametro di ritorno
   if ( &vInfo == nullptr)
      return false ;
   vInfo.clear() ;

  // limito la linea al box dei triangoli della superficie
   BBox3d b3Stm = Stm.GetAllTriaBox() ;
   if ( b3Stm.IsEmpty())
      return false ;
  // lo ingrandisco per non avere problemi con faccia piana su piani canonici
   b3Stm.Expand( 10 * EPS_SMALL) ;
   double dU1, dU2 ;
   if ( ! IntersLineBox( ptL, vtDir, b3Stm.GetMin() , b3Stm.GetMax(), dU1, dU2))
      return true ;
   if ( bFinite) {
      dU1 = max( dU1, 0.) ;
      dU2 = min( dU2, dLen) ;
      if ( dU2 - dU1 < EPS_SMALL)
         return true ;
   }
   Point3d ptStart = ptL + dU1 * vtDir ;
   double dLenEff = dU2 - dU1 ;
  // cerco i triangoli intersecati dalla linea
   const double BOX_STEP = 10 ;
   int nStep = int( ceil( dLenEff / BOX_STEP)) ;
   Vector3d vtStep = dLenEff / nStep * vtDir ;
   INTVECTOR vPrevT ;
   for ( int i = 0 ; i < nStep ; ++ i) {
      BBox3d b3Box( ptStart + i * vtStep, ptStart + ( i + 1) * vtStep) ;
      INTVECTOR vT ;
      if ( Stm.GetAllTriaOverlapBox( b3Box, vT)) {
         for ( auto nT : vT) {
            if ( find( vPrevT.begin(), vPrevT.end(), nT) == vPrevT.end()) {
               vPrevT.emplace_back( nT) ;
               Triangle3d Tria ;
               Stm.GetTriangle( nT, Tria) ;
              // aggiorno info con intersezione
               UpdateInfoIntersLineSurfTm( ptL, vtDir, dLen, 0, nT, Tria, vInfo, bFinite) ;
            }
         }
      }
   }

  // ordino il vettore delle eventuali intersezioni secondo il senso crescente del parametro di linea
   OrderInfoIntersLineSurfTm( vInfo) ;

   return true ;
}

//----------------------------------------------------------------------------
//  Intersezione di molte linee parallele con una superficie TriMesh
//----------------------------------------------------------------------------
IntersParLinesSurfTm::IntersParLinesSurfTm( const Frame3d& frLines, const ISurfTriMesh& Stm)
   : m_bOk( false), m_frLines( frLines), m_vpSTm( {&Stm})
{
  // verifico esistenza superficie
   if ( m_vpSTm[0] == nullptr  ||  ! m_vpSTm[0]->IsValid())
      return ;

  // aggiorno il vettore degli indici di base per mappare i triangoli con le rispettivi superfici
  // ( in questo caso la superficie è unica, quindi ho solo due elementi)
   m_vBaseInd = { 0, m_vpSTm[0]->GetTriangleCount()} ;

  // creo HashGrid 2d ed eventualmente attivo la griglia
   const int LIM_HG_TRIA = 127 ;
   m_HGrids.SetActivationGrid( m_vBaseInd.back() > LIM_HG_TRIA) ;

  // riempio HashGrid
   Triangle3d Tria ;
   int nT = m_vpSTm[0]->GetFirstTriangle( Tria) ;
   while ( nT != SVT_NULL) {
     // calcolo il BBox del triangolo nel riferimento scelto
      Tria.ToLoc( m_frLines) ;
      BBox3d b3Tria ;
      b3Tria.Add( Tria.GetP( 0)) ;
      b3Tria.Add( Tria.GetP( 1)) ;
      b3Tria.Add( Tria.GetP( 2)) ;
     // inserisco nella griglia
      if ( ! m_HGrids.Add( nT, b3Tria))  // ( 0 + nT, Tria)
         return ;
     // passo al prossimo triangolo
      nT = m_vpSTm[0]->GetNextTriangle( nT, Tria) ;
   }
  // aggiorno
   m_bOk = m_HGrids.Update() ;
}

//----------------------------------------------------------------------------
//  Intersezione di molte linee parallele con un vettore di superfici TriMesh
//----------------------------------------------------------------------------
IntersParLinesSurfTm::IntersParLinesSurfTm( const Frame3d& frLines, const CISURFTMPVECTOR& vStm)
   : m_bOk( false), m_frLines( frLines), m_vpSTm( vStm), m_vBaseInd( {0})
{
  // verifico esistenza superfici
   if ( m_vpSTm.empty())
      return ;
   for ( const ISurfTriMesh* pStm : m_vpSTm) {
      if ( pStm == nullptr  ||  ! pStm->IsValid())
         return ;
   }

  // aggiorno il vettore degli indici di base per mappare i triangoli con le rispettivi superfici
  // NB. dal costruttore è già inizializzato a {0}
   for ( int i = 0 ; i < int( m_vpSTm.size()) ; ++ i)
      m_vBaseInd.emplace_back( m_vBaseInd.back() + m_vpSTm[i]->GetTriangleCount()) ;

  // creo HashGrid 2d ed eventualmente attivo la griglia
   const int LIM_HG_TRIA = 256 ;
   m_HGrids.SetActivationGrid( m_vBaseInd.back() > LIM_HG_TRIA) ;

  // riempio HashGrid
   for ( int i = 0 ; i < int( m_vpSTm.size()) ; ++ i) {
      Triangle3d Tria ;
      int nT = m_vpSTm[i]->GetFirstTriangle( Tria) ;
      while ( nT != SVT_NULL) {
        // calcolo il BBox del triangolo nel riferimento scelto
         Tria.ToLoc( m_frLines) ;
         BBox3d b3Tria ;
         b3Tria.Add( Tria.GetP( 0)) ;
         b3Tria.Add( Tria.GetP( 1)) ;
         b3Tria.Add( Tria.GetP( 2)) ;
        // inserisco nella griglia ( aggiungo shift per indice del triangolo)
         if ( ! m_HGrids.Add( m_vBaseInd[i] + nT, b3Tria))
            return ;
        // passo al prossimo triangolo
         nT = m_vpSTm[i]->GetNextTriangle( nT, Tria) ;
      }
   }
  // aggiorno
   m_bOk = m_HGrids.Update() ;
}

//----------------------------------------------------------------------------
bool
IntersParLinesSurfTm::GetInters( const Point3d& ptL, double dLen, ILSIVECTOR& vInfo, bool bFinite) const
{
  // verifico parametro di ritorno
   if ( &vInfo == nullptr)
      return false ;
   vInfo.clear() ;
  // verifico validità
   if ( ! m_bOk)
      return false ;

  // calcolo box linea (nel riferimento)
   BBox3d b3Line ;
   b3Line.Add( ptL) ;

  // calcolo punto nel riferimento trimesh
   Point3d ptLL = ptL ;
   ptLL.ToGlob( m_frLines) ;

  // recupero indici triangoli che intersecano box in 2d
   INTVECTOR vnIds ;
   if ( m_HGrids.Find( b3Line, vnIds)) {
      for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) {
        // recupero la superficie
         int nInd = vnIds[i] ;
         int nSurf = GetSurfInd( nInd) ;
         if ( nSurf == -1)
            return false ;
        // recupero il triangolo
         int nT = nInd - m_vBaseInd[nSurf] ;
         Triangle3d Tria ;
         if ( ! m_vpSTm[nSurf]->GetTriangle( nT, Tria))
            return false ;
        // aggiorno info con intersezione
         UpdateInfoIntersLineSurfTm( ptLL, m_frLines.VersZ(), dLen, nSurf, nT, Tria, vInfo, bFinite) ;
      }
   }

  // ordino il vettore delle eventuali intersezioni secondo il senso crescente del parametro di linea
   OrderInfoIntersLineSurfTm( vInfo) ;

   return true ;
}

//----------------------------------------------------------------------------
int
IntersParLinesSurfTm::GetSurfInd( int nT) const
{
  // verifico la presenza di almeno un intervallo
   if ( m_vBaseInd.size() < 2)
      return -1 ;
  // se la superficie è unica, allora non devo cercarla
   if ( int( m_vBaseInd.size()) == 2)
      return 0 ;
  // ricerca binaria dell'intervallo contenente la posizione del triangolo
   int nS = 0 ;
   int nE = int( m_vBaseInd.size()) - 1 ;
   while ( true) {
      if ( nT < m_vBaseInd[nS]  ||  nT >= m_vBaseInd[nE])
         return -1 ;
      if ( nE - nS == 1)
         return nS ;
      int nM = ( nS + nE) / 2 ;
      if ( nT == m_vBaseInd[nM])
         return nM ;
      if ( nT < m_vBaseInd[nM])
         nE = nM ;
      else
         nS = nM ;
   }
   return -1 ;
}

//----------------------------------------------------------------------------
bool
FilterLineSurfTmInters( const ILSIVECTOR& vInfo, INTDBLVECTOR& vInters)
{
  // ciclo sulle intersezioni
   for ( const auto& Info : vInfo) {
     // se intersezione puntuale
      if ( Info.nILTT == ILTT_VERT  ||  Info.nILTT == ILTT_EDGE  ||  Info.nILTT == ILTT_IN) {
         int nFlag = LST_TOUCH ;
         if ( Info.dCosDN > EPS_ZERO)
            nFlag = LST_OUT ;
         else if ( Info.dCosDN < -EPS_ZERO)
            nFlag = LST_IN ;
         vInters.emplace_back( nFlag, Info.dU) ;
      }
     // se altrimenti intersezione con coincidenza
      else if ( Info.nILTT == ILTT_SEGM  ||  Info.nILTT == ILTT_SEGM_ON_EDGE) {
         vInters.emplace_back( LST_TG_INI, Info.dU) ;
         vInters.emplace_back( LST_TG_FIN, Info.dU2) ;
      }
   }
  // elimino intersezioni ripetute
   for ( size_t j = 1 ; j < vInters.size() ; ) {
     // intersezione precedente
      size_t i = j - 1 ;
     // se hanno lo stesso parametro
      if ( abs( vInters[i].second - vInters[j].second) < EPS_SMALL) {
        // se sono entrambe entranti o uscenti, elimino la seconda
         if ( ( vInters[i].first == LST_IN  &&  vInters[j].first == LST_IN)  ||
              ( vInters[i].first == LST_OUT  &&  vInters[j].first == LST_OUT)) {
            vInters.erase( vInters.begin() + j) ;
            continue ;
         }
        // se una entrante e l'altra uscente, cambio in touch ed elimino la seconda
         else if ( ( vInters[i].first == LST_IN  &&  vInters[j].first == LST_OUT)  ||
                   ( vInters[i].first == LST_OUT  &&  vInters[j].first == LST_IN)) {
            vInters[i].first = LST_TOUCH ;
            vInters.erase( vInters.begin() + j) ;
            continue ;
         }
        // se una puntuale e l'altra inizio di coincidenza, elimino la prima
         else if ( ( vInters[i].first == LST_IN  ||  vInters[i].first == LST_OUT  ||  vInters[i].first == LST_TOUCH)  &&  vInters[j].first == LST_TG_INI) {
            vInters.erase( vInters.begin() + i) ;
            continue ;
         }
        // se una fine di coincidenza e l'altra puntuale, elimino la seconda
         else if ( vInters[i].first == LST_TG_FIN  &&  ( vInters[j].first == LST_IN  ||  vInters[j].first == LST_OUT  ||  vInters[j].first == LST_TOUCH)) {
            vInters.erase( vInters.begin() + j) ;
            continue ;
         }
        // se una fine di coincidenza e l'altra inizio di coincidenza, elimino entrambe
         else if ( i > 0  &&  vInters[i].first == LST_TG_FIN  &&  vInters[j].first == LST_TG_INI) {
            vInters.erase( vInters.begin() + j) ;
            vInters.erase( vInters.begin() + i) ;
            -- j ;
            continue ;
         }
      }
     // passo alla successiva
      ++ j ;
   }

   return true ;
}