//----------------------------------------------------------------------------
//  EgalTech 2024-2024
//----------------------------------------------------------------------------
// File : CAvSilhouetteSurfTm.cpp    Data : 08.06.24    Versione : 2.6f2
// Contenuto : Implementazione della funzione CAvSilhouetteSurfTm.
//
//
//
// Modifiche : 08.06.24 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

#include "stdafx.h"
#include "CAvSilhouetteSurfTm.h"
#include "GeoConst.h"
#include "/EgtDev/Include/EGkChainCurves.h"
#include "/EgtDev/Include/EGkOffsetCurve.h"
#include <thread>
#include <future>

using namespace std ;

//----------------------------------------------------------------------------
// Funzioni locali per calcolo isolinee con metodo Marching Squares
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// Quadrato (C=corner E=edge) :
// 
//      C3 - E2 - C2
//      |          |
//      E3        E1
//      |          |
//      C0 - E0 - C1
// 
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
static Point3d
CalcPoint( const Point3d& ptS, const Point3d& ptE, double dLevel,
           const ICAvToolSurfTm& cavTstm, const Frame3d &frGrid)
{
  // Distanza tra gli estremi iniziali
   double dDist = max( abs( ptS.x - ptE.x), abs( ptS.y - ptE.y)) ;

  // Ricerca con metodo di bisezione (si arresta quando il medio è allineato agli estremi)
   const int MAX_ITER = 10 ;
   int nCount = 0 ;
   Point3d ptP1 = ptS ;
   Point3d ptP2 = ptE ;
   Point3d ptMid = Media( ptP1, ptP2) ;
   while ( nCount < MAX_ITER) {
     // verifica del punto medio
      ptMid.z = 0 ;
      Point3d ptTest = GetToGlob( ptMid + cavTstm.GetToolHeight() * Z_AX, frGrid) ;
      double dMove ;
      cavTstm.TestPosition( ptTest, frGrid.VersZ(), frGrid.VersZ(), dMove) ;
      ptMid.z = dMove ;
     // se sta sulla linea che unisce gli estremi, basta interpolazione lineare
      if ( abs( ptMid.z - ( ptP1.z + ptP2.z) / 2) < EPS_SMALL / 2)
         return Media( ptP1, ptP2, ( ptP1.z - dLevel) / ( ptP1.z - ptP2.z)) ;
     // altrimenti nuova suddivisione della parte con estremi da parte opposta rispetto al livello
      bool b1On = ( ptP1.z > dLevel) ;
      bool bMidOn = ( ptMid.z > dLevel) ;
      if ( b1On != bMidOn)
         ptP2 = ptMid ;
      else
         ptP1 = ptMid ;
      ptMid = Media( ptP1, ptP2) ;
      ++ nCount ;
   }
   ptMid.z = dLevel ;
   return ptMid ;
}

//----------------------------------------------------------------------------
static int
ProcessSquare( int nFlag, int& nI1s, int& nI1e, int& nI2s, int& nI2e)
{
   static int LineTable[16][5] = { { 0, -1, -1, -1, -1},
                                   { 1,  0,  3, -1, -1},
                                   { 1,  1,  0, -1, -1},
                                   { 1,  1,  3, -1, -1},
                                   { 1,  2,  1, -1, -1},
                                   { 2,  0,  1,  2,  3},
                                   { 1,  2,  0, -1, -1},
                                   { 1,  2,  3, -1, -1},
                                   { 1,  3,  2, -1, -1},
                                   { 1,  0,  2, -1, -1},
                                   { 2,  1,  2,  3,  0},
                                   { 1,  1,  2, -1, -1},
                                   { 1,  3,  1, -1, -1},
                                   { 1,  0,  1, -1, -1},
                                   { 1,  3,  0, -1, -1},
                                   { 0, -1, -1, -1, -1}} ;
  // flag fuori dai limiti
   if ( nFlag < 0  ||  nFlag > 15)
      return -1 ;
  // nessuna linea
   if ( LineTable[nFlag][0] == 0)
      return 0 ;
  // una linea
   else if ( LineTable[nFlag][0] == 1) {
      nI1s = LineTable[nFlag][1] ;
      nI1e = LineTable[nFlag][2] ;
      return 1 ;
   }
  // due linee
   else if ( LineTable[nFlag][0] == 2) {
      nI1s = LineTable[nFlag][1] ;
      nI1e = LineTable[nFlag][2] ;
      nI2s = LineTable[nFlag][3] ;
      nI2e = LineTable[nFlag][4] ;
      return 2 ;
   }
   return -1 ;
}

