//----------------------------------------------------------------------------
//  EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : FilletChamfer.cpp         Data : 19.03.15       Versione : 1.6c4
// Contenuto : Implementazione funzioni per fillet e chamfer.
//
//
//
// Modifiche : 19.03.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CurveArc.h"
#include "CurveLine.h"
#include "/EgtDev/Include/EGkFilletChamfer.h"
#include "/EgtDev/Include/EGkDistPointCurve.h"
#include "/EgtDev/Include/EGkIntersCurves.h"
#include "/EgtDev/Include/EgkOffsetCurve.h"
#include "/EgtDev/Include/EgtPointerOwner.h"

using namespace std ;

//----------------------------------------------------------------------------
static bool
CalcForFillet( const ICurve& cCrv1, const Point3d& ptNear1,
               const ICurve& cCrv2, const Point3d& ptNear2,
               const Vector3d& vtNorm, double dRadius,
               Point3d& ptCen, Point3d& ptTg1, Point3d& ptTg2,
               int& nSide1, int& nSide2, double& dSinA, double& dTgPar1, double& dTgPar2)
{
  // calcolo un riferimento sul piano perpendicolare alla normale
   Frame3d frIntr ;
   if ( ! frIntr.Set( ORIG, vtNorm))
      return false ;

  // determino il lato di offset della curva 1
   DistPointCurve dPC1( ptNear2, cCrv1) ;
   if ( ! dPC1.GetSideAtMinDistPoint( 0, vtNorm, nSide1))
      return false ;
   double dOffs1 = ( nSide1 == MDS_RIGHT ? dRadius : - dRadius) ;
  // calcolo gli offset nel piano locale e dal lato opportuno di una copia della curva 1
   PtrOwner<ICurve> pCopy1( cCrv1.Clone()) ;
   if ( IsNull( pCopy1))
      return false ;
   pCopy1->ToLoc( frIntr) ;
   pCopy1->SetExtrusion( Z_AX) ;
   OffsetCurve OffsCrv1 ;
   OffsCrv1.Make( pCopy1, dOffs1, ICurve::OFF_FILLET) ;
   ICURVEPOVECTOR vOffs1 ;  
   ICurve* pCrv = OffsCrv1.GetLongerCurve() ;
   while ( pCrv != nullptr) {
      vOffs1.emplace_back( pCrv) ;
      pCrv = OffsCrv1.GetLongerCurve() ;
   }
   if ( vOffs1.empty())
      return false ;

  // determino il lato di offset della curva 2
   DistPointCurve dPC2( ptNear1, cCrv2) ;
   if ( ! dPC2.GetSideAtMinDistPoint( 0, vtNorm, nSide2))
      return false ;
   double dOffs2 = ( nSide2 == MDS_RIGHT ? dRadius : - dRadius) ;
  // calcolo gli offset nel piano locale e dal lato opportuno di una copia della curva 2
   PtrOwner<ICurve> pCopy2( cCrv2.Clone()) ;
   if ( IsNull( pCopy2))
      return false ;
   pCopy2->ToLoc( frIntr) ;
   pCopy2->SetExtrusion( Z_AX) ;
   OffsetCurve OffsCrv2 ;
   OffsCrv2.Make( pCopy2, dOffs2, ICurve::OFF_FILLET) ;
   ICURVEPOVECTOR vOffs2 ;
   pCrv = OffsCrv2.GetLongerCurve() ;
   while ( pCrv != nullptr) {
      vOffs2.emplace_back( pCrv) ;
      pCrv = OffsCrv2.GetLongerCurve() ;
   }
   if ( vOffs2.empty())
      return false ;

  // calcolo le intersezioni tra tutte le curve di offset e seleziono quella più vicina ai punti passati
   Point3d ptInt1 = P_INVALID, ptInt2 = P_INVALID ;
   Point3d ptNearI = Media( ptNear1, ptNear2) ;
   ptNearI.ToLoc( frIntr) ;
   double dMinDist = INFINITO ;
   for ( int i = 0 ; i < int( vOffs1.size()) ; i++) {
      for ( int j = 0 ; j < int( vOffs2.size()) ; j ++) {
         IntersCurveCurve intCC( *vOffs1[i], *vOffs2[j]) ;
         if ( intCC.GetIntersCount() == 0)
            continue ;
         Point3d ptInt1Curr, ptInt2Curr ;
         if ( ! intCC.GetIntersPointNearTo( 0, ptNearI, ptInt1Curr)  ||
              ! intCC.GetIntersPointNearTo( 1, ptNearI, ptInt2Curr))
            return false ;
         Point3d ptCenCurr = Media( ptInt1Curr, ptInt2Curr) ;
         double dDist = Dist( ptNearI, ptCenCurr) ;
         if ( dDist < dMinDist - EPS_SMALL) {
            dMinDist = dDist ;
            ptInt1 = ptInt1Curr ;
            ptInt2 = ptInt2Curr ;
         }
      }  
   }
  // se non sono state trovate intersezioni esco
   if ( ! ptInt1.IsValid()  ||  ! ptInt2.IsValid())
      return false ;   
   
   ptInt1.ToGlob( frIntr) ;
   ptInt2.ToGlob( frIntr) ;
   ptCen = Media( ptInt1, ptInt2) ;

  // proiezione del punto di intersezione sulla prima curva
   DistPointCurve dPCI1( ptInt1, cCrv1) ;
   int nFlag1 ;
   if ( ! dPCI1.GetParamAtMinDistPoint( 0, dTgPar1, nFlag1)  ||  nFlag1 != MDPCI_NORMAL)
      return false ;
   Vector3d vtTg1 ;
   if ( ! cCrv1.GetPointTang( dTgPar1, ICurve::FROM_MINUS, ptTg1, vtTg1))
      return false ;

  // proiezione del punto di intersezione sulla seconda curva
   DistPointCurve dPCI2( ptInt2, cCrv2) ;
   int nFlag2 ;
   if ( ! dPCI2.GetParamAtMinDistPoint( 0, dTgPar2, nFlag2)  ||  nFlag2 != MDPCI_NORMAL)
      return false ;
   Vector3d vtTg2 ;
   if ( ! cCrv2.GetPointTang( dTgPar2, ICurve::FROM_MINUS, ptTg2, vtTg2))
      return false ;

  // determino rotazione tra le curve
   dSinA = ( vtTg1 ^ vtTg2) * vtNorm ;

   return true ;
}

