//----------------------------------------------------------------------------
//  EgalTech 2013-2014
//----------------------------------------------------------------------------
// File : BiArcs.cpp         Data : 30.07.14       Versione : 1.5g4
// Contenuto : Implementazione funzioni per calcolo biarchi.
//
//
//
// Modifiche : 30.07.14 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "BiArcs.h"
#include "CurveLine.h"
#include "CurveArc.h"
#include "CurveComposite.h"
#include "/EgtDev/Include/EGkAngle.h"
#include "/EgtDev/Include/EGkArcSpecial.h"
#include "/EgtDev/Include/EGkDistPointCurve.h"
#include "/EgtDev/Include/EgtNumUtils.h"
#include "/EgtDev/Include/EgtPointerOwner.h"

using namespace std ;

//----------------------------------------------------------------------------
static ICurve* CalcJCurve( const Point3d& ptP0, double dDir0Deg, const Point3d& ptP1, double dDir1Deg) ;


//----------------------------------------------------------------------------
ICurve*
GetBiArc( const Point3d& ptP0, double dDir0Deg, const Point3d& ptP1, double dDir1Deg, double dU)
{
  // calcolo la curva dove giacciono i punti di giunzione tra i due archi del biarco
   PtrOwner<ICurve> pJCrv( CalcJCurve( ptP0, dDir0Deg, ptP1, dDir1Deg)) ;
   if ( IsNull( pJCrv))
      return nullptr ;

  // limito il parametro nell'intervallo 0 - 1
   if ( dU < 0)
      dU = 0 ;
   else if ( dU > 1)
      dU = 1 ;

  // recupero il punto di giunzione
   Point3d ptJ ;
   if ( ! pJCrv->GetPointD1D2( dU, ICurve::FROM_MINUS, ptJ))
      return nullptr ;

  // preparo la curva composita per i biarchi
   PtrOwner<CurveComposite> pBiArc( CreateBasicCurveComposite()) ;
   if ( IsNull( pBiArc))
      return nullptr ;

  // calcolo la curva dal punto iniziale alla giunzione
   if ( ! AreSamePointApprox( ptP0, ptJ)) {
      ICurve* pCrv = GetArc2PD( ptP0, ptJ, dDir0Deg) ;
      if ( pCrv == nullptr)
         return nullptr ;
      pBiArc->AddCurve( pCrv) ;
   }

  // calcolo la curva dalla giunzione al punto finale
   if ( ! AreSamePointApprox( ptJ, ptP1)) {
     // curva dal punto finale, direzione opposta alla giunzione
      ICurve* pCrv = GetArc2PD( ptP1, ptJ, dDir1Deg + ANG_STRAIGHT) ;
      if ( pCrv == nullptr)
         return nullptr ;
     // inverto la curva
      pCrv->Invert() ;
      pBiArc->AddCurve( pCrv) ;
   }

  // se il biarco non esiste
   if ( pBiArc->GetCurveCount() == 0)
      return nullptr ;

   return ::Release( pBiArc) ;
}

//----------------------------------------------------------------------------
ICurve*
GetBiArc( const Point3d& ptP0, double dDir0Deg, const Point3d& ptP1, double dDir1Deg,
          const PolyLine& PL, double& dDist, double dTol)
{
  // calcolo la curva dove giacciono i punti di giunzione tra i due archi del biarco
   PtrOwner<ICurve> pJCrv( CalcJCurve( ptP0, dDir0Deg, ptP1, dDir1Deg)) ;
   if ( IsNull( pJCrv))
      return nullptr ;
  // calcolo le intersezioni tra la curva delle giunzioni (segmento o arco) e la polilinea
   PtrOwner<ICurve> pBiArc ;
   if ( pJCrv->GetType() == CRV_LINE) {
      pBiArc.Set( GetBiArc( ptP0, dDir0Deg, ptP1, dDir1Deg, 0.5)) ;
   }
   else {
      const CurveArc* pArc = GetBasicCurveArc( pJCrv) ;
      if ( pArc == nullptr)
         return nullptr ;
      double dU = -1 ;
      double dRad = pArc->GetRadius() ;
      double dSqRad = dRad * dRad ;
      Point3d ptCen = pArc->GetCenter() ;
      Point3d ptIni, ptFin ;
      for ( bool bLine = PL.GetFirstLine( ptIni, ptFin) ;
            bLine ;
            bLine = PL.GetNextLine( ptIni, ptFin)) {
         double dSqDistIni = SqDistXY( ptIni, ptCen) ;
         double dSqDistFin = SqDistXY( ptFin, ptCen) ;
         if ( ( dSqDistIni > dSqRad  &&  dSqDistFin < dSqRad)  ||
              ( dSqDistIni < dSqRad  &&  dSqDistFin > dSqRad)) {
            double dDiffIni = DistXY( ptIni, ptCen) - dRad ;
            double dDiffFin = DistXY( ptFin, ptCen) - dRad ;
            Point3d ptMid = Media( ptIni, ptFin, abs( dDiffIni) / ( abs( dDiffIni) + abs( dDiffFin))) ;
            int nPos ;
            double dTmp ;
            if ( pArc->CalcPointParamPosiz( ptMid, dTmp, nPos)) {
               if ( nPos != ICurve::PP_START  &&  nPos != ICurve::PP_END  &&
                    abs( dTmp - 0.5) < abs( dU - 0.5))
                  dU = dTmp ;
            }
         }
      }
     // non c'è intersezione, assegno valore medio
      if ( dU < -0.5)
         dU = 0.5 ;
     // elimino casi vicino agli estremi, danno solo problemi
      dU = Clamp( dU, 0.1, 0.9) ;
      pBiArc.Set( GetBiArc( ptP0, dDir0Deg, ptP1, dDir1Deg, dU)) ;
   }

  // se biarco non trovato, errore
   if ( IsNull( pBiArc))
      return nullptr ;

  // determino la massima distanza tra la curva e il biarco
   double dSqDist = 0 ;
   const double STEP = 10 ;
   Point3d ptCurr ;
   bool bPnt = PL.GetFirstPoint( ptCurr) ;
   Point3d ptPrev = ptCurr ;
   while ( bPnt) {
      double dLen = Dist( ptCurr, ptPrev) ;
      int nStep = int( dLen / STEP) + 1 ;
      int nMinStep = ( dLen > 50 * dTol ? 3 : ( dLen > 10 * dTol ? 2 : 1)) ;
      int nMaxStep = 10 ;
      nStep = Clamp( nStep, nMinStep, nMaxStep) ;
      for ( int i = 1 ; i <= nStep ; ++ i) {
         double dCoeff = double( i) / nStep ;
         Point3d ptP = Media( ptPrev, ptCurr, dCoeff) ;
         DistPointCurve dstPC( ptP, *pBiArc) ;
         double dSqDistPC ;
         if ( dstPC.GetSqDist( dSqDistPC)  &&  dSqDistPC > dSqDist)
            dSqDist = dSqDistPC ;
      }
      ptPrev = ptCurr ;
      bPnt = PL.GetNextPoint( ptCurr) ;
   }
   dDist = sqrt( dSqDist) ;

   return Release( pBiArc) ;
}