//----------------------------------------------------------------------------
static bool
TestSubEdges( unordered_map<int, Point3d>& umEdgePnt, const INTVECTOR& vEdgeInd, int nFirst, int nLast,
              const DBLVECTOR& vdGrid, int nStepX, double dStep,
              double dLevel, const ICAvToolSurfTm& cavTstm, const Frame3d &frGrid)
{
   for ( int k = nFirst ; k <= nLast ; ++ k) {
     // recupero i differenti indici
      int nKey = vEdgeInd[k] ;
      int nInd = nKey / 2 ;
      bool bOnX = ( ( nKey % 2) == 0) ;
      int j = nInd / ( nStepX + 1) ;
      int i = nInd % ( nStepX + 1) ;
     // determino i punti estremi dell'edge
      Point3d ptS{ i * dStep, j * dStep, vdGrid[nInd]} ;
      Point3d ptE{ ptS.x + ( bOnX ? dStep : 0), ptS.y + ( bOnX ? 0 : dStep), vdGrid[nInd + ( bOnX ? 1 : nStepX + 1)]} ;
     // calcolo il punto
      Point3d ptQ = CalcPoint( ptS, ptE, dLevel, cavTstm, frGrid) ;
      umEdgePnt[ nKey] = ptQ ;
   }
   return true ;
}

