//----------------------------------------------------------------------------
//  EgalTech 2013-2014
//----------------------------------------------------------------------------
// File : DistPointCrvAux.cpp     Data : 13.01.14        Versione : 1.5a3
// Contenuto : Funzioni ausiliarie per calcolo distanza punto da curva.
//
//
//
// Modifiche : 13.01.14 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "DllMain.h"
#include "GeoConst.h"
#include "DistPointCrvAux.h"
#include "/EgtDev/Include/EGkDistPointLine.h"


//----------------------------------------------------------------------------
bool
CalcMinDistPointPolyLine( const Point3d& ptP, PolyLine& PL, double dLinTol, MDCVECTOR& vApproxMin)
{

   vApproxMin.reserve( 4) ;
   vApproxMin.clear() ;

   bool bFound = false ;
   bool bOnEnd = false ;
   double dUIni, dUFin ;
   Point3d ptIni, ptFin ;
   double dMinDist, dSqMinDist ;
   for ( bool bLine = PL.GetFirstULine( &dUIni, &ptIni, &dUFin, &ptFin) ;
         bLine ;
         bLine = PL.GetNextULine( &dUIni, &ptIni, &dUFin, &ptFin)) {
     // calcolo la distanza del punto dal segmento
      DistPointLine dstPtLn( ptP, ptIni, ptFin) ;
      double dSqDist ;
      if ( ! dstPtLn.GetSqDist( dSqDist))
         continue ;
     // altro punto con la stessa minima distanza già trovata
      if ( bFound  &&  abs( dSqDist - dSqMinDist) < 2 * dMinDist * dLinTol) {
        // salvo i dati nella struttura
         MinDistCalc approxMin ;
         approxMin.dDist = dMinDist ;
         dstPtLn.GetMinDistPoint( approxMin.ptQ) ;
         double dPar ;
         dstPtLn.GetParamAtMinDistPoint( dPar) ;
         approxMin.dPar = ( 1 - dPar) * dUIni + dPar * dUFin ;
         approxMin.dParMin = dUIni ;
         approxMin.dParMax = dUFin ;
        // setto che il punto è alla fine
         bOnEnd = ( AreSamePointApprox( approxMin.ptQ, ptFin)) ;
        // aggiungo alla lista  
         vApproxMin.push_back( approxMin) ;
      }
     // primo punto o punto con minima distanza più bassa
      else if ( ! bFound  ||  dSqDist < dSqMinDist) {
        // aggiorno i minimi
         bFound = true ;
         dSqMinDist = dSqDist ;
         dMinDist = sqrt( dSqMinDist) ;
        // salvo i dati nella struttura
         MinDistCalc approxMin ;
         approxMin.dDist = dMinDist ;
         dstPtLn.GetMinDistPoint( approxMin.ptQ) ;
         double dPar ;
         dstPtLn.GetParamAtMinDistPoint( dPar) ;
         approxMin.dPar = ( 1 - dPar) * dUIni + dPar * dUFin ;
         approxMin.dParMin = dUIni ;
         approxMin.dParMax = dUFin ;
        // setto che il punto è alla fine
         bOnEnd = ( AreSamePointApprox( approxMin.ptQ, ptFin)) ;
        // il nuovo vettore deve contenere solo quest'ultimo minimo
         vApproxMin.clear() ;
         vApproxMin.push_back( approxMin) ; 
      }
     // il minimo era alla fine, devo allargare l'intervallo di raffinamento del parametro
      else if ( bOnEnd) {
         bOnEnd = false ;
         vApproxMin.back().dParMax = dUFin ;
      }
   }

   return bFound ;
}

//----------------------------------------------------------------------------
bool
PolishMinDistPointCurve( const Point3d& ptP, const ICurve& cCurve,
                         const MinDistCalc& approxMin, double& dPrevPar, Point3d& ptQ)
{
   const int MAX_COUNT = 16 ;
   ICurve::Side nSide ;
   double dPar ;
   double dTemp ;
   double dSqCosA ;
   Vector3d vtDer1 ;
   Vector3d vtDer2 ;
   Vector3d vtDiff ;
 

  // raffino i punti trovati (con algoritmo tipo Newton da TheNurbsBook pag 230)
   int nCount = 0 ;
   bool bClampedFromSing = false ;
   dPar = approxMin.dPar ;
   do {
     // contatore iterazioni
      nCount ++ ;
     // calcolo P, D1 e D2
      nSide = ( abs( dPar - approxMin.dParMin) < EPS_PARAM) ? ICurve::FROM_PLUS : ICurve::FROM_MINUS ;
      cCurve.GetPointD1D2( dPar, nSide, ptQ, &vtDer1, &vtDer2) ;
     // vettore dal punto al piede sulla curva
      vtDiff = ptQ - ptP ;
     // angolo tra vettore e tangente
      dTemp = vtDer1 * vtDiff ;
      if ( abs( dTemp) > EPS_ZERO) 
         dSqCosA = dTemp * dTemp / ( vtDer1.SqLen() * vtDiff.SqLen()) ;
      else
         dSqCosA = 0 ;
     // stima prossimo valore del parametro (Newton : Unext = U - F(U) / F'(U))
      dPrevPar = dPar ;
      dTemp = vtDer2 * vtDiff + vtDer1.SqLen() ;
      if ( abs( dTemp) > EPS_ZERO)
         dPar = dPrevPar - ( vtDer1 * vtDiff) / dTemp ;
     // clipping parametro
      if ( dPar < approxMin.dParMin) {
         if ( approxMin.bParMinSing  &&  ! bClampedFromSing) {
            dPar = approxMin.dParMax ;
            bClampedFromSing = true ;
         }
         else
            dPar = approxMin.dParMin ;
      }
      else if ( dPar > approxMin.dParMax) {
         if ( approxMin.bParMaxSing  &&  ! bClampedFromSing) {
            dPar = approxMin.dParMin ;
            bClampedFromSing = true ;
         }
         else
            dPar = approxMin.dParMax ;
      }
   }  while ( nCount < MAX_COUNT  &&  abs( dPar - dPrevPar) > EPS_PARAM  &&
              abs( dSqCosA) > COS_ORTO_ANG_ZERO * COS_ORTO_ANG_ZERO) ;

   if ( nCount == MAX_COUNT  &&  GetEGkDebugLev() >= 5)
      LOG_DBG_ERR( GetEGkLogger(), "ERROR : Exceeded recursions") ;

   return true ;
}

