//----------------------------------------------------------------------------
//  EgalTech 2018-2018
//----------------------------------------------------------------------------
// File : CAvToolSurfTm.cpp    Data : 08.05.18    Versione : 1.9e2
// Contenuto : Implementazione della classe CAvToolSurfTm.
//
//
//
// Modifiche : 27.04.18 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

#include "stdafx.h"
#include "CAvToolTriangle.h"
#include "CAvToolSurfTm.h"
#include "DllMain.h"
#include "/EgtDev/Include/EGkDistPointLine.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include <thread>
#include <future>

using namespace std ;

//----------------------------------------------------------------------------
const int STEP_PE = 50 ;

//----------------------------------------------------------------------------
ICAvToolSurfTm*
CreateCAvToolSurfTm( void)
{
   return static_cast<ICAvToolSurfTm*> ( new(nothrow) CAvToolSurfTm) ;
}


//----------------------------------------------------------------------------
// CAvToolSurfTm
//----------------------------------------------------------------------------
CAvToolSurfTm::CAvToolSurfTm( void)
   :  m_Tool( false)
{
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::SetSurfTm( const ISurfTriMesh& Stm)
{
  // pulisco la lista dei puntatori a Stm
   m_vSTM.clear() ;
  // pulisco e inizializzo la prima posizione della lista delle basi per gli indici dei triangoli
   m_vBaseInd.clear() ;
   m_vBaseInd.emplace_back( 0) ;
  // aggiungo la superficie
   return AddSurfTm( Stm) ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::AddSurfTm( const ISurfTriMesh& Stm)
{
  // verifico validità superficie
   const SurfTriMesh* pStm = GetBasicSurfTriMesh( &Stm) ;
   if ( pStm == nullptr  ||  ! pStm->IsValid())
      return false ;
  // inserisco superficie in lista
   m_vSTM.emplace_back( pStm) ;
  // inserisco base per indice triangoli prossima superficie
   m_vBaseInd.emplace_back( m_vBaseInd.back() + pStm->GetTriangleSize()) ;
   return true ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::SetStdTool( double dH, double dR, double dCornR)
{
   return m_Tool.SetStdTool( "", dH, dR, dCornR, 0, 0) ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::SetAdvTool( double dH, double dR, double dTipH, double dTipR, double dCornR)
{
   return m_Tool.SetAdvTool( "", dH, dR, dTipH, dTipR, dCornR, 0, 0) ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::SetSawTool( double dH, double dR, double dThick, double dStemR, double dCornR)
{
   return m_Tool.SetSawTool( "", dH, dR, dThick, dStemR, dCornR, 0) ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::SetGenTool( const ICurveComposite* pToolOutline)
{
   return m_Tool.SetGenTool( "", pToolOutline, 0) ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::TestPosition( const Point3d& ptT, const Vector3d& vtDir, const Vector3d& vtMove, double& dTotDist)
{
  // Se utensile non definito, errore
   if ( m_Tool.GetType() == Tool::UNDEF)
      return false ;
  // Se direzioni non definite, errore
   if ( vtDir.IsSmall()  ||  vtMove.IsSmall())
      return false ;
  // Imposto il riferimento di movimento
   if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
      m_frMove.Set( ORIG, vtMove, vtDir) ;
   else
      m_frMove.Set( ORIG, vtMove) ;
  // Eseguo controllo
   Point3d ptCurr = ptT ;
   dTotDist = MyTestPosition( ptCurr, vtDir) ;
   return ( dTotDist > - EPS_SMALL) ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::TestPath( PNTULIST& lPntM, const Vector3d& vtDir, const Vector3d& vtMove, double dLinTol)
{
  // Se utensile non definito, errore
   if ( m_Tool.GetType() == Tool::UNDEF)
      return false ;
  // Se direzioni non definite, errore
   if ( vtDir.IsSmall()  ||  vtMove.IsSmall())
      return false ;
  // Se lista vuota, non devo fare alcunché
   if ( lPntM.empty())
      return true ;
  // Imposto il riferimento di movimento
   if ( ! AreSameOrOppositeVectorApprox( vtDir, vtMove))
      m_frMove.Set( ORIG, vtMove, vtDir) ;
   else
      m_frMove.Set( ORIG, vtMove) ;
  // Predispongo Hash Grid
   if ( ! PrepareHashGrid())
      return false ;
  // Determino il numero di punti del path
   m_nTotPnt = int( lPntM.size()) ;
  // Recupero il numero massimo di thread concorrenti
   int nThreadMax = thread::hardware_concurrency() ;
   bool bOk = true ;
  // Se un solo thread o pochi punti
   if ( nThreadMax <= 1  ||  m_nTotPnt < 500) {
      bOk = TestSubPath( -1, lPntM, vtDir, dLinTol) ;
      ProcessEvents( 100, 0) ;
   }
  // altrimenti
   else {
      const int MAX_PARTS = 32 ;
      PNTULIST vlPntM[MAX_PARTS] ;
     // divido la lista in parti
      int nPartCnt = min( nThreadMax, MAX_PARTS) ;
      int nPartDim = m_nTotPnt / nPartCnt ;
      for ( int i = nPartCnt - 1 ; i > 0 ; -- i) {
         auto itSplit = prev( lPntM.end(), nPartDim) ;
         vlPntM[i].splice( vlPntM[i].end(), lPntM, itSplit, lPntM.end()) ;
         vlPntM[i].push_front( lPntM.back()) ;
      }
      vlPntM[0].splice( vlPntM[0].end(), lPntM) ;
     // processo le parti
      m_nCurrPnt = 0 ;
      m_bBreak = false ;
      future<bool> vRes[MAX_PARTS] ;
      for ( int i = 0 ; i < nPartCnt ; ++ i)
         vRes[i] = async( launch::async, &CAvToolSurfTm::TestSubPath, this, i, ref( vlPntM[i]), cref( vtDir), dLinTol) ;
     // attendo i risultati
      int nFin = 0 ;
      int nNextPE = 0 ;
      while ( nFin < nPartCnt) {
         for ( int i = 0 ; i < nPartCnt ; ++ i) {
            if ( vRes[i].valid()  &&  vRes[i].wait_for( chrono::nanoseconds{ 1}) == future_status::ready) {
               bOk = vRes[i].get() && bOk ;
               ++ nFin ;
            }
         }
         if ( m_nCurrPnt > nNextPE) {
            int nRes = ProcessEvents( int( m_nCurrPnt * 100. / m_nTotPnt), 10) ;
            nNextPE += STEP_PE ;
            if ( nRes == 1)
               m_bBreak = true ;
         }
      }
     // unisco le liste risultati
      for ( int i = 0 ; i < nPartCnt ; ++ i) {
         if ( i > 0)
            lPntM.pop_back() ;
         lPntM.splice( lPntM.end(), vlPntM[i]) ;
      }
      ProcessEvents( 100, 0) ;
   }
  // pulisco HashGrid 2d
   m_HGrids.Clear() ;
   return bOk ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::TestSubPath( int nId, PNTULIST& lPntM, const Vector3d& vtDir, double dLinTol)
{
  // Se lista vuota, non devo fare alcunché
   if ( lPntM.empty())
      return true ;
  // Ciclo sui punti
   Point3d ptPrev, ptCurr ;
   auto itPntMPrev = lPntM.end() ;
   auto itPntMCurr = lPntM.begin() ;
   while ( itPntMCurr != lPntM.end()) {
     // verifico il punto
      ptCurr = itPntMCurr->first ;
      double dMove = MyTestPositionHG( itPntMCurr->first, vtDir) ;
      itPntMCurr->second = - dMove ;
      if ( dMove < - EPS_SMALL)
         return false ;
     // se esiste il punto precedente devo verificare il medio
      if ( itPntMPrev != lPntM.end()) {
         MyTestMidPointHG( lPntM, itPntMPrev, itPntMCurr, ptPrev, ptCurr, vtDir, dLinTol, 1) ;
      }
     // passo al successivo
      ptPrev = ptCurr ;
      itPntMPrev = itPntMCurr ;
      ++ itPntMCurr ;
      ++ m_nCurrPnt ;
     // se singolo thread
      if ( nId == -1) {
        // gestione eventi (ogni STEP_PE punti)
         if (( m_nCurrPnt % STEP_PE) == 0) {
            int nRes = ProcessEvents( int( m_nCurrPnt * 100. / m_nTotPnt), 0) ;
            if ( nRes == 1)
               return false ;
         }
      }
     // altrimenti multithread
      else {
         if ( m_bBreak)
            return false ;
      }
   }
   return true ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::MyTestMidPointHG( PNTULIST& lPntM, const PNTULIST::iterator& itPntMPrev, const PNTULIST::iterator& itPntMCurr,
                                 const Point3d& ptPrev, const Point3d& ptCurr, const Vector3d& vtDir, double dLinTol, int nLev)
{
  // se superato limite di ricursione, esco
   const int MAX_LEV = 10 ;
   if ( nLev > MAX_LEV)
      return true ;
  // determino il punto medio tra gli originali 
   Point3d ptMid = Media( ptPrev, ptCurr, 0.5) ;
  // ne effettuo la correzione per evitare la collisione
   Point3d ptNewMid = ptMid ;
   double dMidMove = MyTestPositionHG( ptNewMid, vtDir) ;
   if ( dMidMove < - EPS_SMALL)
      return false ;
  // massima distanza ammissibile
   double dMaxSqDist = max( m_Tool.GetRefRadius() * m_Tool.GetRefRadius(), 0.5 * 0.5) ;
  // se punto medio oltre tolleranza dalla linea tra i punti includenti o linea troppo lunga, devo aggiungerlo
   if ( abs(( Media( itPntMPrev->first, itPntMCurr->first, 0.5) - ptNewMid) * m_frMove.VersZ()) > 0.5 * dLinTol  ||
        SqDist( itPntMPrev->first, itPntMCurr->first) > dMaxSqDist) {
     // aggiungo
      lPntM.emplace( itPntMCurr, ptNewMid, - dMidMove) ;
      auto itPntMMid = itPntMCurr ;
      -- itPntMMid ;
     // verifico intervallo precedente
      MyTestMidPointHG( lPntM, itPntMPrev, itPntMMid, ptPrev, ptMid, vtDir, dLinTol, nLev + 1) ;
     // verifico intervallo successivo
      MyTestMidPointHG( lPntM, itPntMMid, itPntMCurr, ptMid, ptCurr, vtDir, dLinTol, nLev + 1) ;
   }
   return true ;
}

//----------------------------------------------------------------------------
double
CAvToolSurfTm::MyTestPosition( Point3d& ptT, const Vector3d& vtDir)
{
   double dTotDist = 0 ;
   for ( auto pStm : m_vSTM) {
      Triangle3d Tria ;
      for ( int nTria = pStm->GetFirstTriangle( Tria) ;
            nTria != SVT_NULL ;
            nTria = pStm->GetNextTriangle( nTria, Tria)) {
         double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, m_frMove.VersZ()) ;
         if ( dDist < - EPS_SMALL)
            return -1 ;
         if ( dDist > EPS_SMALL) {
            dTotDist += dDist ;
            ptT += dDist * m_frMove.VersZ() ;
         }
      }
   }
   return dTotDist ;
}

//----------------------------------------------------------------------------
double
CAvToolSurfTm::MyTestPositionHG( Point3d& ptT, const Vector3d& vtDir)
{
  // calcolo box utensile nel riferimento di movimento
   BBox3d b3Tool ;
   Point3d ptTL = ptT ; ptTL.ToLoc( m_frMove) ;
   Vector3d vtDirL = vtDir ; vtDirL.ToLoc( m_frMove) ;
   b3Tool.Add( ptTL) ;
   b3Tool.Add( ptTL - vtDirL * m_Tool.GetHeigth()) ;
   if ( vtDirL.IsX())
      b3Tool.Expand( 0, m_Tool.GetRadius(), m_Tool.GetRadius()) ;
   else if ( vtDirL.IsY())
      b3Tool.Expand( m_Tool.GetRadius(), 0, m_Tool.GetRadius()) ;
   else if ( vtDirL.IsZ())
      b3Tool.Expand( m_Tool.GetRadius(), m_Tool.GetRadius(), 0) ;
   else {
      double dExpandX = m_Tool.GetRadius() * sqrt( 1 - vtDirL.x * vtDirL.x) ;
      double dExpandY = m_Tool.GetRadius() * sqrt( 1 - vtDirL.y * vtDirL.y) ;
      double dExpandZ = m_Tool.GetRadius() * sqrt( 1 - vtDirL.z * vtDirL.z) ;
      b3Tool.Expand( dExpandX, dExpandY, dExpandZ) ;
   }
  // ciclo sui triangoli che intersecano box in 2d 
   double dTotDist = 0 ;
   INTVECTOR vnIds ;
   if ( m_HGrids.Find( b3Tool, vnIds)) {
      for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) {
         int nInd = vnIds[i] ;
         int nSurf = GetSurfInd( nInd) ;
         if ( nSurf == -1)
            return -1 ;
         int nT = nInd - m_vBaseInd[nSurf] ;
         Triangle3d Tria ;
         if ( ! m_vSTM[nSurf]->GetTriangle( nT, Tria))
            return -1 ;
         double dDist = CAvToolTriangle( m_Tool, ptT, vtDir, Tria, m_frMove.VersZ()) ;
         if ( dDist > EPS_SMALL) {
            dTotDist += dDist ;
            ptT += dDist * m_frMove.VersZ() ;
         }
         else if ( dDist < -EPS_SMALL)
            return -1 ;
      }
   }
   return dTotDist ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::PrepareHashGrid( void)
{
  // pulisco HashGrid 2d
   m_HGrids.Clear() ;
  // verifico esistenza superfici
   if ( m_vSTM.size() == 0  ||  m_vBaseInd.size() < m_vSTM.size() + 1)
      return false ;
  // creo HashGrid 2d
   const int LIM_HG_TRIA = 256 ;
   m_HGrids.SetActivationGrid( m_vBaseInd.back() > LIM_HG_TRIA) ;
  // riempio HashGrid 
   for ( int i = 0 ; i < int( m_vSTM.size()) ; ++ i) {
      Triangle3d Tria ;
      int nT = m_vSTM[i]->GetFirstTriangle( Tria) ;
      while ( nT != SVT_NULL) {
        // calcolo il BBox del triangolo nel riferimento scelto
         Tria.ToLoc( m_frMove) ;
         BBox3d b3Tria ;
         Tria.GetLocalBBox( b3Tria) ;
        // inserisco nella griglia
         if ( ! m_HGrids.Add( m_vBaseInd[i] + nT, b3Tria))
            return false ;
        // passo al prossimo triangolo 
         nT = m_vSTM[i]->GetNextTriangle( nT, Tria) ;
      }
   }
  // aggiorno
   return m_HGrids.Update() ;
}

//----------------------------------------------------------------------------
int
CAvToolSurfTm::GetSurfInd( int nT)
{
  // verifico la presenza di almeno un intervallo
   if ( m_vBaseInd.size() < 2)
      return -1 ;
  // 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 ;
}