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

#include "Clothoids.hh"

#include <cfloat>

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic ignored "-Wswitch-enum"
#endif
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wsign-conversion"
#pragma clang diagnostic ignored "-Wswitch-enum"
#endif

namespace G2lib {
  
  #ifndef DOXYGEN_SHOULD_SKIP_THIS

  using std::abs;
  using std::fpclassify;
  using std::copy;
  using std::back_inserter;
  using std::fill;
  using std::vector;

  inline
  real_type
  power2( real_type a )
  { return a*a; }

  inline
  real_type
  power3( real_type a )
  { return a*a*a; }

  inline
  real_type
  power4( real_type a )
  { real_type a2 = a*a; return a2*a2; }

  #endif

  /*\
   |    ____ ____            _           ____
   |   / ___|___ \ ___  ___ | |_   _____|___ \ __ _ _ __ ___
   |  | |  _  __) / __|/ _ \| \ \ / / _ \ __) / _` | '__/ __|
   |  | |_| |/ __/\__ \ (_) | |\ V /  __// __/ (_| | | | (__
   |   \____|_____|___/\___/|_| \_/ \___|_____\__,_|_|  \___|
  \*/

  int
  G2solve2arc::build(
    real_type _x0,
    real_type _y0,
    real_type _theta0,
    real_type _kappa0,
    real_type _x1,
    real_type _y1,
    real_type _theta1,
    real_type _kappa1
  ) {

    x0     = _x0;
    y0     = _y0;
    theta0 = _theta0;
    kappa0 = _kappa0;
    x1     = _x1;
    y1     = _y1;
    theta1 = _theta1;
    kappa1 = _kappa1;

    // scale problem
    real_type dx = x1 - x0;
    real_type dy = y1 - y0;
    phi    = atan2( dy, dx );
    lambda = hypot( dx, dy );

    real_type C = dx/lambda;
    real_type S = dy/lambda;
    lambda /= 2;

    xbar = -(x0*C+y0*S+lambda);
    ybar = x0*S-y0*C;

    th0 = theta0 - phi;
    th1 = theta1 - phi;

    k0 = kappa0*lambda;
    k1 = kappa1*lambda;

    DeltaK     = k1  - k0;
    DeltaTheta = th1 - th0;

    return solve();
  }

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

  void
  G2solve2arc::setTolerance( real_type tol ) {
    UTILS_ASSERT(
      tol > 0 && tol <= 0.1,
      "G2solve2arc::setTolerance, tolerance = {} must be in (0,0.1]\n", tol
    );
    tolerance = tol;
  }

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

  void
  G2solve2arc::setMaxIter( int miter ) {
    UTILS_ASSERT(
      miter > 0 && miter <= 1000,
      "G2solve2arc::setMaxIter, maxIter = {} must be in [1,1000]\n", miter
    );
    maxIter = miter;
  }

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

  void
  G2solve2arc::evalA(
    real_type   alpha,
    real_type   L,
    real_type & A
  ) const {
    real_type K  = k0+k1;
    real_type aK = alpha*DeltaK;
    A = alpha*(L*(aK-K)+2*DeltaTheta);
  }

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

  void
  G2solve2arc::evalA(
    real_type   alpha,
    real_type   L,
    real_type & A,
    real_type & A_1,
    real_type & A_2
  ) const {
    real_type K  = k0+k1;
    real_type aK = alpha*DeltaK;
    A   = alpha*(L*(aK-K)+2*DeltaTheta);
    A_1 = (2*aK-K)*L+2*DeltaTheta;
    A_2 = alpha*(aK-K);
  }

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

  void
  G2solve2arc::evalG(
    real_type alpha,
    real_type L,
    real_type th,
    real_type k,
    real_type G[2]
  ) const {
    real_type A, X, Y;
    evalA( alpha, L, A );
    real_type ak = alpha*k;
    GeneralizedFresnelCS( A, ak*L, th, X, Y );
    G[0] = alpha*X;
    G[1] = alpha*Y;
  }

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

  void
  G2solve2arc::evalG(
    real_type alpha,
    real_type L,
    real_type th,
    real_type k,
    real_type G[2],
    real_type G_1[2],
    real_type G_2[2]
  ) const {

    real_type A, A_1, A_2, X[3], Y[3];
    evalA( alpha, L, A, A_1, A_2 );
    real_type ak = alpha*k;
    real_type Lk = L*k;
    GeneralizedFresnelCS( 3, A, ak*L, th, X, Y );

    G[0]   = alpha*X[0];
    G_1[0] = X[0]-alpha*(Y[2]*A_1/2+Y[1]*Lk);
    G_2[0] =     -alpha*(Y[2]*A_2/2+Y[1]*ak);

    G[1]   = alpha*Y[0];
    G_1[1] = Y[0]+alpha*(X[2]*A_1/2+X[1]*Lk);
    G_2[1] =      alpha*(X[2]*A_2/2+X[1]*ak);

  }

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

  void
  G2solve2arc::evalF( real_type const vars[2], real_type F[2] ) const {
    real_type alpha = vars[0];
    real_type L     = vars[1];
    real_type G[2];
    evalG( alpha, L, th0, k0, G );
    F[0] = G[0] - 2/L;  F[1] = G[1];
    evalG( alpha-1, L, th1, k1, G );
    F[0] -= G[0]; F[1] -= G[1];
  }

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

  void
  G2solve2arc::evalFJ(
    real_type const vars[2],
    real_type       F[2],
    real_type       J[2][2]
  ) const {

    real_type alpha = vars[0];
    real_type L     = vars[1];
    real_type G[2], G_1[2], G_2[2];

    evalG( alpha, L, th0, k0, G, G_1, G_2 );

    F[0]    = G[0] - 2/L;       F[1]    = G[1];
    J[0][0] = G_1[0];           J[1][0] = G_1[1];
    J[0][1] = G_2[0] + 2/(L*L); J[1][1] = G_2[1];

    evalG( alpha-1, L, th1, k1, G, G_1, G_2 );
    F[0]    -= G[0];   F[1]    -= G[1];
    J[0][0] -= G_1[0]; J[1][0] -= G_1[1];
    J[0][1] -= G_2[0]; J[1][1] -= G_2[1];
  }

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