//----------------------------------------------------------------------------
ICurveArc*
CreateFillet( const ICurve& cCrv1, const Point3d& ptNear1,
              const ICurve& cCrv2, const Point3d& ptNear2,
              const Vector3d& vtNorm, double dRadius, double& dPar1, double& dPar2)
{
  // verifico validità parametri ricevuti
   if ( &cCrv1 == nullptr  ||  &ptNear1 == nullptr  ||
        &cCrv2 == nullptr  ||  &ptNear2 == nullptr  ||
        &vtNorm == nullptr  ||  &dPar1 == nullptr  ||  &dPar2 == nullptr)
      return nullptr ;

  // verifico il minimo raggio
   if ( dRadius < 10 * EPS_SMALL)
      return nullptr ;

  // eseguo calcoli
   Point3d ptCen, ptTg1, ptTg2 ;
   int nSide1, nSide2 ;
   double dSinA, dTgPar1, dTgPar2 ;
   if ( ! CalcForFillet( cCrv1, ptNear1, cCrv2, ptNear2, vtNorm, dRadius,
                         ptCen, ptTg1, ptTg2, nSide1, nSide2, dSinA, dTgPar1, dTgPar2))
      return nullptr ;

  // se tangenti parallele al contatto con fillet, ricalcolo con raggio più piccolo
   bool bParallel = ( abs( dSinA) < EPS_SMALL) ;
   if ( bParallel) {
      Point3d ptQQQ, ptQQ1, ptQQ2 ;
      double dQQQ1, dQQQ2 ;
      if ( ! CalcForFillet( cCrv1, ptNear1, cCrv2, ptNear2, vtNorm, dRadius - 10 * EPS_SMALL,
                            ptQQQ, ptQQ1, ptQQ2, nSide1, nSide2, dSinA, dQQQ1, dQQQ2)  ||  abs( dSinA) < EPS_SMALL)
         return nullptr ;
   }

  // verifico dimensione minima
   double dLen = Dist( ptTg1, ptTg2) ;
   if ( dLen < 2 * EPS_SMALL)
      return nullptr ;

  // orientamento tra le curve
   bool bCCW = ( dSinA > 0) ;

  // assegno i valori dei parametri di trim (+ da inizio, - da fine)
   if ( bCCW) {
      dPar1 = ( nSide2 == MDS_RIGHT ? dTgPar1 : - dTgPar1) ;
      dPar2 = ( nSide1 == MDS_RIGHT ? - dTgPar2 : + dTgPar2) ;
   }
   else {
      dPar1 = ( nSide2 == MDS_RIGHT ? - dTgPar1 : dTgPar1) ;
      dPar2 = ( nSide1 == MDS_RIGHT ? dTgPar2 : - dTgPar2) ;
   }

  // creo l'arco di fillet
   PtrOwner<CurveArc> crvFillet( CreateBasicCurveArc()) ;
   if ( IsNull( crvFillet)  ||
        ! crvFillet->SetC2PN( ptCen, ptTg1, ptTg2, vtNorm))
      return nullptr ;
  // se direzioni agli estremi praticamente parallele
   if ( bParallel) {
     // calcolo il verso dell'arco di fillet
      bool bFilletCcw = ( ( bCCW  &&  nSide1 == nSide2)  ||  ( ! bCCW  &&  nSide1 != nSide2)) ;
     // si deve verificare se va usato l'arco esplementare
      bool bArcCcw = ( crvFillet->GetAngCenter() > 0) ;
      if ( bFilletCcw != bArcCcw)
         crvFillet->ToExplementary() ;
   }
   return Release(crvFillet) ;
}