//----------------------------------------------------------------------------
bool
FilterMinDistPointCurve( const Point3d& ptP, const ICurve& cCurve,
                         const MDCVECTOR& vApproxMin, double& dMinDist, MDPCIVECTOR& Info)
{
  // determino i minimi raffinati da tenere
   bool bFound = false ;
   bool bLastOnEnd = false ;       // flag per ultimo punto su fine del suo intervallo
   for ( auto Iter = vApproxMin.cbegin() ; Iter != vApproxMin.cend() ; ++Iter) {
     // altro punto con la stessa minima distanza
      if ( bFound  &&  abs( (*Iter).dDist - dMinDist) < EPS_SMALL) {
        // se abbastanza lontano e non su bordi intervallino lo aggiungo
         if ( SqDist( (*Iter).ptQ, Info.back().ptQ) > MIN_LEN_CONT_MDPC  &&
              ! bLastOnEnd  &&  abs( (*Iter).dPar - (*Iter).dParMin) > EPS_PARAM) {
            Info.emplace_back( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ) ;
            bLastOnEnd = abs( (*Iter).dPar - (*Iter).dParMax) < EPS_PARAM ;
         }
        // altrimenti lo sostituisco se distanza minore
         else if ( (*Iter).dDist < dMinDist) {
            Info.back() = MinDistPCInfo( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ) ;
            bLastOnEnd = abs( (*Iter).dPar - (*Iter).dParMax) < EPS_PARAM ;
         }
      }
     // primo punto o punto con minima distanza più bassa
      else if ( ! bFound  ||  (*Iter).dDist < dMinDist) {
        // aggiorno i minimi
         bFound = true ;
         dMinDist = (*Iter).dDist ;
        // il nuovo vettore deve contenere solo quest'ultimo minimo
         Info.clear() ;
         Info.emplace_back( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ) ; 
         bLastOnEnd = abs( (*Iter).dPar - (*Iter).dParMax) < EPS_PARAM ;
      }
   }
   if ( Info.empty())
      return false ;

  // se 2 o più minimi, verifico se tratto continuo
   if ( Info.size() >= 2) {
      bool bCont = true ;
      double dU ;
      Point3d ptQ ;
     // se tutti i punti intermedi hanno la stessa distanza, è una zona continua
      for ( int i = 1 ; i < (int) Info.size() ; ++ i) {
         dU = 0.5 * ( Info[i-1].dPar + Info[i].dPar) ;
         cCurve.GetPointD1D2( dU, ICurve::FROM_MINUS, ptQ) ;
         if ( abs( SqDist( ptP, ptQ) - dMinDist * dMinDist) > 2 * dMinDist * EPS_SMALL) {
            bCont = false ;
            break ;
         }
      }
     // se zona continua è un arco di circonferenza, tengo solo primo e ultimo punto e imposto opportuni flag
      if ( bCont) {
        // se è praticamente tutta la curva, la faccio diventare tutta
         if ( ( Info.back().dPar - Info.front().dPar) > 0.8) {
           // primo elemento == inizio della curva
            Info[0].dPar = 0 ;
            Info[0].nFlag = MDPCI_START_CONT ;
            cCurve.GetStartPoint( Info[0].ptQ) ;
           // ultimo elemento == fine curva
            Info[1].dPar = 1 ;
            Info[1].nFlag = MDPCI_END_CONT ;
            cCurve.GetEndPoint( Info[1].ptQ) ;
         }
         else {
           // sistemo flag primo elemento
            Info[0].nFlag = MDPCI_START_CONT ;
           // sposto ultimo elemento al secondo posto e ne sistemo il flag
            Info[1] = Info.back() ;
            Info[1].nFlag = MDPCI_END_CONT ;
         }
        // cancello tutti gli altri elementi
         Info.erase( Info.begin() + 2, Info.end()) ;
      }
   }

   return true ;
}