  int
  G2solve2arc::solve() {
    Solve2x2 solver;
    real_type X[2] = { 0.5, 2 };
    int iter = 0;
    bool converged = false;
    do {
      real_type F[2], J[2][2], d[2];
      evalFJ( X, F, J );
      if ( !solver.factorize( J ) ) break;
      solver.solve( F, d );
      real_type lenF = hypot(F[0],F[1]);
      #if 0
      X[0] -= d[0];
      X[1] -= d[1];
      #else
      real_type FF[2], dd[2], XX[2];
      // Affine invariant Newton solver
      real_type nd = hypot( d[0], d[1] );
      bool step_found = false;
      real_type tau = 2;
      do {
        tau  /= 2;
        XX[0] = X[0]-tau*d[0];
        XX[1] = X[1]-tau*d[1];
        evalF(XX, FF);
        solver.solve(FF, dd);
        step_found = hypot( dd[0], dd[1] ) <= (1-tau/2)*nd + 1e-6
                     && XX[0] > 0 && XX[0] < 1 && XX[1] > 0;
      } while ( tau > 1e-6 && !step_found );
      if ( !step_found ) break;
      X[0] = XX[0];
      X[1] = XX[1];
      #endif
      converged = lenF < tolerance;
    } while ( ++iter < maxIter && !converged );
    if ( converged ) converged = X[1] > 0 && X[0] > 0 && X[0] < 1;
    if ( converged ) buildSolution( X[0], X[1] );
    return converged ? iter : -1;
  }

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

  void
  G2solve2arc::buildSolution( real_type alpha, real_type L ) {
    real_type beta = 1-alpha;
    real_type s0   = L*alpha;
    real_type s1   = L*beta;
    real_type tmp  = 2*DeltaTheta-L*(k0+k1);
    real_type A0   = alpha*(s0*DeltaK+tmp);
    real_type A1   = beta*(s1*DeltaK-tmp);

    real_type dk0  = A0/(s0*s0);
    real_type dk1  = A1/(s1*s1);

    // transform solution from (-1,0)--(1,0) to (x0,y0)--(x1,y1)
    //S0.build( -1, 0, th0, k0, dk0, s0 );
    //S1.build(  1, 0, th1, k1, dk1, s1 );
    //S1.changeCurvilinearOrigin( -s1, s1 );
    s0  *= lambda;
    s1  *= lambda;
    dk0 /= lambda*lambda;
    dk1 /= lambda*lambda;

    S0.build( x0, y0, theta0, kappa0, dk0, s0 );
    S1.build( x1, y1, theta1, kappa1, dk1, s1 );
    S1.changeCurvilinearOrigin( -s1, s1 );
  }

  /*\
   |    ____ ____            _            ____ _     ____
   |   / ___|___ \ ___  ___ | |_   _____ / ___| |   / ___|
   |  | |  _  __) / __|/ _ \| \ \ / / _ \ |   | |  | |
   |  | |_| |/ __/\__ \ (_) | |\ V /  __/ |___| |__| |___
   |   \____|_____|___/\___/|_| \_/ \___|\____|_____\____|
  \*/

  int
  G2solveCLC::build(
    real_type _x0,
    real_type _y0,
    real_type _theta0,
    real_type _kappa0,
    real_type _x1,
    real_type _y1,
    real_type _theta1,
    real_type _kappa1
  ) {

    x0     = _x0;
    y0     = _y0;
    theta0 = _theta0;
    kappa0 = _kappa0;
    x1     = _x1;
    y1     = _y1;
    theta1 = _theta1;
    kappa1 = _kappa1;

    // scale problem
    real_type dx = x1 - x0;
    real_type dy = y1 - y0;
    phi    = atan2( dy, dx );
    lambda = hypot( dx, dy );

    real_type C = dx/lambda;
    real_type S = dy/lambda;
    lambda /= 2;

    xbar = -(x0*C+y0*S+lambda);
    ybar = x0*S-y0*C;

    th0 = theta0 - phi;
    th1 = theta1 - phi;

    k0 = kappa0*lambda;
    k1 = kappa1*lambda;

    return solve();
  }

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

  void
  G2solveCLC::setTolerance( real_type tol ) {
    UTILS_ASSERT(
      tol > 0 && tol <= 0.1,
      "G2solveCLC::setTolerance, tolerance = {} must be in (0,0.1]\n", tol
    );
    tolerance = tol;
  }

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

  void
  G2solveCLC::setMaxIter( int miter ) {
    UTILS_ASSERT(
      miter > 0 && miter <= 1000,
      "G2solveCLC::setMaxIter, maxIter = {} must be in [1,1000]\n", miter
    );
    maxIter = miter;
  }

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

  int
  G2solveCLC::solve() {
    real_type X0[3], Y0[3], X1[3], Y1[3];
    real_type thM = 0, sM = 0.0;
    int iter = 0;
    bool converged = false;
    do {
      real_type D0 = thM - th0;
      real_type D1 = thM - th1;

      GeneralizedFresnelCS( 3, 2*D0, -2*D0, D0, X0, Y0 );
      GeneralizedFresnelCS( 3, 2*D1, -2*D1, D1, X1, Y1 );

      real_type F  = D0*k1*Y0[0]-D1*k0*Y1[0] - k0*k1*sin(thM);
      real_type dF = D0*k1*(X0[2]-2*X0[1]+X0[0])
                     - D1*k0*(X1[2]-2*X1[1]+X1[0])
                     - k0*k1*cos(thM)
                     + k1*Y0[0]-k0*Y1[0];

      if ( abs(dF) < 1e-10 ) break;
      real_type d = F/dF;
      #if 0
      thM -= d;
      #else
      real_type FF, dd, thM1;
      // Affine invariant Newton solver
      bool step_found = false;
      real_type tau = 2;
      do {
        tau  /= 2;
        thM1 = thM-tau*d;
        D0 = thM1 - th0;
        D1 = thM1 - th1;
        GeneralizedFresnelCS( 1, 2*D0, -2*D0, D0, X0, Y0 );
        GeneralizedFresnelCS( 1, 2*D1, -2*D1, D1, X1, Y1 );
        FF = D0*k1*Y0[0]-D1*k0*Y1[0] - k0*k1*sin(thM1);
        dd = FF/dF;
        step_found = abs( dd ) <= (1-tau/2)*abs(d) + 1e-6;
      } while ( tau > 1e-6 && !step_found );
      if ( !step_found ) break;
      thM = thM1;
      #endif
      converged = abs(d) < tolerance;
    } while ( ++iter < maxIter && !converged );
    if ( converged ) {
      real_type D0 = thM - th0;
      real_type D1 = thM - th1;
      GeneralizedFresnelCS( 1, 2*D0, -2*D0, D0, X0, Y0 );
      GeneralizedFresnelCS( 1, 2*D1, -2*D1, D1, X1, Y1 );
      sM = cos(thM) + D1*X1[0]/k1 - D0*X0[0]/k0;
      converged = sM > 0 && sM < 1e100;
    }
    if ( converged ) converged = buildSolution( sM, thM );
    return converged ? iter : -1;
  }

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