//----------------------------------------------------------------------------
static bool
MarchingSquares( const DBLVECTOR& vdGrid, int nStepX, int nStepY, double dStep, double dRad,
                 double dLevel, const ICAvToolSurfTm& cavTstm, const Frame3d &frGrid, POLYLINEVECTOR& vPL)
{
  // Analizzo gli edge da cui passano le curve cercate
   unordered_map<int, Point3d> umEdgePnt( 4 * ( nStepX + nStepY)) ;
   INTVECTOR vEdgeInd ;
   vEdgeInd.reserve( 4 * ( nStepX + nStepY)) ;
   for ( int j = 0 ; j < nStepY ; ++ j) {
      for ( int i = 0 ; i < nStepX ; ++ i) {
        // indici dei vertici nella griglia
         int nInd0 = i + j * ( nStepX + 1) ;
         int nInd1 = ( i + 1) + j * ( nStepX + 1) ;
         int nInd2 = ( i + 1) + ( j + 1) * ( nStepX + 1) ;
         int nInd3 = i + ( j + 1) * ( nStepX + 1) ;
        // flag del quadrato
         bool bUp0 = ( vdGrid[nInd0] > dLevel) ;
         bool bUp1 = ( vdGrid[nInd1] > dLevel) ;
         bool bUp2 = ( vdGrid[nInd2] > dLevel) ;
         bool bUp3 = ( vdGrid[nInd3] > dLevel) ;
        // se tutti uguali, passo al successivo
         if ( bUp0 == bUp1  &&  bUp0 == bUp2  &&  bUp0 == bUp3)
            continue ;
        // verifico quali calcolare
         if ( bUp0 != bUp1) {
            int nKey = 2 * nInd0 ;
            if ( umEdgePnt.find( nKey) == umEdgePnt.end()) {
               umEdgePnt[ nKey] = P_INVALID ;
               vEdgeInd.emplace_back( nKey) ;
            }
         }
         if ( bUp3 != bUp2) {
            int nKey = 2 * nInd3 ;
            if ( umEdgePnt.find( nKey) == umEdgePnt.end()) {
               umEdgePnt[ nKey] = P_INVALID ;
               vEdgeInd.emplace_back( nKey) ;
            }
         }
         if ( bUp0 != bUp3) {
            int nKey = 2 * nInd0 + 1 ;
            if ( umEdgePnt.find( nKey) == umEdgePnt.end()) {
               umEdgePnt[ nKey] = P_INVALID ;
               vEdgeInd.emplace_back( nKey) ;
            }
         }
         if ( bUp1 != bUp2) {
            int nKey = 2 * nInd1 + 1 ;
            if ( umEdgePnt.find( nKey) == umEdgePnt.end()) {
               umEdgePnt[ nKey] = P_INVALID ;
               vEdgeInd.emplace_back( nKey) ;
            }
         }
      }
   }
  // Numero di edge da valutare
   int nEdgeCnt = int( vEdgeInd.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  ||  nEdgeCnt < 50) {
      TestSubEdges( umEdgePnt, vEdgeInd, 0, nEdgeCnt - 1, vdGrid, nStepX, dStep, dLevel, cavTstm, frGrid) ;
   }
  // altrimenti
   else {
      const int MAX_PARTS = 32 ;
      INTINTVECTOR vFstLst( MAX_PARTS) ;
     // calcolo le parti del vettore
      int nPartCnt = min( nThreadMax, MAX_PARTS) ;
      int nPartDim = nEdgeCnt / nPartCnt + 1 ;
      for ( int i = 0 ; i < nPartCnt ; ++ i) {
         vFstLst[i].first = i * nPartDim ;
         vFstLst[i].second = min( ( i + 1) * nPartDim, nEdgeCnt) - 1 ;
      }
     // processo le parti
      future<bool> vRes[MAX_PARTS] ;
      for ( int i = 0 ; i < nPartCnt ; ++ i)
         vRes[i] = async( launch::async, &TestSubEdges, ref( umEdgePnt), cref( vEdgeInd), vFstLst[i].first, vFstLst[i].second,
                          cref( vdGrid), nStepX, dStep, dLevel, cref( cavTstm), cref( frGrid)) ;
     // attendo i risultati
      int nFin = 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 ;
            }
         }
      }
   }

  // Predispongo il concatenamento
   BIPNTVECTOR vBiPnt ;
   vBiPnt.reserve( 2 * ( nStepX + nStepY)) ;
   ChainCurves chainC ;
   chainC.Init( false, EPS_SMALL, 2 * ( nStepX + nStepY)) ;
  // Ciclo sui quadrati da analizzare
   for ( int j = 0 ; j < nStepY ; ++ j) {
      for ( int i = 0 ; i < nStepX ; ++ i) {
        // indici dei vertici nella griglia
         int nInd0 = i + j * ( nStepX + 1) ;
         int nInd1 = ( i + 1) + j * ( nStepX + 1) ;
         int nInd2 = ( i + 1) + ( j + 1) * ( nStepX + 1) ;
         int nInd3 = i + ( j + 1) * ( nStepX + 1) ;
        // flag del quadrato
         int nFlag = ( vdGrid[nInd0] > dLevel ? 1 : 0) +
                     ( vdGrid[nInd1] > dLevel ? 2 : 0) +
                     ( vdGrid[nInd2] > dLevel ? 4 : 0) +
                     ( vdGrid[nInd3] > dLevel ? 8 : 0) ;
        // se quadrato con vertici tutti dello stesso tipo, passo al successivo
         if ( nFlag == 0  ||  nFlag == 15)
            continue ;
        // chiavi
         int vKey[4] = { 2 * nInd0, 2 * nInd1 + 1, 2 * nInd3, 2 * nInd0 + 1} ;
        // calcolo segmenti da inserire
         int nI1s, nI1e, nI2s, nI2e ;
         int nSegCnt = ProcessSquare( nFlag, nI1s, nI1e, nI2s, nI2e) ;
         if ( nSegCnt == -1)
            return false ;
         else if ( nSegCnt == 1) {
            Point3d ptL1s = umEdgePnt.find( vKey[nI1s])->second ;
            Point3d ptL1e = umEdgePnt.find( vKey[nI1e])->second ;
            vBiPnt.emplace_back( ptL1s, ptL1e) ;
            Vector3d vtDir1 = ptL1e - ptL1s ; vtDir1.Normalize() ;
            chainC.AddCurve( int( vBiPnt.size()), ptL1s, vtDir1, ptL1e, vtDir1) ;
         }
         else if ( nSegCnt == 2) {
            Point3d ptL1s = umEdgePnt.find( vKey[nI1s])->second ;
            Point3d ptL1e = umEdgePnt.find( vKey[nI1e])->second ;
            vBiPnt.emplace_back( ptL1s, ptL1e) ;
            Vector3d vtDir1 = ptL1e - ptL1s ; vtDir1.Normalize() ;
            chainC.AddCurve( int( vBiPnt.size()), ptL1s, vtDir1, ptL1e, vtDir1) ;
            Point3d ptL2s = umEdgePnt.find( vKey[nI2s])->second ;
            Point3d ptL2e = umEdgePnt.find( vKey[nI2e])->second ;
            vBiPnt.emplace_back( ptL2s, ptL2e) ;
            Vector3d vtDir2 = ptL2e - ptL2s ; vtDir2.Normalize() ;
            chainC.AddCurve( int( vBiPnt.size()), ptL2s, vtDir2, ptL2e, vtDir2) ;
         }
      }
   }

  // Recupero i contorni
   INTVECTOR vnId ;
   while ( chainC.GetChainFromNear( ORIG, false, vnId)) {
     // creo una composita
      CurveComposite crvCompo ;
      crvCompo.AddPoint( vBiPnt[vnId[0]-1].first) ;
      for ( int i = 0 ; i < int( vnId.size()) ; ++ i) {
         Point3d ptCurr = vBiPnt[vnId[i]-1].second ;
         crvCompo.AddLine( ptCurr) ;
      }
     // la chiudo
      crvCompo.Close() ;
     // elimino le parti allineate
      crvCompo.MergeCurves( 10 * EPS_SMALL, ANG_TOL_STD_DEG) ;
     // salto i contorni orari con area inferiore al doppio del quadrato
      double dCmpArea ;
      if ( ! crvCompo.GetAreaXY( dCmpArea)  ||  ( dCmpArea < 0  &&  abs( dCmpArea) < 2 * dStep * dStep)) 
         continue ;
     // per test mettere a 1
      #if 0
         vPL.emplace_back( PolyLine()) ;
         crvCompo.ApproxWithLines( 0, 0, ICurve::APL_SPECIAL, vPL.back()) ;
      #else
        // eseguo offset a sinistra pari allo step
         OffsetCurve offsCompo ;
         offsCompo.Make( &crvCompo, -( dRad - 2 * EPS_SMALL), ICurve::OFF_CHAMFER) ;
         PtrOwner<ICurve> pCrvOffset( offsCompo.GetLongerCurve()) ;
         if ( ! IsNull( pCrvOffset)) {
            vPL.emplace_back( PolyLine()) ;
            pCrvOffset->ApproxWithLines( 10 * EPS_SMALL, ANG_TOL_STD_DEG, ICurve::APL_STD, vPL.back()) ;
         }
      #endif
   }

   return true ;
}


