//----------------------------------------------------------------------------
//  EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : CurveByApprox.cpp        Data : 23.07.15       Versione : 1.6g7
// Contenuto : Implementazione della classe CurveByApprox, per creare
//             una curva mediante approssimazione di punti.
//
//
// Modifiche : 23.07.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CurveComposite.h"
#include "CalcDerivate.h"
#include "BiArcs.h"
#include "RemoveCurveDefects.h"
#include "/EgtDev/Include/EGkDistPointLine.h"
#include "/EgtDev/Include/EGkCurveByApprox.h"
#include "/EgtDev/Include/EGkPolyLine.h"
#include "/EgtDev/Include/EGkPolyArc.h"
#include "/EgtDev/Include/EgtPointerOwner.h"
#include <algorithm>

using namespace std ;

//----------------------------------------------------------------------------
bool
CurveByApprox::Reset( void)
{
  // pulisco i diversi vettori
   m_vPnt.clear() ;
   m_vPar.clear() ;
   m_vPrevDer.clear() ;
   m_vNextDer.clear() ;
   m_vSplits.clear() ;
   return true ;
}

//----------------------------------------------------------------------------
bool
CurveByApprox::AddPoint( const Point3d& ptP)
{
  // se il punto coincide con il precedente, lo salto
   if ( ! m_vPnt.empty()  &&  AreSamePointApprox( ptP, m_vPnt.back()))
      return false ;
  // aggiungo il punto
   try { m_vPnt.push_back( ptP) ; }
   catch ( ...) { return false ; }
   return true ;
}

//----------------------------------------------------------------------------
ICurve*
CurveByApprox::GetCurve( int nType, double dLinTol, double dAngTolDeg, double dLinFea)
{
  // se da approssimare con archi
   if ( nType == ARCS  ||  nType == ARCS_CORNER) {
     // calcolo approssimazione
      PolyArc PA ;
      if ( nType == ARCS) {
         if ( ! GetArcs( dLinTol, dAngTolDeg, PA))
            return nullptr ;
      }
      else {
         if ( ! GetArcsCorner( dLinTol, dAngTolDeg, dLinFea, PA))
            return nullptr ;
      }
     // creo la composita formata da questa approssimazione
      PtrOwner<CurveComposite> pCC( CreateBasicCurveComposite()) ;
      if ( ! pCC->FromPolyArc( PA))
         return nullptr ;
     // elimino eventuali Small Z
      pCC->RemoveSmallDefects( dLinTol, dAngTolDeg) ;
     // eventuale fusione di curve compatibili
      pCC->MergeCurves( dLinTol, dAngTolDeg) ;
     // restituisco la curva
      return Release( pCC) ;
   }
  // altrimenti con curve di Bezier cubiche
   else if ( nType == CUBIC_BEZIERS) {
      // !!! NON ANCORA IMPLEMENTATA !!!
      return nullptr ;
   }
  // tipi non previsti
   return nullptr ;
}

//----------------------------------------------------------------------------
bool
CurveByApprox::GetArcs( double dLinTol, double dAngTolDeg, PolyArc& PA)
{
  // pulisco il poliarco
   PA.Clear() ;

  // calcolo una parametrizzazione
   if ( ! CalcParameterization())
      return false ;

  // calcolo le tangenti
   if ( ! CalcAkimaTangents( false))
      return false ;

  // approssimo come unico tratto

  // creo la polilinea che unisce i punti
   PolyLine PL ;
   int nPnt = int( m_vPnt.size()) ;
   for ( int j = 0 ; j < nPnt ; ++ j)
      PL.AddUPoint( j, m_vPnt[j]) ;
  // verifico se retta verticale
   BBox3d b3PL ;
   if ( ! PL.GetLocalBBox( b3PL))
      return false ;
   if ( b3PL.GetDimX() < EPS_SMALL  &&  b3PL.GetDimY() < EPS_SMALL) {
      PA.AddUPoint( 0, m_vPnt[0], 0) ;
      PA.AddUPoint( nPnt - 1, m_vPnt[nPnt - 1], 0) ;
   }
  // altrimenti eseguo l'approssimazione con archi
   else {
      if ( ! BiArcOrSplit( 0, PL, dLinTol, dAngTolDeg, PA))
         return false ;
   }
   return true ;
}