  bool
  G2solveCLC::buildSolution( real_type sM, real_type thM ) {
    real_type dk0 = 0.5*power2(k0/lambda)/(th0-thM);
    real_type dk1 = 0.5*power2(k1/lambda)/(th1-thM);
    real_type L0  = 2*lambda*(thM-th0)/k0;
    real_type L1  = 2*lambda*(th1-thM)/k1;

    if ( ! ( L0 > 0 && L1 > 0 ) ) return false;

    S0.build( x0, y0, theta0, kappa0, dk0, L0 );
    S1.build( x1, y1, theta1, kappa1, dk1, L1 );
    S1.changeCurvilinearOrigin( -L1, L1 );
    SM.build( S0.xEnd(), S0.yEnd(), S0.thetaEnd(), 0, 0, 2*sM*lambda );

    return true;
  }

  /*\
   |    ____ ____            _           _____
   |   / ___|___ \ ___  ___ | |_   _____|___ /  __ _ _ __ ___
   |  | |  _  __) / __|/ _ \| \ \ / / _ \ |_ \ / _` | '__/ __|
   |  | |_| |/ __/\__ \ (_) | |\ V /  __/___) | (_| | | | (__
   |   \____|_____|___/\___/|_| \_/ \___|____/ \__,_|_|  \___|
  \*/

  void
  G2solve3arc::setTolerance( real_type tol ) {
    UTILS_ASSERT(
      tol > 0 && tol <= 0.1,
      "G2solve3arc::setTolerance, tolerance = {} must be in (0,0.1]\n", tol
    );
    tolerance = tol;
  }

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

  void
  G2solve3arc::setMaxIter( int miter ) {
    UTILS_ASSERT(
      miter > 0 && miter <= 1000,
      "G2solve3arc::setMaxIter, maxIter = {} must be in [1,1000]\n", miter
    );
    maxIter = miter;
  }

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

  int
  G2solve3arc::build(
    real_type _x0,
    real_type _y0,
    real_type _theta0,
    real_type _kappa0,
    real_type _x1,
    real_type _y1,
    real_type _theta1,
    real_type _kappa1,
    real_type Dmax,
    real_type dmax
  ) {
    try {
      // save data
      x0     = _x0;
      y0     = _y0;
      theta0 = _theta0;
      kappa0 = _kappa0;
      x1     = _x1;
      y1     = _y1;
      theta1 = _theta1;
      kappa1 = _kappa1;

      // transform to reference frame
      real_type dx = x1 - x0;
      real_type dy = y1 - y0;
      phi    = atan2( dy, dx );
      Lscale = 2/hypot( dx, dy );

      th0 = theta0 - phi;
      th1 = theta1 - phi;

      // put in range
      rangeSymm(th0);
      rangeSymm(th1);

      K0 = (kappa0/Lscale); // k0
      K1 = (kappa1/Lscale); // k1

      if ( Dmax <= 0 ) Dmax = Utils::m_pi;
      if ( dmax <= 0 ) dmax = Utils::m_pi/8;

      if ( Dmax > Utils::m_2pi  ) Dmax = Utils::m_2pi;
      if ( dmax > Utils::m_pi/4 ) dmax = Utils::m_pi/4;

      // compute guess G1
      ClothoidCurve SG;
      SG.build_G1( -1, 0, th0, 1, 0, th1 );

      real_type kA = SG.kappaBegin();
      real_type kB = SG.kappaEnd();
      real_type dk = abs(SG.dkappa());
      real_type L3 = SG.length()/3;

      real_type tmp = 0.5*abs(K0-kA)/dmax;
      s0 = L3;
      if ( tmp*s0 > 1 ) s0 = 1/tmp;
      tmp = (abs(K0+kA)+s0*dk)/(2*Dmax);
      if ( tmp*s0 > 1 ) s0 = 1/tmp;

      tmp = 0.5*abs(K1-kB)/dmax;
      s1 = L3;
      if ( tmp*s1 > 1 ) s1 = 1/tmp;
      tmp = (abs(K1+kB)+s1*dk)/(2*Dmax);
      if ( tmp*s1 > 1 ) s1 = 1/tmp;

      real_type dth   = abs(th0-th1) / Utils::m_2pi;
      real_type scale = power3(cos( power4(dth)*Utils::m_pi_2 ));
      s0 *= scale;
      s1 *= scale;

      real_type L   = (3*L3-s0-s1)/2;
      real_type thM = SG.theta(s0+L);
      th0 = SG.thetaBegin();
      th1 = SG.thetaEnd();

      // setup

      K0 *= s0;
      K1 *= s1;

      real_type t0 = 2*th0+K0;
      real_type t1 = 2*th1-K1;

      c0  = s0*s1;
      c1  = 2 * s0;
      c2  = 0.25*((K1-6*(K0+th0)-2*th1)*s0 - 3*K0*s1);
      c3  = -c0 * (K0 + th0);
      c4  = 2 * s1;
      c5  = 0.25*((6*(K1-th1)-K0-2*th0)*s1 + 3*K1*s0);
      c6  = c0 * (K1 - th1);
      c7  = -0.5*(s0 + s1);
      c8  = th0 + th1 + 0.5*(K0 - K1);
      c9  = 0.25*(t1*s0 + t0*s1);
      c10 = 0.5*(s1 - s0);
      c11 = 0.5*(th1 - th0) - 0.25*(K0 + K1);
      c12 = 0.25*(t1*s0 - t0*s1);
      c13 = 0.5*s0*s1;
      c14 = 0.75*(s0 + s1);
      return solve( L, thM );
    } catch (...) {
      return -1;
      // nothing to do
    }
  }

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

  int
  G2solve3arc::build_fixed_length(
    real_type _s0,
    real_type _x0,
    real_type _y0,
    real_type _theta0,
    real_type _kappa0,
    real_type _s1,
    real_type _x1,
    real_type _y1,
    real_type _theta1,
    real_type _kappa1
  ) {
    try {
      // save data
      x0     = _x0;
      y0     = _y0;
      theta0 = _theta0;
      kappa0 = _kappa0;
      x1     = _x1;
      y1     = _y1;
      theta1 = _theta1;
      kappa1 = _kappa1;

      // transform to reference frame
      real_type dx = x1 - x0;
      real_type dy = y1 - y0;
      phi    = atan2( dy, dx );
      Lscale = 2/hypot( dx, dy );

      th0 = theta0 - phi;
      th1 = theta1 - phi;

      // put in range
      rangeSymm(th0);
      rangeSymm(th1);

      K0 = (kappa0/Lscale); // k0
      K1 = (kappa1/Lscale); // k1

      // compute guess G1
      ClothoidCurve SG;
      SG.build_G1( -1, 0, th0, 1, 0, th1 );

      s0 = _s0 * Lscale;
      s1 = _s1 * Lscale;

      real_type L   = (SG.length()-s0-s1)/2;
      real_type thM = SG.theta(s0+L);
      th0 = SG.thetaBegin();
      th1 = SG.thetaEnd();

      // setup

      K0 *= s0;
      K1 *= s1;

      real_type t0 = 2*th0+K0;
      real_type t1 = 2*th1-K1;

      c0  = s0*s1;
      c1  = 2 * s0;
      c2  = 0.25*((K1-6*(K0+th0)-2*th1)*s0 - 3*K0*s1);
      c3  = -c0 * (K0 + th0);
      c4  = 2 * s1;
      c5  = 0.25*((6*(K1-th1)-K0-2*th0)*s1 + 3*K1*s0);
      c6  = c0 * (K1 - th1);
      c7  = -0.5*(s0 + s1);
      c8  = th0 + th1 + 0.5*(K0 - K1);
      c9  = 0.25*(t1*s0 + t0*s1);
      c10 = 0.5*(s1 - s0);
      c11 = 0.5*(th1 - th0) - 0.25*(K0 + K1);
      c12 = 0.25*(t1*s0 - t0*s1);
      c13 = 0.5*s0*s1;
      c14 = 0.75*(s0 + s1);

      return solve( L, thM );

    } catch (...) {

      return -1;
      // nothing to do
    }
  }

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