//----------------------------------------------------------------------------
//  Silhouette rispetto ad una direzione e sopra una quota di una superficie TriMesh
//  ( dTol è il passo della griglia di campionamento e il raggio del cilindro di prova)
//----------------------------------------------------------------------------
bool
CAvSilhouetteSurfTm( const ISurfTriMesh& Stm, const Plane3d& plPlane, double dTol, POLYLINEVECTOR& vPL)
{
  // verifico superficie
   if ( &Stm == nullptr  ||  ! Stm.IsValid())
      return false ;
  // verifico piano
   if ( &plPlane == nullptr  ||  plPlane.GetVersN().IsSmall())
      return false ;
  // verifico tolleranza
   dTol = max( dTol, 100 * EPS_SMALL) ;
  // verifico parametri di ritorno
   if ( &vPL == nullptr)
      return false ;
   vPL.clear() ;

  // sistema di riferimento nel piano
   Frame3d frGrid ;
   if ( ! frGrid.Set( plPlane.GetPoint(), plPlane.GetVersN()))
      return false ;

  // bounding box della superficie nel riferimento del piano
   BBox3d b3Surf ;
   if ( ! Stm.GetBBox( GetInvert( frGrid), b3Surf, BBF_STANDARD))
      return false ;
  // calcolo dati della griglia
   const double EXTRA_XY = 1.5 * dTol ;
   const double EXTRA_Z = 10 * dTol ;
   b3Surf.Expand( EXTRA_XY, EXTRA_XY, EXTRA_Z) ;
   int nStepX = int( ceil( b3Surf.GetDimX() / dTol)) ;
   int nStepY = int( ceil( b3Surf.GetDimY() / dTol)) ;
   frGrid.ChangeOrig( GetToGlob( b3Surf.GetMin(), frGrid)) ;
   double dDimZ = b3Surf.GetDimZ() ;
   double dLevelOffs = - b3Surf.GetMin().z ;

  // calcolo dei punti della griglia (sul top del cilindro)
   PNTUVECTOR vPntM( ( nStepX + 1) * ( nStepY + 1)) ;
   for ( int j = 0 ; j <= nStepY ; ++ j) {
      for ( int i = 0 ; i <= nStepX ; ++ i) {
         int nInd = i + j * ( nStepX + 1) ;
         Point3d ptP = GetToGlob( Point3d( i * dTol, j * dTol, dDimZ), frGrid) ;
         vPntM[nInd] = { ptP, 0.} ;
      }
   }

  // esecuzione della verifica
   double dRad = SQRT1_2 * dTol ;
   CAvToolSurfTm cavTstm ;
   cavTstm.SetStdTool( dDimZ, dRad, 0) ;
   cavTstm.SetSurfTm( Stm) ;
   if ( ! cavTstm.TestSeries( vPntM, frGrid.VersZ(), frGrid.VersZ(), -1))
      return false ;

  // griglia degli spostamenti
   DBLVECTOR vdGrid( ( nStepX + 1) * ( nStepY + 1)) ;
   for ( int j = 0 ; j <= nStepY ; ++ j) {
      for ( int i = 0 ; i <= nStepX ; ++ i) {
         int nInd = i + j * ( nStepX + 1) ;
         vdGrid[nInd] = vPntM[nInd].second ;
      }
   }

  // calcolo della silhouette con il metodo MarchingSquares
   double dLevel = dLevelOffs ;
   if ( ! MarchingSquares( vdGrid, nStepX, nStepY, dTol, dRad, dLevel, cavTstm, frGrid, vPL))
      return false ;
  // riporto nella corretta posizione le curve trovate
   for ( auto& PL : vPL)
      PL.ToGlob( frGrid) ;

   return true ;
}