//----------------------------------------------------------------------------
ICurveLine*
CreateChamfer( const ICurve& cCrv1, const Point3d& ptNear1,
               const ICurve& cCrv2, const Point3d& ptNear2,
               const Vector3d& vtNorm, double dDist, double& dPar1, double& dPar2)
{
  // verifico validità parametri ricevuti
   if ( &cCrv1 == nullptr  ||  &ptNear1 == nullptr  ||
        &cCrv2 == nullptr  ||  &ptNear2 == nullptr  ||
        &vtNorm == nullptr  ||  &dPar1 == nullptr  ||  &dPar2 == nullptr)
      return nullptr ;

  // verifico lo smusso minimo
   if ( dDist < 10 * EPS_SMALL)
      return nullptr ;

  // calcolo un riferimento sul piano perpendicolare alla normale
   Frame3d frIntr ;
   if ( ! frIntr.Set( ORIG, vtNorm))
      return nullptr ;

  // determino il lato di offset della curva 1
   DistPointCurve dPC1( ptNear2, cCrv1) ;
   int nSide1 ;
   if ( ! dPC1.GetSideAtMinDistPoint( 0, vtNorm, nSide1))
      return nullptr ;
  // porto la curva1 nel piano locale
   PtrOwner<ICurve> pCopy1( cCrv1.Clone()) ;
   if ( IsNull( pCopy1))
      return nullptr ;
   pCopy1->ToLoc( frIntr) ;
   pCopy1->SetExtrusion( Z_AX) ;

  // determino il lato di offset della curva 2
   DistPointCurve dPC2( ptNear1, cCrv2) ;
   int nSide2 ;
   if ( ! dPC2.GetSideAtMinDistPoint( 0, vtNorm, nSide2))
      return nullptr ;
  // porto la curva2 nel piano locale
   PtrOwner<ICurve> pCopy2( cCrv2.Clone()) ;
   if ( IsNull( pCopy2))
      return nullptr ;
   pCopy2->ToLoc( frIntr) ;
   pCopy2->SetExtrusion( Z_AX) ;

  // calcolo l'intersezione tra le due curve
   Point3d ptInt1, ptInt2 ;
   Point3d ptNear1I = ptNear1 ;
   ptNear1I.ToLoc( frIntr) ;
   IntersCurveCurve intCC( *pCopy1, *pCopy2) ;
   if ( ! intCC.GetIntersPointNearTo( 0, ptNear1I, ptInt1)  ||
        ! intCC.GetIntersPointNearTo( 1, ptNear1I, ptInt2))
      return nullptr ;
   ptInt1.ToGlob( frIntr) ;
   ptInt2.ToGlob( frIntr) ;

  // determino le posizioni parametriche e le distanze dell'intersezione sulle due curve
   double dU1, dDist1 ;
   if ( ! cCrv1.GetParamAtPoint( ptInt1, dU1)  ||  ! cCrv1.GetLengthAtParam( dU1, dDist1))
      return nullptr ;
   double dU2, dDist2 ;
   if ( ! cCrv2.GetParamAtPoint( ptInt2, dU2)  ||  ! cCrv2.GetLengthAtParam( dU2, dDist2))
      return nullptr ;

  // tangenti alle curve nel punto di intersezione
   Point3d ptTg1 ;
   Vector3d vtTg1 ;
   if ( ! cCrv1.GetPointTang( dU1, ICurve::FROM_MINUS, ptTg1, vtTg1))
      return nullptr ;
   Point3d ptTg2 ;
   Vector3d vtTg2 ;
   if ( ! cCrv2.GetPointTang( dU2, ICurve::FROM_MINUS, ptTg2, vtTg2))
      return nullptr ;

  // determino rotazione tra le curve
   bool bCCW = (( vtTg1 ^ vtTg2) * vtNorm) > 0 ;

  // assegno i valori delle distanze degli estremi dello smusso
   if ( bCCW) {
      dDist1 += ( nSide2 == MDS_RIGHT ? dDist : - dDist) ;
      dDist2 += ( nSide1 == MDS_RIGHT ? - dDist : + dDist) ;
   }
   else {
      dDist1 += ( nSide2 == MDS_RIGHT ? - dDist : dDist) ;
      dDist2 += ( nSide1 == MDS_RIGHT ? dDist : - dDist) ;
   }
  // li converto in posizioni parametriche
   if ( ! cCrv1.GetParamAtLength( dDist1, dU1)  ||
        ! cCrv2.GetParamAtLength( dDist2, dU2))
      return nullptr ;

  // assegno i valori dei parametri di trim (+ da inizio, - da fine)
   if ( bCCW) {
      dPar1 = ( nSide2 == MDS_RIGHT ? dU1 : - dU1) ;
      dPar2 = ( nSide1 == MDS_RIGHT ? - dU2 : + dU2) ;
   }
   else {
      dPar1 = ( nSide2 == MDS_RIGHT ? - dU1 : dU1) ;
      dPar2 = ( nSide1 == MDS_RIGHT ? dU2 : - dU2) ;
   }

  // calcolo la linea di smusso
   Point3d ptP1, ptP2 ;
   if ( ! cCrv1.GetPointD1D2( dU1, ICurve::FROM_MINUS, ptP1)  ||
        ! cCrv2.GetPointD1D2( dU2, ICurve::FROM_MINUS, ptP2))
      return nullptr ;
   PtrOwner<CurveLine> crvChamfer( CreateBasicCurveLine()) ;
   if ( IsNull( crvChamfer)  ||
        ! crvChamfer->Set( ptP1, ptP2))
      return nullptr ;
   return Release(crvChamfer) ;
}