/*--------------------------------------------------------------------------*\
 |                                                                          |
 |  Copyright (C) 2017                                                      |
 |                                                                          |
 |         , __                 , __                                        |
 |        /|/  \               /|/  \                                       |
 |         | __/ _   ,_         | __/ _   ,_                                | 
 |         |   \|/  /  |  |   | |   \|/  /  |  |   |                        |
 |         |(__/|__/   |_/ \_/|/|(__/|__/   |_/ \_/|/                       |
 |                           /|                   /|                        |
 |                           \|                   \|                        |
 |                                                                          |
 |      Enrico Bertolazzi                                                   |
 |      Dipartimento di Ingegneria Industriale                              |
 |      Universita` degli Studi di Trento                                   |
 |      email: enrico.bertolazzi@unitn.it                                   |
 |                                                                          |
\*--------------------------------------------------------------------------*/

#include "Clothoids.hh"

// Workaround for Visual Studio
#ifdef min
  #undef min
#endif

#ifdef max
  #undef max
#endif

namespace G2lib {

  using std::abs;
  using std::sqrt;
  using std::sin;
  using std::cos;
  using std::max;
  using std::min;

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  real_type
  ClothoidCurve::closestPointBySample(
    real_type   ds,
    real_type   qx,
    real_type   qy,
    real_type & X,
    real_type & Y,
    real_type & S
  ) const {
    S = 0;
    X = m_CD.x0;
    Y = m_CD.y0;
    real_type DST = hypot( X-qx, Y-qy );
    real_type SSS = ds;
    while ( SSS <= m_L ) {
      real_type theta, kappa, XS, YS;
      m_CD.evaluate( SSS, theta, kappa, XS, YS );
      real_type dst = hypot( XS-qx, YS-qy );
      if ( dst < DST ) {
        DST = dst;
        S   = SSS;
        X   = XS;
        Y   = YS;
      }
      SSS += ds;
    }
    return DST;
  }

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  #ifndef DOXYGEN_SHOULD_SKIP_THIS
  static
  bool
  closestPointQC2(
    real_type            epsi,
    ClothoidData const & CD,
    real_type            L,
    real_type            qx,
    real_type            qy,
    real_type          & S
  ) {

    // S = GUESS
    int nb = 0;
    real_type theta, kappa, dS, dx, dy;
    real_type s = S;
    for ( int iter = 0; iter < 20 && nb < 2; ++iter ) {
      CD.evaluate( s, theta, kappa, dx, dy ); dx -= qx; dy -= qy;

      real_type Cs  = cos(theta);
      real_type Ss  = sin(theta);
      real_type a0  = Cs * dy - Ss * dx;
      real_type b0  = Ss * dy + Cs * dx;
      real_type tmp = a0*kappa;

      // approx clothoid with a circle
      if ( 1+2*tmp > 0 ) {

        tmp = b0/(1+tmp);
        dS = -tmp*Atanc(tmp*kappa);

      } else {

        real_type om = atan2( b0, a0+1/kappa );
        if ( kappa < 0 ) {
          if ( om < 0 ) om += Utils::m_pi;
          else          om -= Utils::m_pi;
        }

        dS = -om/kappa;
      }

      s += dS;
      if ( abs( dS ) < epsi ) {
        if ( s < -epsi || s > L+epsi ) return false;
        S = s;
        return true;
      }

      // check divergence
      if      ( s < 0 ) { s = 0; ++nb; }
      else if ( s > L ) { s = L; ++nb; }
      else              { nb = 0; }

    }
    return false;
  }
  #endif

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  #ifndef DOXYGEN_SHOULD_SKIP_THIS
  static
  real_type
  closestPointQC1(
    real_type            epsi,
    ClothoidData const & CD,
    real_type            L,
    real_type            qx,
    real_type            qy,
    real_type          & X,
    real_type          & Y,
    real_type          & S
  ) {

    real_type phi0 = CD.theta0 - atan2( CD.y0 - qy, CD.x0 - qx );
    bool ok0 = cos(phi0) < 0; // distanza decrescente

    real_type theta1, kappa1, x1, y1;
    CD.evaluate( L, theta1, kappa1, x1, y1 );
    real_type phi1 = theta1 - atan2( y1 - qy, x1 - qx );
    bool ok1 = cos(phi1) > 0; // distanza crescente

    real_type s0 = 0, x0 = CD.x0, y0 = CD.y0;
    if ( ok0 ) ok0 = closestPointQC2( epsi, CD, L, qx, qy, s0 );
    if ( ok0 ) CD.eval( s0, x0, y0 );
    real_type d0 = hypot( x0-qx, y0-qy );

    real_type s1 = L;
    if ( ok1 ) ok1 = closestPointQC2( epsi, CD, L, qx, qy, s1 );
    if ( ok1 ) CD.eval( s1, x1, y1 );
    real_type d1 = hypot( x1-qx, y1-qy );

    if ( !ok0 && !ok1 ) { // s1 - s0 > 2 * epsi ) { // buoni entrambi estremi
      S = (s0+s1)/2;
      bool okm = closestPointQC2( epsi, CD, L, qx, qy, S );
      if ( okm ) {
        CD.eval( S, X, Y );
        real_type dm = hypot( X-qx, Y-qy );
        if ( dm < d0 && dm < d1 ) return dm;
      }
    }

    if ( d0 < d1 ) { S = s0; X = x0; Y = y0; return d0; }
    else           { S = s1; X = x1; Y = y1; return d1; }

  }
  #endif

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  #ifndef DOXYGEN_SHOULD_SKIP_THIS
  static
  real_type
  closestPointQC(
    real_type            epsi,
    ClothoidData const & CD,
    real_type            L,
    real_type            qx,
    real_type            qy,
    real_type          & X,
    real_type          & Y,
    real_type          & S
  ) {

    real_type DTheta = abs( CD.theta(L) - CD.theta0 );
    if ( DTheta <= Utils::m_2pi )
      return closestPointQC1( epsi, CD, L, qx, qy, X, Y, S );

    real_type cx = CD.c0x();
    real_type cy = CD.c0y();

    //if ( hypot( CD.x0 - cx, CD.y0 - cy ) <= hypot( qx - cx, qy - cy ) ) {
    if ( 1 <= abs(CD.kappa0) * hypot( qx - cx, qy - cy ) ) {
      real_type ell = CD.aplus(Utils::m_2pi);
      return closestPointQC1( epsi, CD, ell, qx, qy, X, Y, S );
    }

    ClothoidData CD1;
    CD.reverse( L, CD1 );
    cx = CD1.c0x();
    cy = CD1.c0y();

    //if ( hypot( CD1.x0 - cx, CD1.y0 - cy ) >= hypot( qx - cx, qy - cy ) ) {
    if ( 1 >= abs(CD1.kappa0) * hypot( qx - cx, qy - cy ) ) {
      real_type ell = CD1.aplus(Utils::m_2pi);
      real_type d   = closestPointQC1( epsi, CD1, ell, qx, qy, X, Y, S );
      S = L - S;
      return d;
    }

    real_type ell = CD.aplus(DTheta/2);
    real_type d0  = closestPointQC( epsi, CD, ell, qx, qy, X, Y, S );

    CD.eval( ell, CD1 );

    real_type X1, Y1, S1;
    real_type d1 = closestPointQC( epsi, CD1, L-ell, qx, qy, X1, Y1, S1 );

    if ( d1 < d0 ) { S = ell+S1; X = X1; Y = Y1; return d1; }

    return d0;

  }
  #endif

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  #ifndef DOXYGEN_SHOULD_SKIP_THIS
  static
  bool
  closestPointStandard3(
    real_type   epsi,
    real_type   a,
    real_type   b,
    real_type   qx,
    real_type   qy,
    real_type & S
  ) {
    // S = GUESS
    int nb = 0;
    real_type s = S, dS, dx, dy;
    for ( int iter = 0; iter < 20 && nb < 2; ++iter ) {
      // approx clothoid with a circle
      real_type kappa = Utils::m_pi * s;
      real_type theta = 0.5*(kappa*s);
      FresnelCS( s, dx, dy ); dx -= qx; dy -= qy;

      real_type Cs  = cos(theta);
      real_type Ss  = sin(theta);
      real_type a0  = Cs * dy - Ss * dx;
      real_type b0  = Ss * dy + Cs * dx;
      real_type tmp = a0*kappa;

      if ( 1+2*tmp > 0 ) {

        tmp = b0/(1+tmp);
        dS = -tmp*Atanc(tmp*kappa);

      } else {

        real_type om = atan2( b0, a0+1/kappa );
        if ( kappa < 0 ) {
          if ( om < 0 ) om += Utils::m_pi;
          else          om -= Utils::m_pi;
        }

        dS = -om/kappa;
      }

      s += dS;
      if ( abs( dS ) < epsi ) {
        if ( s < a-epsi|| s > b+epsi ) break;
        S = s;
        return true;
      }

      // check divergence
      if      ( s < a ) { s = a; ++nb; }
      else if ( s > b ) { s = b; ++nb; }
      else              { nb = 0; }

    }
    return false;
  }
  #endif

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  #ifndef DOXYGEN_SHOULD_SKIP_THIS
  static
  real_type
  closestPointStandard2(
    real_type   epsi,
    real_type   a,
    real_type   b,
    real_type   qx,
    real_type   qy,
    real_type & S
  ) {

    real_type dx, dy;
    FresnelCS( a, dx, dy ); dx -= qx; dy -= qy;
    real_type phia = Utils::m_pi_2 * (a*a) - atan2( dy, dx );
    bool ok0 = cos(phia) < 0; // distanza decrescente

    FresnelCS( b, dx, dy ); dx -= qx; dy -= qy;
    real_type phib = Utils::m_pi_2 * (b*b) - atan2( dy, dx );
    bool ok1 = cos(phib) > 0; // distanza crescente

    real_type s0 = a;
    if ( ok0 ) ok0 = closestPointStandard3( epsi, a, b, qx, qy, s0 );
    FresnelCS( s0, dx, dy ); dx -= qx; dy -= qy;
    real_type d0 = hypot( dx, dy );

    real_type s1 = b;
    if ( ok1 ) ok1 = closestPointStandard3( epsi, a, b, qx, qy, s1 );
    FresnelCS( s1, dx, dy ); dx -= qx; dy -= qy;
    real_type d1 = hypot( dx, dy );

    if ( !ok0 && !ok1 ) {  // s1 - s0 > 2 * epsi ) { // buoni entrambi estremi
      S = (s0+s1)/2;
      bool ok = closestPointStandard3( epsi, a, b, qx, qy, S );
      if ( ok ) {
        FresnelCS( S, dx, dy ); dx -= qx; dy -= qy;
        real_type dm = hypot( dx, dy );
        if ( dm < d0 && dm < d1 ) return dm;
      }
    }

    if ( d0 < d1 ) { S = s0; return d0; }
    else           { S = s1; return d1; }

  }
  #endif

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  #ifndef DOXYGEN_SHOULD_SKIP_THIS
  static
  real_type
  closestPointStandard(
    real_type            epsi,
    ClothoidData const & CD,
    real_type            L,
    real_type            qx,
    real_type            qy,
    real_type          & S
  ) {

    // transform to standard clothoid
    real_type sflex  = -CD.kappa0/CD.dk;

    UTILS_ASSERT( sflex <= 0, " bad sflex = {}\n", sflex );

    real_type thflex = CD.theta0 + 0.5*CD.kappa0*sflex;
    real_type ssf    = sin(thflex);
    real_type csf    = cos(thflex);
    real_type gamma  = sqrt(abs(CD.dk)/Utils::m_pi);
    real_type a      = -sflex*gamma;
    real_type b      = (L-sflex)*gamma;
    real_type xflex, yflex;
    CD.eval( sflex, xflex, yflex );
    real_type xx = qx - xflex;
    real_type yy = qy - yflex;
    // gamma * R^(-1)
    real_type qqx = gamma * (  csf * xx + ssf * yy );
    real_type qqy = gamma * ( -ssf * xx + csf * yy );
    // M^(-1)
    if ( CD.dk < 0 ) qqy = -qqy;

    // now in standard form
    if ( b*b-a*a <= 4 ) {
      real_type d = closestPointStandard2( epsi, a, b, qqx, qqy, S );
      S = sflex + S/gamma;
      return d/gamma;
    }

    FresnelCS( a, xx, yy );
    real_type di = hypot(qqx-0.5,qqy-0.5);
    real_type da = hypot(xx-0.5,yy-0.5);

    if ( di >= da ) {
      real_type La = 4/(a+sqrt(a*a+4));
      real_type d  = closestPointStandard2( epsi, a, a+La, qqx, qqy, S );
      S = sflex + S/gamma;
      return d/gamma;
    }

    FresnelCS( b, xx, yy );
    real_type db = hypot(xx-0.5,yy-0.5);

    if ( di <= db ) {
      real_type Lb = 4/(b+sqrt(b*b-4));
      real_type d  = closestPointStandard2( epsi, b-Lb, b, qqx, qqy, S );
      S = sflex + S/gamma;
      return d/gamma;
    }

    real_type ss = a;
    bool converged = false;
    for ( int iter = 0; iter < 20 && !converged; ++iter ) {
      FresnelCS( ss, xx, yy );
      real_type kappa = Utils::m_pi * ss;
      real_type theta = Utils::m_pi_2 * (ss*ss);
      real_type rhox  = xx - 0.5;
      real_type rhoy  = yy - 0.5;
      real_type rho   = hypot( rhox, rhoy );
      real_type f     = rho - di;
      //if ( abs(f) < epsi ) break;
      real_type tphi  = theta - atan2( rhoy, rhox );
      real_type df    = cos( tphi );
      real_type t     = sin( tphi );
      real_type ddf   = t*(kappa-t/rho);
      real_type ds    = (f*df)/((df*df)-f*ddf/2);
      ss -= ds;
      converged = abs(ds) < epsi;
    }

    UTILS_ASSERT0( converged, "closestPointStandard not converged\n" );

    real_type Lp = min( b-ss, 4/(ss+sqrt(ss*ss+4)) );
    real_type Lm = min( ss-a, 4/(ss+sqrt(ss*ss-4)) );

    real_type sp, sm;
    real_type dp = closestPointStandard2( epsi, ss, ss+Lp, qqx, qqy, sp );
    real_type dm = closestPointStandard2( epsi, ss-Lm, ss, qqx, qqy, sm );

    if ( dp < dm ) { S = sflex + sp/gamma; return dp/gamma; }
    else           { S = sflex + sm/gamma; return dm/gamma; }
  }
  #endif

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  #ifndef DOXYGEN_SHOULD_SKIP_THIS
  static
  real_type
  closestPoint1(
    real_type            epsi,
    ClothoidData const & CD,
    real_type            L,
    real_type            qx,
    real_type            qy,
    real_type          & X,
    real_type          & Y,
    real_type          & S
  ) {

    real_type NT = 4; // number of turn of the clothid after wich is considered quasi-circular
    real_type DK = sqrt(NT*Utils::m_2pi*abs(CD.dk));
    if ( abs(CD.kappa0) >= DK ) {
      return closestPointQC( epsi, CD, L, qx, qy, X, Y, S );
    }

    if ( abs(CD.kappa0)+abs(CD.dk)*L <= DK ) {
      real_type d = closestPointStandard( epsi, CD, L, qx, qy, S );
      CD.eval( S, X, Y );
      return d;
    }

    real_type ell = (DK-abs(CD.kappa0))/abs(CD.dk);

    UTILS_ASSERT( ell > 0 && ell < L, "bad ell = {} L = {}\n", ell, L );

    ClothoidData CDS;
    CD.eval( ell, CDS );

    real_type S0;
    real_type d0 = closestPointStandard( epsi, CD, ell, qx, qy, S0 );
    real_type d1 = closestPointQC( epsi, CDS, L-ell, qx, qy, X, Y, S );
    if ( d0 < d1 ) {
      S = S0;
      CD.eval( S, X, Y );
      return d0;
    } else {
      S += ell;
      return d1;
    }
  }
  #endif

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