//----------------------------------------------------------------------------
//  Silhouette rispetto ad una direzione e sopra diverse quote di una superficie TriMesh
//----------------------------------------------------------------------------
ICAvParSilhouettesSurfTm*
CreateCAvParSilhouettesSurfTm( void)
{
   return static_cast<ICAvParSilhouettesSurfTm*> ( new(nothrow) CAvParSilhouettesSurfTm) ;
}

//----------------------------------------------------------------------------
CAvParSilhouettesSurfTm::CAvParSilhouettesSurfTm( void)
   : m_dTol( 100 * EPS_SMALL), m_nStepX( 0), m_nStepY( 0), m_dRad( m_dTol), m_dLevelOffs( 0), m_bGridOk( false)
{
}

//----------------------------------------------------------------------------
bool
CAvParSilhouettesSurfTm::SetData( const CISURFTMPVECTOR& vpStm, const Frame3d& frPlanes, double dTol)
{
   m_vpStm = vpStm ;
   m_frGrid = frPlanes ;
   m_dTol = max( dTol, 100 * EPS_SMALL) ;
   m_dRad = SQRT1_2 * m_dTol ;
   m_nStepX = 0 ;
   m_nStepY = 0 ;
   m_dDimZ = 0 ;
   m_dLevelOffs = 0 ;
   m_bGridOk = false ;
   return true ;
}