//----------------------------------------------------------------------------
bool
CurveByApprox::GetArcsCorner( double dLinTol, double dAngTolDeg, double dLinFea, PolyArc& PA)
{
  // pulisco il poliarco
   PA.Clear() ;

  // calcolo una parametrizzazione
   if ( ! CalcParameterization())
      return false ;

  // calcolo le tangenti
   if ( ! CalcAkimaTangents( true))
      return false ;

  // divido in tratti (rettilinei o tra spigoli)
   if ( ! CalcSplitPoints( dLinTol, dAngTolDeg, dLinFea))
      return false ;

  // approssimo ogni singolo tratto
   int nPrev = 0 ;
   int nSplits = int( m_vSplits.size()) ;
   for ( int i = 0 ; i < nSplits ; ++ i) {
     // creo la polilinea che unisce i punti
      PolyLine PL ;
      for ( int j = nPrev ; j <= m_vSplits[i] ; ++ j)
         PL.AddUPoint( j, m_vPnt[j]) ;
     // verifico se retta verticale
      BBox3d b3PL ;
      if ( ! PL.GetLocalBBox( b3PL))
         return false ;
      if ( b3PL.GetDimX() < EPS_SMALL  &&  b3PL.GetDimY() < EPS_SMALL) {
         PA.AddUPoint( m_vSplits[i], m_vPnt[m_vSplits[i]], 0) ;
      }
     // altrimenti eseguo l'approssimazione con archi
      else {
         if ( ! BiArcOrSplit( 0, PL, dLinTol, dAngTolDeg, PA))
            return false ;
      }
     // salvo fine come prox inizio
      nPrev = m_vSplits[i] ;
   }
   return true ;
}

//----------------------------------------------------------------------------
bool
CurveByApprox::CalcParameterization( void)
{
  // pulisco il vettore dei parametri
   m_vPar.clear() ;

  // numero di punti
   int nSize = int( m_vPnt.size()) ;

  // sono necessari almeno due punti
   if ( nSize < 2)
      return false ;

  // calcolo le distanze tra i punti per derivarne i parametri
   m_vPar.reserve( nSize) ;
   double dPar = 0 ;
   m_vPar.push_back( dPar) ;
   for ( int i = 1 ; i < nSize ; ++ i) {
      double dDist = Dist( m_vPnt[i-1], m_vPnt[i]) ;
      dPar += dDist ;
      m_vPar.push_back( dPar) ;
   }

   return true ;
}

//----------------------------------------------------------------------------
bool
CurveByApprox::CalcAkimaTangents( bool bDetectCorner)
{
   return ComputeAkimaTangents( bDetectCorner, m_vPar, m_vPnt, m_vPrevDer, m_vNextDer) ;
}

//----------------------------------------------------------------------------
bool
CurveByApprox::CalcBesselTangents( void)
{
   return ComputeBesselTangents( m_vPar, m_vPnt, m_vPrevDer, m_vNextDer) ;
}

//----------------------------------------------------------------------------
bool
CurveByApprox::CalcSplitPoints( double dLinTol, double dAngTolDeg, double dLinFea)
{
  // precalcolo funzioni dei limiti
   double dSqLinTol = dLinTol * dLinTol ;
   double dAngTolCos = cos( max( dAngTolDeg, 0.) * DEGTORAD) ;
   double d2AngTolCos = cos( max( 2 * dAngTolDeg, 0.) * DEGTORAD) ;
   double dSqLinFea = dLinFea * dLinFea ;
  // cerco punti angolosi e punti di separazione di tratti lineari e non
  // verifico i punti tranne primo e ultimo
   int nSize = int( m_vPnt.size()) ;
   for ( int i = 1 ; i < nSize - 1 ; ++ i) {
     // se stimato angolo, allora punto di divisione
      if ( ( m_vPrevDer[i] * m_vNextDer[i]) < dAngTolCos) {
         m_vSplits.push_back(i) ;
         continue ;
      }
     // dati sui segmenti precedente e successivo
      Vector3d vtPrev = m_vPnt[i] - m_vPnt[i-1] ;
      double dLenPrev = m_vPar[i] - m_vPar[i-1] ;
      Vector3d vtNext = m_vPnt[i+1] - m_vPnt[i] ;
      double dLenNext = m_vPar[i+1] - m_vPar[i] ;
     // se i segmenti formano un angolo maggiore del doppio ammesso, allora punto di divisione
      if ( ( vtPrev * vtNext) / ( dLenPrev * dLenNext) < d2AngTolCos) {
         m_vSplits.push_back(i) ;
         continue ;
      }
     // verifico linearità del tratto precedente
      bool bPrevLin = vtPrev.SqLen() > dSqLinFea  &&
                      ( m_vNextDer[i-1] * m_vPrevDer[i]) > dAngTolCos  &&
                      ( vtPrev ^ m_vNextDer[i-1]).SqLen() < dSqLinTol  &&
                      ( vtPrev ^ m_vPrevDer[i]).SqLen() < dSqLinTol ;
     // verifico linearità del tratto successivo
      bool bNextLin = vtNext.SqLen() > dSqLinFea  &&
                      ( m_vNextDer[i] * m_vPrevDer[i+1]) > dAngTolCos  &&
                      ( vtNext ^ m_vNextDer[i]).SqLen() < dSqLinTol  &&
                      ( vtNext ^ m_vPrevDer[i+1]).SqLen() < dSqLinTol ;
     // se uno lineare e l'altro no, allora punto di divisione
      if ( ( bPrevLin  &&  ! bNextLin)  ||  ( ! bPrevLin  &&  bNextLin)) {
         m_vSplits.push_back(i) ;
         continue ;
      }
     // se entrambi non lineari, vado oltre
      if ( ! bPrevLin  &&  ! bNextLin)
         continue ;
     // sono entrambi tratti lineari, verifico siano allineati
      DistPointLine dPL( m_vPnt[i], m_vPnt[i-1], m_vPnt[i+1]) ;
      double dSqDist ;
      if ( ! dPL.GetSqDist( dSqDist)  ||  dSqDist > dSqLinTol) {
         m_vSplits.push_back(i) ;
         continue ;
      }
   }
  // in ogni caso ci deve essere l'ultimo punto
   m_vSplits.push_back(nSize - 1) ;

   return true ;
}