//----------------------------------------------------------------------------
static ICurve*
CalcJCurve( const Point3d& ptP0, double dDir0Deg, const Point3d& ptP1, double dDir1Deg)
{
  // se i due punti coincidono, non si può fare alcunché
   if ( AreSamePointApprox( ptP0, ptP1))
      return nullptr ;

  // angolo di rotazione dalla prima direzione alla seconda -> angolo al centro
   double dAngDeg = DiffAngle( dDir1Deg, dDir0Deg) ;

  // se rotazione nulla, allora segmento di retta tra i due punti
   if ( abs( dAngDeg) < EPS_ANG_SMALL) {
      PtrOwner<CurveLine> pLine( CreateBasicCurveLine()) ;
      if ( IsNull( pLine)  ||  ! pLine->Set( ptP0, ptP1))
         return nullptr ;
     // inverto per avere parametrizzazione crescente allontanandosi da Dir0 e avvicinandosi a Dir1
      pLine->Invert() ;
      return ::Release( pLine) ;
   }

  // caso generico
   Point3d ptMed = 0.5 * ( ptP0 + ptP1) ;
   ptMed.z = ptP0.z ;
   Vector3d vtDiff = ptMed - ptP0 ;
   double dHalfDist = vtDiff.LenXY() ;
   vtDiff /= dHalfDist ;
   double dDirDiffDeg ;
   vtDiff.ToSpherical( nullptr, nullptr, &dDirDiffDeg) ;
   double dH = dHalfDist / tan( 0.5 * dAngDeg * DEGTORAD) ;
   vtDiff.Rotate( Z_AX, 0, 1) ;
   Point3d ptCen = ptMed + dH * vtDiff ;
   Vector3d vtStart = ptP0 - ptCen ;
   double dRad, dAngStart ;
   vtStart.ToSpherical( &dRad, nullptr, &dAngStart) ;
   PtrOwner<CurveArc> pArc( CreateBasicCurveArc()) ;
   if ( IsNull( pArc)  ||  ! pArc->SetXY( ptCen, dRad, dAngStart, dAngDeg, ( ptP1.z - ptP0.z)))
      return nullptr ;
   double dDirStartDeg = dAngStart + ( dAngDeg > 0 ? ANG_RIGHT : - ANG_RIGHT) ;
  // determinazione regione tra le curve estreme in cui contenere l'arco (tramite angoli su start)
   // direzione iniziale arco rispetto a direzione P0->P1
   double dDirStartRelDeg = DiffAngle( dDirStartDeg, dDirDiffDeg) ;
   // direzione iniziale primo arco limite rispetto a direzione P0->P1
   double dDir0RelDeg = DiffAngle( dDir0Deg, dDirDiffDeg) ;
   // direzione iniziale secondo arco limite rispetto a direzione P0->P1 (dalla finale simmetrico e invert)
   double dDir1RelDeg = - DiffAngle( dDir1Deg, dDirDiffDeg) ;
   // nel caso di direzioni a 180deg si sceglie la più compatta
   if ( abs( abs( dDir1RelDeg) - ANG_STRAIGHT) < EPS_SMALL)
      dDir1RelDeg = ( dDir0RelDeg > 0 ? ANG_STRAIGHT : - ANG_STRAIGHT) ;
   else if ( abs( abs( dDir0RelDeg) - ANG_STRAIGHT) < EPS_SMALL)
      dDir0RelDeg = ( dDir1RelDeg > 0 ? ANG_STRAIGHT : - ANG_STRAIGHT) ;
   // intervallo angolare ammissibile a partire da direzione iniziale primo arco ammissibile ( == Dir0)
   double dDeltaAngDeg = - dDir0RelDeg + dDir1RelDeg ;
  // se non è nella regione, prendo l'altra parte di arco
   if ( ! AngleInSpan( dDirStartRelDeg, dDir0RelDeg, dDeltaAngDeg)) 
      pArc->ToExplementary() ;
  // inverto per avere parametrizzazione crescente allontanandosi da Dir0 e avvicinandosi a Dir1
   pArc->Invert() ;
   return ::Release( pArc) ;
}