  void
  G2solve3arc::evalF( real_type const vars[2], real_type F[2] ) const {

    real_type sM  = vars[0];
    real_type thM = vars[1];

    real_type dsM = 1.0 / (c13+(c14+sM)*sM);
    real_type dK0 = dsM*(c0*thM + sM*(c1*thM - K0*sM + c2) + c3);
    real_type dK1 = dsM*(c0*thM + sM*(c4*thM + K1*sM + c5) + c6);
    real_type dKM = dsM*sM*( thM*(c7-2*sM) + c8*sM + c9);
    real_type KM  = dsM*sM*(c10*thM + c11*sM + c12);

    real_type X0, Y0, X1, Y1, XMp, YMp, XMm, YMm;
    GeneralizedFresnelCS( dK0,  K0, th0, X0,  Y0);
    GeneralizedFresnelCS( dK1, -K1, th1, X1,  Y1);
    GeneralizedFresnelCS( dKM,  KM, thM, XMp, YMp);
    GeneralizedFresnelCS( dKM, -KM, thM, XMm, YMm);

    // in the standard problem dx = 2, dy = 0
    F[0] = s0*X0 + s1*X1 + sM*(XMm + XMp) - 2;
    F[1] = s0*Y0 + s1*Y1 + sM*(YMm + YMp) - 0;
  }

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

  void
  G2solve3arc::evalFJ(
    real_type const vars[2],
    real_type       F[2],
    real_type       J[2][2]
  ) const {

    real_type sM  = vars[0];
    real_type thM = vars[1];

    real_type dsM   = 1.0 / (c13+(c14+sM)*sM);
    real_type dsMsM = dsM*sM;
    real_type dK0   = dsM*(c0*thM + sM*(c1*thM + c2 - sM*K0) + c3);
    real_type dK1   = dsM*(c0*thM + sM*(c4*thM + c5 + sM*K1) + c6);
    real_type dKM   = dsMsM*(thM*(c7-2*sM) + c8*sM + c9);
    real_type KM    = dsMsM*(c10*thM + c11*sM + c12);

    real_type X0[3],  Y0[3],
              X1[3],  Y1[3],
              XMp[3], YMp[3],
              XMm[3], YMm[3];
    GeneralizedFresnelCS( 3, dK0,  K0, th0, X0,  Y0);
    GeneralizedFresnelCS( 3, dK1, -K1, th1, X1,  Y1);
    GeneralizedFresnelCS( 3, dKM,  KM, thM, XMp, YMp);
    GeneralizedFresnelCS( 3, dKM, -KM, thM, XMm, YMm);

    // in the standard problem dx = 2, dy = 0
    real_type t0 = XMp[0]+XMm[0];
    real_type t1 = YMp[0]+YMm[0];
    F[0] = s0*X0[0] + s1*X1[0] + sM*t0 - 2;
    F[1] = s0*Y0[0] + s1*Y1[0] + sM*t1 - 0;

    // calcolo J(F)
    real_type dsM2 = dsM*dsM;
    real_type g0   = -(2 * sM + c14)*dsM2;
    real_type g1   = (c13 - sM*sM)*dsM2;
    real_type g2   = sM*(sM*c14+2*c13)*dsM2;

    real_type dK0_sM  = (c0*thM+c3)*g0 + (c1*thM+c2)*g1 - K0*g2;
    real_type dK1_sM  = (c0*thM+c6)*g0 + (c4*thM+c5)*g1 + K1*g2;
    real_type dKM_sM  = (c7*thM+c9)*g1 + (c8-2*thM)*g2;
    real_type KM_sM   = (c10*thM+c12)*g1 + c11*g2;

    real_type dK0_thM = (c0+c1*sM)*dsM;
    real_type dK1_thM = (c0+c4*sM)*dsM;
    real_type dKM_thM = (c7-2*sM)*dsMsM;
    real_type KM_thM  = c10*dsMsM;

    // coeff fresnel per f_j per lo jacobiano
    real_type f0 = -0.5*s0*Y0[2];
    real_type f1 = -0.5*s1*Y1[2];
    real_type f2 = -0.5*sM*(YMm[2] + YMp[2]);
    real_type f3 = sM*(YMm[1] - YMp[1]);
    real_type f4 = 0.5*s0*X0[2];
    real_type f5 = 0.5*s1*X1[2];
    real_type f6 = 0.5*sM*(XMm[2] + XMp[2]);
    real_type f7 = sM*(XMp[1] - XMm[1]);

    J[0][0] = f0 * dK0_sM  + f1 * dK1_sM  + f2 * dKM_sM  + f3 * KM_sM  + t0;
    J[0][1] = f0 * dK0_thM + f1 * dK1_thM + f2 * dKM_thM + f3 * KM_thM - sM * t1;
    J[1][0] = f4 * dK0_sM  + f5 * dK1_sM  + f6 * dKM_sM  + f7 * KM_sM  + t1;
    J[1][1] = f4 * dK0_thM + f5 * dK1_thM + f6 * dKM_thM + f7 * KM_thM + sM * t0;
  }

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

