//----------------------------------------------------------------------------
//  EgalTech 2024
//----------------------------------------------------------------------------
// File : IntersLineSurfBez.cpp      Data : 06.02.24         Versione : 2.6b1
// Contenuto : Implementazione della intersezione linea/superficie bezier.
//
//
//
// Modifiche : 06.02.24 DB Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CurveLine.h"
#include "/EgtDev/Include/EGkDistPointLine.h"
#include "/EgtDev/Include/EGkIntersLineTria.h"
#include "/EgtDev/Include/EGkIntersLineSurfTm.h"
#include "/EgtDev/Include/EGkIntersLineSurfBez.h"
#include "/EgtDev/Include/EGkSurfBezier.h"

using namespace std ;

//----------------------------------------------------------------------------
static bool
RefineIntersNewton( const Point3d& ptL, const Vector3d& vtL, double dLen, bool bFinite,
                    const ISurfBezier* pSurfBz, Point3d& ptSP, Point3d& ptIBz)
{
  // la funzione raffina la posisione del punto ptSP, minimizzando la distanza dalla retta e restituisce il punto di intersezione ptIBz
   pSurfBz->GetPointD1D2( ptSP.x / SBZ_TREG_COEFF, ptSP.y / SBZ_TREG_COEFF, ISurfBezier::FROM_MINUS, ISurfBezier::FROM_MINUS, ptIBz) ;
  // usando un algoritmo di newton cerco di avvicinarmi il pił possibile alla retta
   DistPointLine dpl( ptIBz, ptL, vtL, dLen, bFinite) ;
   double dDistNew = 0, dDistPre = 0 ;
   dpl.GetDist(dDistNew) ;

   int nCount = 0 ;
   double dh = EPS_SMALL ;
   pSurfBz->GetPointD1D2( ptSP.x, ptSP.y, ISurfBezier::FROM_MINUS, ISurfBezier::FROM_MINUS, ptIBz) ;
  // metodo di newton in pił dimensioni
  // vario sia il parametro U che il parametro V e verifico se la distanza dalla retta diminuisce per scostamenti positivi o negativi.
   while ( dDistNew > EPS_SMALL  &&  nCount < 100) {
      dDistPre = dDistNew ;
      Point3d ptIBzNew1 ;
      pSurfBz->GetPointD1D2( ( ptSP.x + dh)  / SBZ_TREG_COEFF, ptSP.y  / SBZ_TREG_COEFF, ISurfBezier::FROM_MINUS, ISurfBezier::FROM_MINUS, ptIBzNew1) ;
      DistPointLine dplNewU( ptIBzNew1, ptL, vtL, dLen, bFinite) ;
      dplNewU.GetDist( dDistNew) ;
      double dfdU = ( dDistNew - dDistPre) / dh ;
      Point3d ptIBzNew2 ;
      pSurfBz->GetPointD1D2( ptSP.x  / SBZ_TREG_COEFF, ( ptSP.y + dh) / SBZ_TREG_COEFF, ISurfBezier::FROM_MINUS, ISurfBezier::FROM_MINUS, ptIBzNew2) ;
      DistPointLine dplNewV( ptIBzNew2, ptL, vtL, dLen, bFinite) ;
      dplNewV.GetDist( dDistNew) ;
      double dfdV = ( dDistNew - dDistPre) / dh ;
     // mi avvicino cercando di annullare la distanza in un colpo solo
      double dr = - dDistPre / ( dfdU + dfdV) ;
      pSurfBz->GetPointD1D2(( ptSP.x + dr * dfdU) / SBZ_TREG_COEFF, ( ptSP.y + dr * dfdV) / SBZ_TREG_COEFF, ISurfBezier::FROM_MINUS, ISurfBezier::FROM_MINUS, ptIBz) ;
      DistPointLine dplNew( ptIBz, ptL, vtL, dLen, bFinite) ;
      dplNew.GetDist( dDistNew) ;
      ++ nCount ;
   }

   return ( nCount != 99) ;
}

//----------------------------------------------------------------------------
static void
UpdateInfoIntersLineSurfBz( const Point3d& ptL, const Vector3d& vtDir, int nILT, int nT, const Point3d& ptSP, const Point3d& ptIBz, double dCos,
                            const Point3d& ptSP2, const Point3d& ptIBz2, double dCos2, ILSBIVECTOR& vInfo)
{
   if ( nILT == ILTT_IN  ||  nILT == ILTT_EDGE  ||  nILT == ILTT_VERT) {
      double dU = ( ptIBz - ptL) * vtDir ;
      vInfo.emplace_back( nILT, dU, nT, dCos, ptIBz, ptSP) ;
   }
   else if ( nILT == ILTT_SEGM  ||  nILT == ILTT_SEGM_ON_EDGE) {
      double dU = ( ptIBz - ptL) * vtDir ;
      double dU2 = ( ptIBz2 - ptL) * vtDir ;
      vInfo.emplace_back( nILT, dU, dU2, nT, dCos2, ptIBz, ptIBz2, ptSP, ptSP2) ;
   }
}

