//----------------------------------------------------------------------------
//  EgalTech 2013-2014
//----------------------------------------------------------------------------
// File : ChainCurves.cpp           Data : 20.07.14       Versione : 1.5g3
// Contenuto : Implementazione della funzione ChainCurves, per creare una o più
//             curve composite a partire dalle curve date.
//
//
// Modifiche : 20.07.14 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "/EgtDev/Include/EGkChainCurves.h"
#include "/EgtDev/Include/EGkGeomDB.h"
#include "/EgtDev/Include/EGkCurve.h"
#include "/EgtDev/Include/EGkCurveComposite.h"
#include <algorithm>

using namespace std ;

//----------------------------------------------------------------------------
bool
ChainCurves::Init( bool bAllowInvert, double dToler, int nCrvNbrHint, double dCellDim)
{
   m_bAllowInvert = bAllowInvert ;
   m_dToler = max( dToler, EPS_SMALL) ;
   m_sCrvId.clear() ;
   m_sCrvId.rehash( nCrvNbrHint) ;
   m_vCrvData.clear() ;
   m_vCrvData.reserve( nCrvNbrHint) ;
   m_PointGrid.Init( 2 * nCrvNbrHint, dCellDim) ;
   m_bFromNear = false ;
   m_vForkData.clear() ;
   m_bIsFork = false ;
   m_vFork.clear() ;
   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::AddCurve( int nId, const Point3d& ptStart, const Vector3d& vtStart,
                       const Point3d& ptEnd, const Vector3d& vtEnd)
{
  // verifico validità Id
   if ( nId <= 0)
      return false ;
  // verifico non sia già stata aggiunta la stessa entità
   if ( ! m_sCrvId.insert( nId).second)
      return true ;
  // inserisco i dati della curva nel vettore
   m_vCrvData.emplace_back( nId, ptStart, vtStart, ptEnd, vtEnd) ;
   int nV = int( m_vCrvData.size()) ;
  // li inserisco nel PointGrid3d 
   m_PointGrid.InsertPoint( ptStart, nV) ;
   m_PointGrid.InsertPoint( ptEnd, - nV) ;
   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::GetChainFromNear( const Point3d& ptStart, bool bHaltOnFork, INTVECTOR& vIds)
{
  // pulisco il risultato
   vIds.clear() ;
   m_bFromNear = true ;
   m_bIsFork = false ;
   m_vFork.clear() ;

  // recupero l'entità più vicina al punto di start
   int nStart ;
   INTVECTOR vStart ;
   if ( ! m_PointGrid.FindNearest( ptStart, vStart)  ||
        ! ChooseStart( ptStart, vStart, nStart)) {
      m_bFromNear = false ;
      return false ;
   }
   // recupero indice e verso
   int nId = abs( nStart) - 1 ;
   bool bEquiv = true ;
   // tolgo dal grid
   RemoveEntityFromGrid( nId) ;

  // se devo fermarmi su biforcazione, verifico se sono già su vecchia biforcazione
   bool bSkip = false ;
   if ( bHaltOnFork) {
      auto iIter = GetForkPoint( m_vCrvData[nId].ptEnd) ;
      if ( iIter != m_vForkData.end()) {
         m_bIsFork = true ;
         m_vFork.insert( m_vFork.end(), iIter->vnFork.begin(), iIter->vnFork.end()) ;
         bSkip = true ;
      }
   }

  // concateno dopo la fine dell'entità di partenza
   INTVECTOR vIdsAfter ;
   bool bClosed = false ;
   if ( ! bSkip  &&  ! GetChainFromPoint( m_vCrvData[nId].ptEnd, m_vCrvData[nId].vtEnd,
                                          m_vCrvData[nId].ptStart, bHaltOnFork, vIdsAfter, bClosed)) {
      m_bFromNear = false ;
      return false ;
   }

  // se devo fermarmi su biforcazione, verifico se sono già su vecchia biforcazione
   bool bRevSkip = false ;
   if ( bHaltOnFork) {
      auto iIter = GetForkPoint( m_vCrvData[nId].ptStart) ;
      if ( iIter != m_vForkData.end()) {
         m_bIsFork = true ;
         m_vFork.insert( m_vFork.end(), iIter->vnFork.begin(), iIter->vnFork.end()) ;
         bRevSkip = true ;
      }
   }

  // se non ho già chiuso l'anello, concateno prima dell'inizio dell'entità di partenza
   INTVECTOR vIdsBefore ;
   if ( ! bClosed) {
      bool bRevClosed ;
      if ( ! bRevSkip  &&  ! GetReverseChainFromPoint( m_vCrvData[nId].ptStart, m_vCrvData[nId].vtStart,
                                                       m_vCrvData[nId].ptEnd, bHaltOnFork, vIdsBefore, bRevClosed)) {
         m_bFromNear = false ;
         return false ;
      }
     // inverto l'ordine
      reverse( vIdsBefore.begin(), vIdsBefore.end()) ;
   }

  // inserisco i risultati nel vettore complessivo
   vIds.reserve( vIdsBefore.size() + 1 + vIdsAfter.size()) ;
   vIds.insert( vIds.end(), vIdsBefore.begin(), vIdsBefore.end()) ;
   AddToChain( nId, bEquiv, vIds) ;
   vIds.insert( vIds.end(), vIdsAfter.begin(), vIdsAfter.end()) ;

   m_bFromNear = false ;
   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::GetChainFromPoint( const Point3d& ptStart, const Vector3d& vtStart,
                                const Point3d& ptStop, bool bHaltOnFork,
                                INTVECTOR& vIds, bool& bStopped)
{
  // pulisco il risultato
   vIds.clear() ;
   bStopped = false ;
   if ( ! m_bFromNear) {
      m_bIsFork = false ;
      m_vFork.clear() ;
   }

  // concateno in senso normale
   Point3d ptCurr = ptStart ;
   Vector3d vtCurr = vtStart ;
   int nNext ;
   INTVECTOR vNext ;
   while ( m_PointGrid.Find( ptCurr, m_dToler, vNext)  &&
           ChooseNext( ptCurr, vtCurr, vNext, bHaltOnFork, nNext)) {
     // recupero indice e verso
      int nId = abs( nNext) - 1 ;
      bool bEquiv = ( nNext > 0) ;
     // inserisco nella concatenazione
      AddToChain( nId, bEquiv, vIds) ;
     // tolgo dal grid
      RemoveEntityFromGrid( nId) ;
     // determino il prossimo inizio
      ptCurr = bEquiv ? m_vCrvData[nId].ptEnd : m_vCrvData[nId].ptStart ;
      vtCurr = bEquiv ? m_vCrvData[nId].vtEnd : - m_vCrvData[nId].vtStart ;
     // verifico se sono arrivato al punto di chiusura
      if ( AreSamePointEpsilon( ptCurr, ptStop, 0.5 * EPS_SMALL)) {
         bStopped = true ;
         break ;
      }
     // se devo fermarmi su biforcazione, verifico se sono su vecchia biforcazione
      if ( bHaltOnFork) {
         auto iIter = GetForkPoint( ptCurr) ;
         if ( iIter != m_vForkData.end()) {
            m_bIsFork = true ;
            m_vFork.insert( m_vFork.end(), iIter->vnFork.begin(), iIter->vnFork.end()) ;
            break ;
         }
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::GetReverseChainFromPoint( const Point3d& ptStart, const Vector3d& vtStart,
                                       const Point3d& ptStop,  bool bHaltOnFork,
                                       INTVECTOR& vIds, bool& bStopped)
{
  // pulisco il risultato
   vIds.clear() ;
   bStopped = false ;
   if ( ! m_bFromNear) {
      m_bIsFork = false ;
      m_vFork.clear() ;
   }

  // concateno in senso invertito
   Point3d ptCurr = ptStart ;
   Vector3d vtCurr = vtStart ;
   int nPrev ;
   INTVECTOR vPrev ;
   while ( m_PointGrid.Find( ptCurr, m_dToler, vPrev)  &&
           ChoosePrev( ptCurr, vtCurr, vPrev, bHaltOnFork, nPrev)) {
     // recupero indice e verso
      int nId = abs( nPrev) - 1 ;
      bool bEquiv = ( nPrev < 0) ;
     // inserisco nella concatenazione
      AddToChain( nId, bEquiv, vIds) ;
     // tolgo dal grid
      RemoveEntityFromGrid( nId) ;
     // determino il prossimo inizio
      ptCurr = bEquiv ? m_vCrvData[nId].ptStart : m_vCrvData[nId].ptEnd ;
      vtCurr = bEquiv ? m_vCrvData[nId].vtStart : - m_vCrvData[nId].vtEnd ;
     // verifico se sono arrivato al punto di stop
      if ( AreSamePointEpsilon( ptCurr, ptStop, m_dToler)) {
         bStopped = true ;
         break ;
      }
     // se devo fermarmi su biforcazione, verifico se sono su vecchia biforcazione
      if ( bHaltOnFork) {
         auto iIter = GetForkPoint( ptCurr) ;
         if ( iIter != m_vForkData.end()) {
            m_bIsFork = true ;
            m_vFork.insert( m_vFork.end(), iIter->vnFork.begin(), iIter->vnFork.end()) ;
            break ;
         }
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::AddToChain( int nId, bool bEquiv, INTVECTOR& vIds)
{
   try { vIds.push_back( ( bEquiv ? m_vCrvData[nId].nId : - m_vCrvData[nId].nId)) ; }
   catch (...) { return false ; }
   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::RemoveEntityFromGrid( int nId)
{
   bool bOk = m_PointGrid.RemovePoint( m_vCrvData[nId].ptStart, ( nId + 1)) ;
   bOk = m_PointGrid.RemovePoint( m_vCrvData[nId].ptEnd, - ( nId + 1)) && bOk ;
   return bOk ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::ChooseStart( const Point3d& ptStart, const INTVECTOR& vStart, int& nStart)
{
  // numero di entità candidate
   int nSize = int( vStart.size()) ;

  // se nessuna, errore
   if ( nSize == 0)
      return false ;

  // se una, la ritorno
   if ( nSize == 1) {
      nStart = vStart[0] ;
      return true ;
   }

  // altrimenti, cerco la migliore
   int nI = - 1 ;
   double dDistMin = INFINITO ;
   for ( int i = 0 ; i < nSize ; ++ i) {
     // recupero indice e verso
      int nId = abs( vStart[i]) - 1 ;
      bool bEquiv = ( vStart[i] > 0) ;
     // calcolo un punto vicino all'estremo lungo la tangente
      Point3d ptNear = ( bEquiv ? m_vCrvData[nId].ptStart + m_vCrvData[nId].vtStart :
                                  m_vCrvData[nId].ptEnd - m_vCrvData[nId].vtEnd) ;
      double dDist = Dist( ptStart, ptNear) ;
      // tengo il segmento più vicino al punto
      // favorendo eventualmente quello equiverso se entro EPS da un concorrente
      if ( dDist < dDistMin - EPS_SMALL  ||
         ((dDist < dDistMin + EPS_SMALL)  &&  bEquiv  &&  vStart[nI] < 0)) {
         dDistMin = dDist ;
         nI = i ;
      }
   }

  // se non trovato
   if ( nI < 0)
      return false ;

  // altrimenti assegno il migliore
   nStart = vStart[nI] ;
   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::ChooseNext( const Point3d& ptCurr, const Vector3d& vtCurr, const INTVECTOR& vNext, bool bHaltOnFork, int& nNext)
{
   INTVECTOR vMyNext = vNext ;
  // scarto quelle entità che sono più vicine all'altro estremo del più vicino
   int nM = -1 ;
   Point3d ptRef ;
   double dSqMinDist = m_dToler * m_dToler ;
   for ( int i = 0 ; i < int( vMyNext.size()) ; ++ i) {
     // recupero indice e verso
      int nId = abs( vMyNext[i]) - 1 ;
      bool bEquiv = ( vMyNext[i] > 0) ;
     // determino minima distanza
      double dSqDist = SqDist( ptCurr, ( bEquiv ? m_vCrvData[nId].ptStart : m_vCrvData[nId].ptEnd)) ;
      if ( dSqDist < dSqMinDist) {
         ptRef = ( bEquiv ? m_vCrvData[nId].ptEnd : m_vCrvData[nId].ptStart) ;
         dSqMinDist = dSqDist ;
         nM = i ;
      }
   }
   for ( int i = 0 ; i < int( vMyNext.size()) ; ++ i) {
     // salto l'entità più vicina
      if ( i == nM)
         continue ;
     // recupero indice e verso
      int nId = abs( vMyNext[i]) - 1 ;
      bool bEquiv = ( vMyNext[i] > 0) ;
     // verifico se più vicino al più vicino
      double dCurrSqDist = SqDist( ptCurr, ( bEquiv ? m_vCrvData[nId].ptStart : m_vCrvData[nId].ptEnd)) ;
      double dRefSqDist = SqDist( ptRef, ( bEquiv ? m_vCrvData[nId].ptStart : m_vCrvData[nId].ptEnd)) ;
      if ( dRefSqDist < dCurrSqDist)
         vMyNext[i] = 0 ;
   }
  // cerco la direzione più vicina
   int nI = -1 ;
   int nF = 0 ;
   INTVECTOR vFork ;
   double dProScaMax = - 1.1 ;
   for ( int i = 0 ; i < int( vMyNext.size()) ; ++ i) {
     // salto gli scartati
      if ( vMyNext[i] == 0)
         continue ;
     // recupero indice e verso
      int nId = abs( vMyNext[i]) - 1 ;
      bool bEquiv = ( vMyNext[i] > 0) ;
     // incremento contatore indice entità tra cui scegliere
      ++ nF ;
      vFork.push_back( vMyNext[i]) ;
     // se vietata inversione, salto se controverso
      if ( ! m_bAllowInvert  &&  ! bEquiv)
         continue ;
     // determino scarto angolare
      Vector3d vtDir = ( bEquiv ? m_vCrvData[nId].vtStart : - m_vCrvData[nId].vtEnd) ;
      double dProSca = vtCurr * vtDir ;
      if ( dProSca > dProScaMax) {
         dProScaMax = dProSca ;
         nI = i ;
      }
   }

  // se non trovato
   if ( nI < 0) {
      m_bIsFork = false ;
      return false ;
   }

  // se biforcazione, aggiungo il punto nel vettore relativo
   if ( nF > 1) {
      if ( GetForkPoint( ptCurr) == m_vForkData.end())
         m_vForkData.emplace_back( ptCurr, vFork) ;
   }

  // se richiesto arresto su biforcazione e trovata biforcazione
   if ( bHaltOnFork  &&  nF > 1) {
      m_bIsFork = true ;
      m_vFork.insert( m_vFork.end(), vFork.begin(), vFork.end()) ;
      return false ;
   }

  // altrimenti assegno il migliore
   nNext = vMyNext[nI] ;
   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::ChoosePrev( const Point3d& ptCurr, const Vector3d& vtCurr, const INTVECTOR& vPrev, bool bHaltOnFork, int& nPrev)
{
   INTVECTOR vMyPrev = vPrev ;
  // scarto quelle entità che sono più vicine all'altro estremo del più vicino
   int nM = -1 ;
   Point3d ptRef ;
   double dSqMinDist = m_dToler * m_dToler ;
   for ( int i = 0 ; i < int( vMyPrev.size()) ; ++ i) {
     // recupero indice e verso
      int nId = abs( vMyPrev[i]) - 1 ;
      bool bEquiv = ( vMyPrev[i] < 0) ;
     // determino minima distanza
      double dSqDist = SqDist( ptCurr, ( bEquiv ? m_vCrvData[nId].ptEnd : m_vCrvData[nId].ptStart)) ;
      if ( dSqDist < dSqMinDist) {
         ptRef = ( bEquiv ? m_vCrvData[nId].ptStart : m_vCrvData[nId].ptEnd) ;
         dSqMinDist = dSqDist ;
         nM = i ;
      }
   }
   for ( int i = 0 ; i < int( vMyPrev.size()) ; ++ i) {
     // salto l'entità più vicina
      if ( i == nM)
         continue ;
     // recupero indice e verso
      int nId = abs( vMyPrev[i]) - 1 ;
      bool bEquiv = ( vMyPrev[i] < 0) ;
     // verifico se più vicino al più vicino
      double dCurrSqDist = SqDist( ptCurr, ( bEquiv ? m_vCrvData[nId].ptEnd : m_vCrvData[nId].ptStart)) ;
      double dRefSqDist = SqDist( ptRef, ( bEquiv ? m_vCrvData[nId].ptEnd : m_vCrvData[nId].ptStart)) ;
      if ( dRefSqDist < dCurrSqDist)
         vMyPrev[i] = 0 ;
   }
  // cerco la direzione più vicina
   int nI = - 1 ;
   int nF = 0 ;
   double dProScaMax = - 1.1 ; 
   INTVECTOR vFork ;
   for ( int i = 0 ; i < int( vMyPrev.size()) ; ++ i) {
     // salto gli scartati
      if ( vMyPrev[i] == 0)
         continue ;
     // recupero indice e verso
      int nId = abs( vMyPrev[i]) - 1 ;
      bool bEquiv = ( vMyPrev[i] < 0) ;
     // incremento contatore indice entità tra cui scegliere
      ++ nF ;
      vFork.push_back( vMyPrev[i]) ;
     // se vietata inversione, salto se controverso
      if ( ! m_bAllowInvert  &&  ! bEquiv)
         continue ;
     // determino scarto angolare
      Vector3d vtDir = ( bEquiv ? m_vCrvData[nId].vtEnd : - m_vCrvData[nId].vtStart) ;
      double dProSca = vtCurr * vtDir ;
      if ( dProSca > dProScaMax) {
         dProScaMax = dProSca ;
         nI = i ;
      }
   }

  // se non trovato
   if ( nI < 0) {
      m_bIsFork = false ;
      return false ;
   }

  // se biforcazione, aggiungo il punto nel vettore relativo
   if ( nF > 1) {
      if ( GetForkPoint( ptCurr) == m_vForkData.end())
         m_vForkData.emplace_back( ptCurr, vFork) ;
   }

  // se richiesto arresto su biforcazione e trovata biforcazione
   if ( bHaltOnFork  &&  nF > 1) {
      m_bIsFork = true ;
      m_vFork.insert( m_vFork.end(), vFork.begin(), vFork.end()) ;
      return false ;
   }

  // altrimenti assegno il migliore
   nPrev = vMyPrev[nI] ;
   return true ;
}

//----------------------------------------------------------------------------
bool
ChainCurves::GetForkIds( INTVECTOR& vForkIds)
{
  // pulisco il risultato
   vForkIds.clear() ;

  // verifico di essere su una diramazione
   if ( ! m_bIsFork  ||  m_vFork.empty())
      return false ;

  // assegno gli Id del GeomDB
   for ( int i = 0 ; i < int( m_vFork.size()) ; ++ i) {
     int nId = abs( m_vFork[i]) - 1 ;
     vForkIds.push_back( m_vCrvData[nId].nId) ;
   }
   return true ;
}

//----------------------------------------------------------------------------
ChainCurves::FDV_CONST_ITER
ChainCurves::GetForkPoint( const Point3d& ptP)
{
   return ( find_if( m_vForkData.begin(), m_vForkData.end(),
                     [&]( const ForkData& frkData) { return AreSamePointEpsilon( frkData.ptFork, ptP, m_dToler) ; })) ;
}