  int
  G2solve3arc::solve( real_type sM_guess, real_type thM_guess ) {

    Solve2x2 solver;
    real_type F[2], d[2], X[2], J[2][2];
    X[0] = sM_guess;
    X[1] = thM_guess;

    //real_type thmin = min(th0,th1)-2*m_2pi;
    //real_type thmax = max(th0,th1)+2*m_2pi;

    int iter = 0;
    bool converged = false;
    try {
      do {
        evalFJ(X, F, J);
        real_type lenF = hypot(F[0], F[1]);
        converged = lenF < tolerance;
        if ( converged || !solver.factorize(J) ) break;
        solver.solve(F, d);
        #if 1
        // use undamped Newton
        X[0] -= d[0];
        X[1] -= d[1];
        #else
        real_type FF[2], dd[2], XX[2];
        // Affine invariant Newton solver
        real_type nd = hypot( d[0], d[1] );
        bool step_found = false;
        real_type tau = 2;
        do {
          tau  /= 2;
          XX[0] = X[0]-tau*d[0];
          XX[1] = X[1]-tau*d[1];
          evalF(XX, FF);
          solver.solve(FF, dd);
          step_found = hypot( dd[0], dd[1] ) <= (1-tau/2)*nd + 1e-6;
                       //&& XX[0] > 0; // && XX[0] > X[0]/4 && XX[0] < 4*X[0];
                       //&& XX[1] > thmin && XX[1] < thmax;
        } while ( tau > 1e-6 && !step_found );
        if ( !step_found ) break;
        X[0] = XX[0];
        X[1] = XX[1];
        #endif
      } while ( ++iter < maxIter );

      // re-check solution
      if ( converged )
        converged = FP_INFINITE != fpclassify(X[0]) &&
                    FP_NAN      != fpclassify(X[0]) &&
                    FP_INFINITE != fpclassify(X[1]) &&
                    FP_NAN      != fpclassify(X[1]);
    }
    catch (...) {
      std::cerr << "G2solve3arc::solve, something go wrong\n";
      // nothing to do
    }
    if ( converged ) buildSolution(X[0], X[1]); // costruisco comunque soluzione
    return converged ? iter : -1;
  }

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

  void
  G2solve3arc::buildSolution( real_type sM, real_type thM ) {
    // soluzione nel frame di riferimento
    /* real_type k0 = K0
     S0.build( -1, 0, th0, k0, dK0,   0, L0 );
     S1.build( x1, y1, phi+th1, kappa1, dK1, -L1, 0  );
     S1.change_origin(-L1);
    */

    // ricostruzione dati clotoidi trasformati
    real_type dsM = 1.0 / (c13+(c14+sM)*sM);
    real_type dK0 = dsM*(c0*thM + sM*(c1*thM - K0*sM + c2) + c3);
    real_type dK1 = dsM*(c0*thM + sM*(c4*thM + K1*sM + c5) + c6);
    real_type dKM = dsM*sM*(c7*thM + sM*(c8 - 2*thM) + c9);
    real_type KM  = dsM*sM*(c10*thM + c11*sM + c12);

    real_type xa, ya, xmL, ymL;
    GeneralizedFresnelCS( dK0,  K0, th0, xa,  ya  );
    GeneralizedFresnelCS( dKM, -KM, thM, xmL, ymL );

    real_type xM = s0 * xa + sM * xmL - 1;
    real_type yM = s0 * ya + sM * ymL;

    // rovescia trasformazione standard
    real_type L0 = s0/Lscale;
    real_type L1 = s1/Lscale;
    real_type LM = sM/Lscale;

    dK0 *= power2(Lscale/s0);
    dK1 *= power2(Lscale/s1);
    dKM *= power2(Lscale/sM);
    KM  *= Lscale/sM;

    //th0 = theta0 - phi;
    //th1 = theta1 - phi;
    S0.build( x0, y0, phi+th0, kappa0, dK0, L0 );
    S1.build( x1, y1, phi+th1, kappa1, dK1, L1 );
    S1.changeCurvilinearOrigin( -L1, L1 );

    // la trasformazione inversa da [-1,1] a (x0,y0)-(x1,y1)
    // g(x,y) = RotInv(phi)*(1/lambda*[X;Y] - [xbar;ybar]) = [x;y]

    real_type C  = cos(phi);
    real_type S  = sin(phi);
    real_type dx = (xM + 1) / Lscale;
    real_type dy = yM / Lscale;
    SM.build(
      x0 + C * dx - S * dy,
      y0 + C * dy + S * dx,
      thM + phi, KM, dKM, 2*LM
    );
    SM.changeCurvilinearOrigin( -LM, 2*LM );
  }

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

  real_type
  G2solve3arc::thetaMinMax( real_type & thMin, real_type & thMax ) const {
    real_type thMin1, thMax1;
    S0.thetaMinMax( thMin,  thMax );
    S1.thetaMinMax( thMin1, thMax1 );
    if ( thMin > thMin1 ) thMin = thMin1;
    if ( thMax < thMax1 ) thMax = thMax1;
    SM.thetaMinMax( thMin1, thMax1 );
    if ( thMin > thMin1 ) thMin = thMin1;
    if ( thMax < thMax1 ) thMax = thMax1;
    return thMax-thMin;
  }

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

  real_type
  G2solve3arc::curvatureMinMax( real_type & kMin, real_type & kMax ) const {
    real_type kMin1, kMax1;
    S0.curvatureMinMax( kMin,  kMax );
    S1.curvatureMinMax( kMin1, kMax1 );
    if ( kMin > kMin1 ) kMin = kMin1;
    if ( kMax < kMax1 ) kMax = kMax1;
    SM.curvatureMinMax( kMin1, kMax1 );
    if ( kMin > kMin1 ) kMin = kMin1;
    if ( kMax < kMax1 ) kMax = kMax1;
    return kMax-kMin;
  }

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

  real_type
  G2solve3arc::theta( real_type s ) const {
    if ( s < S0.length() ) return S0.theta(s);
    s -= S0.length();
    if ( s < SM.length() ) return SM.theta(s);
    s -= S0.length();
    return S1.theta(s);
  }

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

  real_type
  G2solve3arc::theta_D( real_type s ) const {
    if ( s < S0.length() ) return S0.theta_D(s);
    s -= S0.length();
    if ( s < SM.length() ) return SM.theta_D(s);
    s -= S0.length();
    return S1.theta_D(s);
  }

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

  real_type
  G2solve3arc::theta_DD( real_type s ) const {
    if ( s < S0.length() ) return S0.theta_DD(s);
    s -= S0.length();
    if ( s < SM.length() ) return SM.theta_DD(s);
    s -= S0.length();
    return S1.theta_DD(s);
  }

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

  real_type
  G2solve3arc::theta_DDD( real_type s ) const {
    if ( s < S0.length() ) return S0.theta_DDD(s);
    s -= S0.length();
    if ( s < SM.length() ) return SM.theta_DDD(s);
    s -= S0.length();
    return S1.theta_DDD(s);
  }

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

  real_type
  G2solve3arc::X( real_type s ) const {
    if ( s < S0.length() ) return S0.X(s);
    s -= S0.length();
    if ( s < SM.length() ) return SM.X(s);
    s -= S0.length();
    return S1.X(s);
  }

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

  real_type
  G2solve3arc::Y( real_type s ) const {
    if ( s < S0.length() ) return S0.Y(s);
    s -= S0.length();
    if ( s < SM.length() ) return SM.Y(s);
    s -= S0.length();
    return S1.Y(s);
  }

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

