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

#include "stdafx.h"
#include "CAvToolTriangle.h"
#include "CAvToolSurfTm.h"
#include "DistPointLine.h"

using namespace std ;

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


//----------------------------------------------------------------------------
// CAvToolSurfTm
//----------------------------------------------------------------------------
CAvToolSurfTm::CAvToolSurfTm( void)
   : m_pSTm( nullptr)
{
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::SetSurfTm( const ISurfTriMesh& Stm)
{
   m_pSTm = GetBasicSurfTriMesh( &Stm) ;
   return ( m_pSTm != nullptr  &&  m_pSTm->IsValid()) ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::SetMoveDir( const Vector3d& vtMove)
{
   return m_frMove.Set( ORIG, vtMove) ;
}

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

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

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::TestPosition( const Point3d& ptT, const Vector3d& vtDir, double& dTotDist)
{
   Point3d ptCurr = ptT ;
   dTotDist = MyTestPosition( ptCurr, vtDir) ;
   return ( dTotDist > - EPS_SMALL) ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::TestPath( PNTULIST& lPntM, const Vector3d& vtDir, double dLinTol)
{
  // Se lista vuota, non devo fare alcunché
   if ( lPntM.empty())
      return true ;
  // Predispongo Hash Grid
   if ( ! PrepareHashGrid())
      return false ;
  // Ciclo sui punti
   Point3d ptPrev, ptCurr ;
   auto itPntMPrev = lPntM.end() ;
   auto itPntMCurr = lPntM.begin() ;
   while ( itPntMCurr != lPntM.end()) {
     // verifico il punto
      ptCurr = itPntMCurr->first ;
      itPntMCurr->second = MyTestPositionHG( itPntMCurr->first, vtDir) ;
      if ( itPntMCurr->second < - EPS_SMALL) {
        // pulisco HashGrid 2d
         m_HGrids.Clear() ;
         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 ;
   }
  // pulisco HashGrid 2d
   m_HGrids.Clear() ;
   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 ;
  // se oltre tolleranza dalla linea tra i punti includenti, devo aggiungerlo
   double dSqDist ;
   if ( DistPointLine( ptNewMid, itPntMPrev->first, itPntMCurr->first).GetSqDist( dSqDist)  &&  dSqDist > dLinTol * dLinTol) {
     // 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 ;
   Triangle3d Tria ;
   for ( int nTria = m_pSTm->GetFirstTriangle( Tria) ;
         nTria != SVT_NULL ;
         nTria = m_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)
{
   double dTotDist = 0 ;
  // calcolo box utensile nel riferimento di movimento
   BBox3d b3Tool ;
   b3Tool.Add( ptT) ;
   b3Tool.Add( ptT + vtDir * m_Tool.GetHeigth()) ;
   b3Tool.Expand( m_Tool.GetRadius(), m_Tool.GetRadius(), 0) ;
   b3Tool.ToLoc( m_frMove) ;
  // recupero indici triangoli che intersecano box in 2d 
   INTVECTOR vnIds ;
   if ( m_HGrids.Find( b3Tool, vnIds)) {
      for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) {
         int nT = vnIds[i] ;
         Triangle3d Tria ;
         if ( ! m_pSTm->GetTriangle( nT, Tria))
            return - 1 ;
         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 ;
}

//----------------------------------------------------------------------------
bool
CAvToolSurfTm::PrepareHashGrid( void)
{
  // pulisco HashGrid 2d
   m_HGrids.Clear() ;
  // verifico esistenza superficie
   if ( m_pSTm == nullptr  ||  ! m_pSTm->IsValid())
      return false ;
  // creo HashGrid 2d
   const int LIM_HG_TRIA = 128 ;
   m_HGrids.SetActivationGrid( m_pSTm->GetTriangleCount() > LIM_HG_TRIA) ;
  // riempio HashGrid 
   Triangle3d Tria ;
   int nT = m_pSTm->GetFirstTriangle( Tria) ;
   while ( nT != SVT_NULL) {
     // calcolo il BBox del triangolo nel riferimento scelto
      Tria.ToLoc( m_frMove) ;
      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))
         return false ;
     // passo al prossimo triangolo 
      nT = m_pSTm->GetNextTriangle( nT, Tria) ;
   }
  // aggiorno
   return m_HGrids.Update() ;
}