//----------------------------------------------------------------------------
bool
CAvParSilhouettesSurfTm::Prepare( void)
{
   // ingombro delle superfici nel riferimento dei piani
   BBox3d b3All ;
   Frame3d frInv = GetInvert( m_frGrid) ;
   for ( auto pStm : m_vpStm) {
      BBox3d b3Surf ;
      if ( ! pStm->GetBBox( frInv, b3Surf, BBF_STANDARD))
         return false ;
      b3All.Add( b3Surf) ;
   }

  // calcolo dati della griglia
   const double EXTRA_XY = 1.5 * m_dTol ;
   const double EXTRA_Z = 10 * m_dTol ;
   b3All.Expand( EXTRA_XY, EXTRA_XY, EXTRA_Z) ;
   m_nStepX = int( ceil( b3All.GetDimX() / m_dTol)) ;
   m_nStepY = int( ceil( b3All.GetDimY() / m_dTol)) ;
   m_frGrid.ChangeOrig( GetToGlob( b3All.GetMin(), m_frGrid)) ;
   m_dDimZ = b3All.GetDimZ() ;
   m_dLevelOffs = - b3All.GetMin().z ;

  // calcolo dei punti della griglia (sul top del cilindro)
   PNTUVECTOR vPntM( ( m_nStepX + 1) * ( m_nStepY + 1)) ;
   for ( int j = 0 ; j <= m_nStepY ; ++ j) {
      for ( int i = 0 ; i <= m_nStepX ; ++ i) {
         int nInd = i + j * ( m_nStepX + 1) ;
         Point3d ptP = GetToGlob( Point3d( i * m_dTol, j * m_dTol, m_dDimZ), m_frGrid) ;
         vPntM[nInd] = { ptP, 0.} ;
      }
   }

  // esecuzione della verifica
   if ( ! m_cavTstm.SetStdTool( m_dDimZ, m_dRad, 0))
      return false ;
   if ( m_vpStm.empty()  ||  ! m_cavTstm.SetSurfTm( *( m_vpStm[0])))
      return false ;
   for ( int k = 1 ; k < int( m_vpStm.size()) ; ++ k)
      m_cavTstm.AddSurfTm( *( m_vpStm[k])) ;
   if ( ! m_cavTstm.TestSeries( vPntM, m_frGrid.VersZ(), m_frGrid.VersZ(), -1))
      return false ;

  // griglia degli spostamenti
   m_vdGrid.clear() ;
   m_vdGrid.resize( ( m_nStepX + 1) * ( m_nStepY + 1)) ;
   for ( int j = 0 ; j <= m_nStepY ; ++ j) {
      for ( int i = 0 ; i <= m_nStepX ; ++ i) {
         int nInd = i + j * ( m_nStepX + 1) ;
         m_vdGrid[nInd] = vPntM[nInd].second ;
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
bool
CAvParSilhouettesSurfTm::GetSilhouette( double dLevel, POLYLINEVECTOR& vPL)
{
  // reset risultato
   vPL.clear() ;

  // se necessario eseguo i calcoli preparatori
   if ( ! m_bGridOk  &&  ! Prepare())
      return false ;
   m_bGridOk = true ;

  // calcolo della silhouette con il metodo MarchingSquares
   dLevel += m_dLevelOffs ;
   double dCalcLevel = max( dLevel, 0.) ;
   if ( ! MarchingSquares( m_vdGrid, m_nStepX, m_nStepY, m_dTol, m_dRad, dCalcLevel, m_cavTstm, m_frGrid, vPL))
      return false ;
  // riporto nella corretta posizione le curve trovate
   for ( auto& PL : vPL) {
      if ( dLevel < dCalcLevel - EPS_SMALL)
         PL.Translate( Vector3d{ 0, 0, dLevel - dCalcLevel}) ;
      PL.ToGlob( m_frGrid) ;
   }
   return true ;   
}