  void
  G2solve3arc::eval(
    real_type   s,
    real_type & theta,
    real_type & kappa,
    real_type & x,
    real_type & y
  ) const {
    if ( s < S0.length() ) {
      S0.evaluate( s, theta, kappa, x, y );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.evaluate( s, theta, kappa, x, y );
      } else {
        s -= SM.length();
        S1.evaluate( s, theta, kappa, x, y );
      }
    }
  }

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

  void
  G2solve3arc::eval(
    real_type   s,
    real_type & x,
    real_type & y
  ) const {
    if ( s < S0.length() ) {
      S0.eval(s, x, y );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.eval(s, x, y );
      } else {
        s -= SM.length();
        S1.eval(s, x, y );
      }
    }
  }

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

  void
  G2solve3arc::eval_D(
    real_type   s,
    real_type & x_D,
    real_type & y_D
  ) const {
    if ( s < S0.length() ) {
      S0.eval_D(s, x_D, y_D );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.eval_D(s, x_D, y_D );
      } else {
        s -= SM.length();
        S1.eval_D(s, x_D, y_D );
      }
    }
  }

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

  void
  G2solve3arc::eval_DD(
    real_type   s,
    real_type & x_DD,
    real_type & y_DD
  ) const {
    if ( s < S0.length() ) {
      S0.eval_DD(s, x_DD, y_DD );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.eval_DD(s, x_DD, y_DD );
      } else {
        s -= SM.length();
        S1.eval_DD(s, x_DD, y_DD );
      }
    }
  }

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

  void
  G2solve3arc::eval_DDD(
    real_type   s,
    real_type & x_DDD,
    real_type & y_DDD
  ) const {
    if ( s < S0.length() ) {
      S0.eval_DDD(s, x_DDD, y_DDD );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.eval_DDD(s, x_DDD, y_DDD );
      } else {
        s -= SM.length();
        S1.eval_DDD(s, x_DDD, y_DDD );
      }
    }
  }

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

  // offset curve
  void
  G2solve3arc::eval_ISO(
    real_type   s,
    real_type   offs,
    real_type & x,
    real_type & y
  ) const {
    if ( s < S0.length() ) {
      S0.eval_ISO( s, offs, x, y );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.eval_ISO( s, offs, x, y );
      } else {
        s -= SM.length();
        S1.eval_ISO( s, offs, x, y );
      }
    }
  }

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

  void
  G2solve3arc::eval_ISO_D(
    real_type   s,
    real_type   offs,
    real_type & x_D,
    real_type & y_D
  ) const {
    if ( s < S0.length() ) {
      S0.eval_ISO_D( s, offs, x_D, y_D );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.eval_ISO_D( s, offs, x_D, y_D );
      } else {
        s -= SM.length();
        S1.eval_ISO_D( s, offs, x_D, y_D );
      }
    }
  }

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

  void
  G2solve3arc::eval_ISO_DD(
    real_type   s,
    real_type   offs,
    real_type & x_DD,
    real_type & y_DD
  ) const {
    if ( s < S0.length() ) {
      S0.eval_ISO_DD( s, offs, x_DD, y_DD );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.eval_ISO_DD( s, offs, x_DD, y_DD );
      } else {
        s -= SM.length();
        S1.eval_ISO_DD( s, offs, x_DD, y_DD );
      }
    }
  }

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

  void
  G2solve3arc::eval_ISO_DDD(
    real_type   s,
    real_type   offs,
    real_type & x_DDD,
    real_type & y_DDD
  ) const {
    if ( s < S0.length() ) {
      S0.eval_ISO_DDD( s, offs, x_DDD, y_DDD );
    } else {
      s -= S0.length();
      if ( s < SM.length() ) {
        SM.eval_ISO_DDD( s, offs, x_DDD, y_DDD );
      } else {
        s -= SM.length();
        S1.eval_ISO_DDD( s, offs, x_DDD, y_DDD );
      }
    }
  }

  /*\
   |
   |    ___ _     _   _        _    _ ___      _ _           ___ ___
   |   / __| |___| |_| |_  ___(_)__| / __|_ __| (_)_ _  ___ / __|_  )
   |  | (__| / _ \  _| ' \/ _ \ / _` \__ \ '_ \ | | ' \/ -_) (_ |/ /
   |   \___|_\___/\__|_||_\___/_\__,_|___/ .__/_|_|_||_\___|\___/___|
   |                                     |_|
  \*/

  void
  ClothoidSplineG2::guess(
    real_type * theta_guess,
    real_type * theta_min,
    real_type * theta_max
  ) const {
    size_t nn = size_t( m_npts );
    Utils::Malloc<real_type> mem( "ClothoidSplineG2::guess" );
    mem.allocate( 2*nn );
    real_type * omega = mem(nn);
    real_type * len   = mem(nn);
    G2lib::xy_to_guess_angle(
      m_npts, m_x, m_y, theta_guess, theta_min, theta_max, omega, len
    );
  }

  void
  ClothoidSplineG2::build(
    real_type const * xvec,
    real_type const * yvec,
    int_type          n
  ) {
    m_npts = n;
    size_t n1 = size_t(n-1);

    realValues.reallocate( 2*size_t(n) + 10 * n1 );

    m_x    = realValues( size_t(n) );
    m_y    = realValues( size_t(n) );
    m_k    = realValues( n1 );
    m_dk   = realValues( n1 );
    m_L    = realValues( n1 );
    m_kL   = realValues( n1 );
    m_L_1  = realValues( n1 );
    m_L_2  = realValues( n1 );
    m_k_1  = realValues( n1 );
    m_k_2  = realValues( n1 );
    m_dk_1 = realValues( n1 );
    m_dk_2 = realValues( n1 );
    std::copy_n( xvec, n, m_x );
    std::copy_n( yvec, n, m_y );
  }

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

  int_type
  ClothoidSplineG2::numTheta() const { return m_npts; }

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

  int_type
  ClothoidSplineG2::numConstraints() const {
    switch (m_tt) {
      case P1:
      case P2: return m_npts;
      default: break;
    }
    return m_npts-2;
  }

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

  bool
  ClothoidSplineG2::objective(
    real_type const * theta,
    real_type       & f
  ) const {
    ClothoidCurve cL, cR, c;
    int_type ne  = m_npts - 1;
    int_type ne1 = m_npts - 2;
    switch (m_tt) {
    case P1:
    case P2:
      f = 0;
      break;
    case P3:
      // forward target
      break;
    case P4:
      cL.build_G1( m_x[0],   m_y[0],   theta[0],   m_x[1],  m_y[1],  theta[1] );
      cR.build_G1( m_x[ne1], m_y[ne1], theta[ne1], m_x[ne], m_y[ne], theta[ne] );
      { real_type dk_L = cL.dkappa();
        real_type dk_R = cR.dkappa();
        f = dk_L*dk_L+dk_R*dk_R;
      }
      break;
    case P5:
      cL.build_G1( m_x[0],   m_y[0],   theta[0],   m_x[1],  m_y[1],  theta[1] );
      cR.build_G1( m_x[ne1], m_y[ne1], theta[ne1], m_x[ne], m_y[ne], theta[ne] );
      f = cL.length()+cR.length();
      break;
    case P6:
      f = 0;
      for ( int_type j = 0; j < ne; ++j ) {
        c.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
        f += c.length();
      }
      break;
    case P7:
      f = 0;
      for ( int_type j = 0; j < ne; ++j ) {
        c.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
        real_type Len  = c.length();
        real_type kur  = c.kappaBegin();
        real_type dkur = c.dkappa();
        f = f + Len * ( Len * ( dkur*( (dkur*Len)/3 + kur) ) + kur*kur );
      }
      break;
    case P8:
      f = 0;
      for ( int_type j = 0; j < ne; ++j ) {
        c.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
        real_type Len  = c.length();
        real_type dkur = c.dkappa();
        f += Len*dkur*dkur;
      }
      break;
    case P9:
      f = 0;
      for ( int_type j = 0; j < ne; ++j ) {
        c.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
        real_type Len  = c.length();
        real_type kur  = c.kappaBegin();
        real_type k2   = kur*kur;
        real_type k3   = k2*kur;
        real_type k4   = k2*k2;
        real_type dkur = c.dkappa();
        real_type dk2  = dkur*dkur;
        real_type dk3  = dkur*dk2;
        f = f + (k4+dk2+(2*k3*dkur+(2*k2*dk2+(dk3*(kur+dkur*Len/5))*Len)*Len)*Len)*Len;
      }
      break;
    }
    return true;
  }

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

  bool
  ClothoidSplineG2::gradient(
    real_type const * theta,
    real_type       * g
  ) const {
    ClothoidCurve cL, cR, c;
    real_type     LL_D[2], kL_D[2], dkL_D[2];
    real_type     LR_D[2], kR_D[2], dkR_D[2];
    std::fill_n( g, m_npts, 0 );
    int_type ne  = m_npts - 1;
    int_type ne1 = m_npts - 2;
    switch (m_tt) {
    case P1:
    case P2:
      break;
    case P3:
      break;
    case P4:
      cL.build_G1_D(
        m_x[0], m_y[0], theta[0],
        m_x[1], m_y[1], theta[1],
        LL_D, kL_D, dkL_D
      );
      cR.build_G1_D(
        m_x[ne1], m_y[ne1], theta[ne1],
        m_x[ne],  m_y[ne],  theta[ne],
        LR_D, kR_D, dkR_D
      );
      {
        real_type dkL = cL.dkappa();
        real_type dkR = cR.dkappa();
        g[0]   = 2*dkL*dkL_D[0];
        g[1]   = 2*dkL*dkL_D[1];
        g[ne1] = 2*dkR*dkR_D[0];
        g[ne]  = 2*dkR*dkR_D[1];
      }
      break;
    case P5:
      cL.build_G1_D(
        m_x[0], m_y[0], theta[0],
        m_x[1], m_y[1], theta[1],
        LL_D, kL_D, dkL_D
      );
      cR.build_G1_D(
        m_x[ne1], m_y[ne1], theta[ne1],
        m_x[ne],  m_y[ne],  theta[ne],
        LR_D, kR_D, dkR_D
      );
      g[0]   = LL_D[0];
      g[1]   = LL_D[1];
      g[ne1] = LR_D[0];
      g[ne]  = LR_D[1];
      break;
    case P6:
      for ( int_type j = 0; j < ne; ++j ) {
        real_type L_D[2], k_D[2], dk_D[2];
        c.build_G1_D(
          m_x[j],   m_y[j],   theta[j],
          m_x[j+1], m_y[j+1], theta[j+1],
          L_D, k_D, dk_D
        );
        g[j]   += L_D[0];
        g[j+1] += L_D[1];
      }
      break;
    case P7:
      for ( int_type j = 0; j < ne; ++j ) {
        real_type L_D[2], k_D[2], dk_D[2];
        c.build_G1_D(
          m_x[j],   m_y[j],   theta[j],
          m_x[j+1], m_y[j+1], theta[j+1],
          L_D, k_D, dk_D
        );
        real_type Len  = c.length();
        real_type L2   = Len*Len;
        real_type L3   = Len*L2;
        real_type kur  = c.kappaBegin();
        real_type k2   = kur*kur;
        real_type dkur = c.dkappa();
        real_type dk2  = dkur*dkur;
        g[j]   += 2*(dkur*dk_D[0]*L3)/3
                  + (dk2*L2*L_D[0])
                  + dk_D[0]*L2*kur
                  + 2*dkur*Len*L_D[0]*kur
                  + dkur*L2*k_D[0]
                  + L_D[0]*k2
                  + 2*Len*kur*k_D[0];
        g[j+1] += 2*(dkur*dk_D[1]*L3)/3
                  + (dk2*L2*L_D[1])
                  + dk_D[1]*L2*kur
                  + 2*dkur*Len*L_D[1]*kur
                  + dkur*L2*k_D[1]
                  + L_D[1]*k2
                  + 2*Len*kur*k_D[1];
      }
      break;
    case P8:
      for ( int_type j = 0; j < ne; ++j ) {
        real_type L_D[2], k_D[2], dk_D[2];
        c.build_G1_D(
          m_x[j],   m_y[j],   theta[j],
          m_x[j+1], m_y[j+1], theta[j+1],
          L_D, k_D, dk_D
        );
        real_type Len  = c.length();
        real_type dkur = c.dkappa();
        g[j]   += (2*Len*dk_D[0] + L_D[0]*dkur)*dkur;
        g[j+1] += (2*Len*dk_D[1] + L_D[1]*dkur)*dkur;
      }
      break;
    case P9:
      for ( int_type j = 0; j < ne; ++j ) {
        real_type L_D[2], k_D[2], dk_D[2];
        c.build_G1_D(
          m_x[j],   m_y[j],   theta[j],
          m_x[j+1], m_y[j+1], theta[j+1],
          L_D, k_D, dk_D
        );
        real_type Len  = c.length();
        real_type kur  = c.kappaBegin();
        real_type k2   = kur*kur;
        real_type k3   = kur*k2;
        real_type dkur = c.dkappa();
        real_type dk2  = dkur*dkur;
        real_type dkL  = dkur*Len;
        real_type A = ( ( (dkL+4*kur)*dkL + 6*k2)*dkL + 4*k3) * dkL + dk2 + k2*k2;
        real_type B = ( ( ( ( 3*kur + 0.8*dkL ) * dkL + 4*k2 ) * dkL +2*k3 ) * Len + 2*dkur ) * Len;
        real_type C = ( ( ( dkL + 4*kur ) * dkL + 6*k2 ) * dkL + 4*k3 ) * Len;
        g[j]   += A*L_D[0] + B*dk_D[0] + C*k_D[0];
        g[j+1] += A*L_D[1] + B*dk_D[1] + C*k_D[1];
      }
      break;
    }
    return true;
  }

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

  bool
  ClothoidSplineG2::constraints(
    real_type const * theta,
    real_type       * c
  ) const {
    ClothoidCurve cc;
    int_type ne  = m_npts - 1;
    int_type ne1 = m_npts - 2;

    for ( int_type j = 0; j < ne; ++j ) {
      cc.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
      m_k[j]  = cc.kappaBegin();
      m_dk[j] = cc.dkappa();
      m_L[j]  = cc.length();
      m_kL[j] = m_k[j]+m_dk[j]*m_L[j];
    }

    for ( int_type j = 0; j < ne1; ++j ) c[j] = m_kL[j]-m_k[j+1];

    switch (m_tt) {
    case P1:
      c[ne1] = diff2pi( theta[0]  - m_theta_I );
      c[ne]  = diff2pi( theta[ne] - m_theta_F );
      break;
    case P2:
      c[ne1] = m_kL[ne1] - m_k[0];
      c[ne]  = diff2pi( theta[0] - theta[ne] );
      break;
    default:
      break;
    }
    return true;
  }

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

  int_type
  ClothoidSplineG2::jacobian_nnz() const {
    int_type nnz = 3*(m_npts-2);
    switch (m_tt) {
    case P1: nnz += 2; break;
    case P2: nnz += 6; break;
    default:            break;
    }
    return nnz;
  }

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

  bool
  ClothoidSplineG2::jacobian_pattern(
    int_type * ii,
    int_type * jj
  ) const {
    ClothoidCurve cc;
    int_type ne  = m_npts - 1;
    int_type ne1 = m_npts - 2;

    int_type kk = 0;
    for ( int_type j = 0; j < ne1; ++j ) {
      ii[kk] = j; jj[kk] = j; ++kk;
      ii[kk] = j; jj[kk] = j+1; ++kk;
      ii[kk] = j; jj[kk] = j+2; ++kk;
    }

    switch (m_tt) {
    case P1:
      ii[kk] = ne1; jj[kk] = 0; ++kk;
      ii[kk] = ne; jj[kk] = ne; ++kk;
      break;
    case P2:
      ii[kk] = ne1; jj[kk] = 0; ++kk;
      ii[kk] = ne1; jj[kk] = 1; ++kk;
      ii[kk] = ne1; jj[kk] = ne1; ++kk;
      ii[kk] = ne1; jj[kk] = ne; ++kk;
      ii[kk] = ne; jj[kk] = 0; ++kk;
      ii[kk] = ne; jj[kk] = ne; ++kk;
      break;
    default:
      break;
    }

    return true;
  }

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

  bool
  ClothoidSplineG2::jacobian_pattern_matlab(
    real_type * ii,
    real_type * jj
  ) const {
    ClothoidCurve cc;
    int_type ne  = m_npts - 1;
    int_type ne1 = m_npts - 2;

    int_type kk = 0;
    for ( int_type j = 1; j <= ne1; ++j ) {
      ii[kk] = j; jj[kk] = j;   ++kk;
      ii[kk] = j; jj[kk] = j+1; ++kk;
      ii[kk] = j; jj[kk] = j+2; ++kk;
    }

    switch (m_tt) {
    case P1:
      ii[kk] = ne;     jj[kk] = 1;      ++kk;
      ii[kk] = m_npts; jj[kk] = m_npts; ++kk;
      break;
    case P2:
      ii[kk] = ne;     jj[kk] = 1;      ++kk;
      ii[kk] = ne;     jj[kk] = 2;      ++kk;
      ii[kk] = ne;     jj[kk] = ne;     ++kk;
      ii[kk] = ne;     jj[kk] = m_npts; ++kk;
      ii[kk] = m_npts; jj[kk] = 1;      ++kk;
      ii[kk] = m_npts; jj[kk] = m_npts; ++kk;
      break;
    default:
      break;
    }

    return true;
  }

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

  bool
  ClothoidSplineG2::jacobian(
    real_type const * theta,
    real_type       * vals
  ) const {
    ClothoidCurve cc;
    int_type ne  = m_npts - 1;
    int_type ne1 = m_npts - 2;

    for ( int_type j = 0; j < ne; ++j ) {
      real_type L_D[2], k_D[2], dk_D[2];
      cc.build_G1_D(
        m_x[j],   m_y[j],   theta[j],
        m_x[j+1], m_y[j+1], theta[j+1],
        L_D, k_D, dk_D
      );
      m_k[j]    = cc.kappaBegin();
      m_dk[j]   = cc.dkappa();
      m_L[j]    = cc.length();
      m_kL[j]   = m_k[j]+m_dk[j]*m_L[j];
      m_L_1[j]  = L_D[0];  m_L_2[j]  = L_D[1];
      m_k_1[j]  = k_D[0];  m_k_2[j]  = k_D[1];
      m_dk_1[j] = dk_D[0]; m_dk_2[j] = dk_D[1];
    }

    int_type kk = 0;
    for ( int_type j = 0; j < ne1; ++j ) {
      vals[kk++] =  m_k_1[j] + m_dk_1[j]*m_L[j] + m_dk[j]*m_L_1[j];
      vals[kk++] =  m_k_2[j] + m_dk_2[j]*m_L[j] + m_dk[j]*m_L_2[j] - m_k_1[j+1];
      vals[kk++] = -m_k_2[j+1];
    }

    switch (m_tt) {
    case P1:
      vals[kk++] = 1;
      vals[kk++] = 1;
      break;
    case P2:
      vals[kk++] = -m_k_1[0];
      vals[kk++] = -m_k_2[0];
      vals[kk++] = m_k_1[ne1]+m_L_1[ne1]*m_dk[ne1]+m_L[ne1]*m_dk_1[ne1];
      vals[kk++] = m_k_2[ne1]+m_L_2[ne1]*m_dk[ne1]+m_L[ne1]*m_dk_2[ne1];
      vals[kk++] = 1;
      vals[kk++] = -1;
      break;
    default:
      break;
    }
    return true;
  }

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

  ostream_type &
  operator << ( ostream_type & stream, ClothoidSplineG2 const & c ) {
    fmt::print( stream,
      "npts   = {}\n"
      "target = {}\n",
      c.m_npts, int(c.m_tt)
    );
    return stream;
  }

}

// EOF: ClothoidG2.cc