//----------------------------------------------------------------------------
static void
OrderInfoIntersLineSurfBz( ILSBIVECTOR& vInfo)
{
   // se non trovati, esco 
   if ( vInfo.size() == 0)
      return ;
   // ordino il vettore delle intersezioni secondo il senso crescente del parametro di linea
   sort( vInfo.begin(), vInfo.end(), 
      []( const IntLinSbzInfo& a, const IntLinSbzInfo& b)
      {  double dUa = ( ( a.nILTT == ILTT_SEGM  ||  a.nILTT == ILTT_SEGM_ON_EDGE) ? ( a.dU + a.dU2) / 2 : a.dU) ;
   double dUb = ( ( b.nILTT == ILTT_SEGM  ||  b.nILTT == ILTT_SEGM_ON_EDGE) ? ( b.dU + b.dU2) / 2 : b.dU) ;
   return ( dUa < dUb) ; }) ;
}

//----------------------------------------------------------------------------
//  Intersezione di una linea con una superficie di Bezier
//----------------------------------------------------------------------------
bool
IntersLineSurfBz( const Point3d& ptL, const Vector3d& vtL, double dLen, const ISurfBezier* pSurfBz,
                  ILSBIVECTOR& vInfo, bool bFinite)
{
   PtrOwner<ICurveLine> pCL( CreateCurveLine()) ;
   if ( bFinite)
      pCL->SetPVL(ptL, vtL, dLen) ;
   else
      pCL->SetPVL(ptL, vtL, 1e6) ;
   // verifico linea
   Vector3d vtDir = vtL ;
   if ( ! vtDir.Normalize( EPS_ZERO))
      return false ;
   // verifico superficie
   if ( pSurfBz == nullptr)
      return false ;
   // verifico parametro di ritorno
   if ( &vInfo == nullptr)
      return false ;
   vInfo.clear() ;

   // trovo le intersezioni con la trimesh ausiliaria
   const ISurfTriMesh* pSurfTm = pSurfBz->GetAuxSurf() ;
   ILSIVECTOR vInfoTm ;
   if ( ! IntersLineSurfTm( ptL, vtL, dLen, *pSurfTm, vInfoTm, bFinite))
      return false ;
   // ricavo le intersezioni con la superficie di Bezier
   for ( IntLinStmInfo InfoTm : vInfoTm ) {
      // devo raffinare i parametri lungo la curva, l'angolo e i punti di intersezione
      Point3d ptI, ptI2 ;
      // devo trovare le intersezioni
      Point3d ptSP, ptSP2 ; // coordinate parametriche delle soluzioni
      pSurfBz->UnprojectPointFromStm( InfoTm.nT, InfoTm.ptI, ptSP, InfoTm.nILTT) ;
      Point3d ptIBz, ptIBz2 ;
      if ( ! RefineIntersNewton( ptL, vtL, dLen, bFinite, pSurfBz, ptSP, ptIBz)) {
      /////// posso provare anche a rilanciare newton con un punto di partenza diverso oppure con una direzione di avvicinamento diversa///////////////////////////////////
         // per restare nel triangolo mi sposto verso un vertice
         int nVert[3] ;
         pSurfTm->GetTriangle( InfoTm.nT, nVert) ;
         double dU0, dV0 ;
         pSurfTm->GetVertexParam( nVert[0], dU0, dV0) ;
         ptSP = ptSP + Point3d(dU0, dV0, 0) ;
         if ( ! RefineIntersNewton( ptL,vtL, dLen, bFinite, pSurfBz, ptSP, ptIBz))
            return false ;
      }
      Vector3d vtN ;
      pSurfBz->GetPointNrmD1D2(ptSP.x / SBZ_TREG_COEFF, ptSP.y / SBZ_TREG_COEFF, ISurfBezier::FROM_MINUS, ISurfBezier::FROM_MINUS, ptIBz, vtN) ;
      double dCos = vtN * vtL ;
      double dCos2 = 0 ;
      // eventualmente ripeto tutto per ptI2 ( se ho un'intersezione con sovrapposizione)
      if ( InfoTm.nILTT == ILTT_SEGM || InfoTm.nILTT == ILTT_SEGM_ON_EDGE ) {
         pSurfBz->UnprojectPointFromStm( InfoTm.nT, InfoTm.ptI2, ptSP2, InfoTm.nILTT) ;
         if ( ! RefineIntersNewton(ptL, vtL, dLen, bFinite, pSurfBz, ptSP2, ptIBz2) ) {
            int nVert[3] ;
            pSurfTm->GetTriangle( InfoTm.nT, nVert) ;
            double dU0, dV0 ;
            pSurfTm->GetVertexParam( nVert[0], dU0, dV0) ;
            ptSP = ptSP + Point3d(dU0, dV0, 0) ;
            if ( ! RefineIntersNewton( ptL,vtL, dLen, bFinite, pSurfBz, ptSP, ptIBz))
               return false ;
         }
         pSurfBz->GetPointNrmD1D2( ptSP2.x / SBZ_TREG_COEFF, ptSP2.y / SBZ_TREG_COEFF, ISurfBezier::FROM_MINUS, ISurfBezier::FROM_MINUS, ptIBz2, vtN) ;
         dCos2 = vtN * vtL ;
      }
      UpdateInfoIntersLineSurfBz( ptL, vtL, InfoTm.nILTT, InfoTm.nT, ptSP, ptIBz, dCos, ptSP2, ptIBz2, dCos2, vInfo) ;
   }
   
   OrderInfoIntersLineSurfBz( vInfo) ;

   return true ;
}