//----------------------------------------------------------------------------
bool
CurveByApprox::BiArcOrSplit( int nLev, PolyLine& PL, double dLinTol, double dAngTolDeg, PolyArc& PA) const
{
   const int MAX_LEV = 10 ;

  // se non chiusa
   if ( ! PL.IsClosed()) {

      PtrOwner<ICurve> pCrv ;
      double dMaxDist ;

     // se la polilinea ha più di 2 punti
      if ( PL.GetPointNbr() > 2)  {
        // calcolo punti e direzioni agli estremi della polilinea usando la curva di Bezier
         int nI ;
         double dU ;
         Point3d ptP0, ptP1 ;
         double dDir0Deg, dDir1Deg ;
         if ( ! PL.GetFirstU( dU))
            return false ;
         nI = int( dU) ;
         ptP0 = m_vPnt[nI] ;
         m_vNextDer[nI].ToSpherical( nullptr, nullptr, &dDir0Deg) ;
         if ( ! PL.GetLastU( dU))
            return false ;
         nI = int( dU) ;
         ptP1 = m_vPnt[nI] ;
         m_vPrevDer[nI].ToSpherical( nullptr, nullptr, &dDir1Deg) ;
        // costruisco un biarco sulla polilinea (secondo metodo di Z. Sir)
         pCrv.Set( GetBiArc( ptP0, dDir0Deg, ptP1, dDir1Deg, PL, dMaxDist, dLinTol)) ;
        // forzo la spezzatura della curva
         if ( IsNull( pCrv))
            dMaxDist = 2 * dLinTol ;
      }
     // se la polilinea è formata da 2 punti
      else if ( PL.GetPointNbr() == 2) {
        // se molto vicini, esco
         double dLen ;
         if ( PL.GetLength( dLen)  &&  dLen < EPS_SMALL)
            return true ;
        // costruisco la retta che li unisce
         PtrOwner<CurveComposite> pCC( CreateBasicCurveComposite()) ;
         if ( IsNull( pCC)  ||  ! pCC->FromPolyLine( PL))
            return false ;
         pCrv.Set( pCC) ;
         dMaxDist = 0 ;
      }
     // se la polilinea ha un solo punto, esco
      else
         return true ;

     // se raggiunto il massimo livello di recursione, forzo l'accettazione del biarco
      if ( nLev >= MAX_LEV) {
         if ( IsNull( pCrv))
            return false ;
         dMaxDist = 0 ;
        // segnalo situazione per debug
         if ( GetEGkDebugLev() >= 5)
            LOG_DBG_ERR( GetEGkLogger(), "ERROR : Exceeded recursions")
      }

     // se lunghezza abbastanza piccola, forzo l'accettazione della curva
      double dLen ;
      if ( PL.GetApproxLength( dLen)  &&  dLen < 10 * EPS_SMALL)
         dMaxDist = 0 ;

     // se in tolleranza
      if ( dMaxDist <= dLinTol) {
        // creo un nuovo poliarco
         PolyArc PA2 ;
         if ( ! pCrv->ApproxWithArcs( dLinTol, dAngTolDeg, PA2))
            return false ;
        // aggiusto la parametrizzazione
         double dUStart, dUEnd ;
         PL.GetFirstU( dUStart) ;
         PL.GetLastU( dUEnd) ;
         PA2.ParamLinearTransform( dUStart, dUEnd) ;
        // accodo all'esistente
         if ( ! PA.Join( PA2))
            return false ;
        // esco
         return true ;
      }
   }

  // se raggiunto il massimo livello di recursione, errore
   if ( nLev >= MAX_LEV) {
     // segnalo situazione per debug
      LOG_ERROR( GetEGkLogger(), "ERROR : Exceeded recursions")
      return false ;
   }

  // spezzo l'intervallo in due parti a metà
   double dParStart, dParEnd ;
   if ( ! PL.GetFirstU( dParStart)  ||  ! PL.GetLastU( dParEnd))
      return false ;
   double dParMid = 0.5 * ( dParStart + dParEnd) ;
   PolyLine PL2 ;
   if ( ! PL.Split( dParMid, PL2))
      return false ;
  // prima metà
   if ( ! BiArcOrSplit( nLev + 1, PL, dLinTol, dAngTolDeg, PA))
      return false ;
  // seconda metà
   return BiArcOrSplit( nLev + 1, PL2, dLinTol, dAngTolDeg, PA) ;
}