  int_type
  ClothoidCurve::closestPoint_ISO(
    real_type   qx,
    real_type   qy,
    real_type & X,
    real_type & Y,
    real_type & S,
    real_type & T,
    real_type & dst
  ) const {

    real_type epsi = 1e-10;

    // check if flex is inside curve, if so then split

    if ( m_CD.kappa0*m_CD.dk >= 0 ) { // flex on the left
      dst = closestPoint1( epsi, m_CD, m_L, qx, qy, X, Y, S );
    } else if ( m_CD.dk*m_CD.kappa(m_L) <= 0 ) { // flex on the right, reverse curve
      ClothoidData CD1;
      m_CD.reverse( m_L, CD1 );
      dst = closestPoint1( epsi, CD1, m_L, qx, qy, X, Y, S );
      S   = m_L-S;
    } else {
      // flex inside, split clothoid
      ClothoidData C0, C1;
      real_type sflex = m_CD.split_at_flex( C0, C1 );

      real_type d0 = closestPoint1( epsi, C0, m_L-sflex, qx, qy, X, Y, S  );
      real_type x1, y1, s1;
      real_type d1 = closestPoint1( epsi, C1, sflex, qx, qy, x1, y1, s1 );

      if ( d1 < d0 ) {
        S   = sflex - s1;
        X   = x1;
        Y   = y1;
        dst = d1;
      } else {
        S  += sflex;
        dst = d0;
      }
    }
    // check if projection is orthogonal
    real_type nx, ny;
    nor_ISO( S, nx, ny );
    real_type qxx = qx - X;
    real_type qyy = qy - Y;
    T = qxx * nx + qyy * ny; // signed distance
    real_type pt = abs(qxx * ny - qyy * nx);
    G2LIB_DEBUG_MESSAGE(
      "ClothoidCurve::closestPoint_ISO: ||P-P0|| = {}, |(P-P0).T| = {}\n",
      dst, pt
    );
    return pt > GLIB2_TOL_ANGLE*dst ? -1 : 1;
  }

/*
+      // approx clothoid with a circle
       real_type theta, kappa;
       eval( S, theta, kappa, X, Y );
-      real_type dx   = X-x;
-      real_type dy   = Y-y;
-      real_type d    = hypot( dx, dy );
-      real_type tphi = theta - atan2( dy, dx );
-      real_type f    = d*cos(tphi);
-      real_type g    = d*sin(tphi);
-      real_type df   = 1-kappa*g;
-      real_type ddf  = -kappa*f*(dk+kappa);
-
*/

}

// EOF: ClothoidDistance.cc