//----------------------------------------------------------------------------
bool
FilterLineSurfBzInters( const ILSBIVECTOR& vInfo, INTDBLVECTOR& vInters)
{
   // tengo per buone la classificazione delle intersezioni fatte sulla trimesh
   // ciclo sulle intersezioni
   for ( const auto& Info : vInfo) {
      // se intersezione puntuale
      if ( Info.nILTT == ILTT_VERT  ||  Info.nILTT == ILTT_EDGE  ||  Info.nILTT == ILTT_IN) {
         int nFlag = LSBT_TOUCH ;
         if ( Info.dCosDN > EPS_ZERO)
            nFlag = LSBT_OUT ;
         else if ( Info.dCosDN < -EPS_ZERO)
            nFlag = LSBT_IN ;
         vInters.emplace_back( nFlag, Info.dU) ;
      }
      // se altrimenti intersezione con coincidenza
      else if ( Info.nILTT == ILTT_SEGM  ||  Info.nILTT == ILTT_SEGM_ON_EDGE) {
         vInters.emplace_back( LSBT_TG_INI, Info.dU) ;
         vInters.emplace_back( LSBT_TG_FIN, Info.dU2) ;
      }
   }
   // elimino intersezioni ripetute
   for ( size_t j = 1 ; j < vInters.size() ; ) {
      // intersezione precedente
      size_t i = j - 1 ;
      // se hanno lo stesso parametro 
      if ( abs( vInters[i].second - vInters[j].second) < EPS_SMALL) {
         // se sono entrambe entranti o uscenti, elimino la seconda
         if ( ( vInters[i].first == LSBT_IN  &&  vInters[j].first == LSBT_IN)  ||
            ( vInters[i].first == LSBT_OUT  &&  vInters[j].first == LSBT_OUT)) {
            vInters.erase( vInters.begin() + j) ;
            continue ;
         }
         // se una entrante e l'altra uscente, cambio in touch ed elimino la seconda
         else if ( ( vInters[i].first == LSBT_IN  &&  vInters[j].first == LSBT_OUT)  ||
            ( vInters[i].first == LSBT_OUT  &&  vInters[j].first == LSBT_IN)) {
            vInters[i].first = LSBT_TOUCH ;
            vInters.erase( vInters.begin() + j) ;
            continue ;
         }
         // se una puntuale e l'altra inizio di coincidenza, elimino la prima
         else if ( ( vInters[i].first == LSBT_IN  ||  vInters[i].first == LSBT_OUT  ||  vInters[i].first == LSBT_TOUCH)  &&  vInters[j].first == LSBT_TG_INI) {
            vInters.erase( vInters.begin() + i) ;
            continue ;
         }
         // se una fine di coincidenza e l'altra puntuale, elimino la seconda
         else if ( vInters[i].first == LSBT_TG_FIN  &&  ( vInters[j].first == LSBT_IN  ||  vInters[j].first == LSBT_OUT  ||  vInters[j].first == LSBT_TOUCH)) {
            vInters.erase( vInters.begin() + j) ;
            continue ;
         }
         // se una fine di coincidenza e l'altra inizio di coincidenza, elimino entrambe
         else if ( i > 0  &&  vInters[i].first == LSBT_TG_FIN  &&  vInters[j].first == LSBT_TG_INI) {
            vInters.erase( vInters.begin() + j) ;
            vInters.erase( vInters.begin() + i) ;
            -- j ;
            continue ;
         }
      }
      // passo alla successiva
      ++ j ;
   }
   return true ;
}