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

///
/// file: Biarc.cc
///

#include "Clothoids.hh"

namespace G2lib {

  using std::numeric_limits;
  using std::abs;

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

  Biarc::Biarc( BaseCurve const & C )
  : BaseCurve(G2LIB_BIARC)
  {
    switch ( C.type() ) {
    case G2LIB_LINE:
      {
        LineSegment const & LS = *static_cast<LineSegment const *>(&C);
        bool ok = this->build(
          LS.xBegin(), LS.yBegin(), LS.thetaBegin(),
          LS.xEnd(),   LS.yEnd(),   LS.thetaEnd()
        );
        UTILS_ASSERT(
          ok,
          "Biarc constructor failed convert from: {}\n",
          CurveType_name[C.type()]
        );
      }
      break;
    case G2LIB_CIRCLE:
      {
        CircleArc const & LS = *static_cast<CircleArc const *>(&C);
        bool ok = this->build(
          LS.xBegin(), LS.yBegin(), LS.thetaBegin(),
          LS.xEnd(),   LS.yEnd(),   LS.thetaEnd()
        );
        UTILS_ASSERT(
          ok,
          "Biarc constructor failed convert from: {}\n",
          CurveType_name[C.type()]
        );
      }
      break;
    case G2LIB_BIARC:
      *this = *static_cast<Biarc const *>(&C);
      break;
    case G2LIB_CLOTHOID:
    case G2LIB_BIARC_LIST:
    case G2LIB_CLOTHOID_LIST:
    case G2LIB_POLYLINE:
      UTILS_ERROR(
        "Biarc constructor cannot convert from: {}\n",
        CurveType_name[C.type()]
      );
    }
  }

  bool
  Biarc::build(
    real_type x0,
    real_type y0,
    real_type theta0,
    real_type x1,
    real_type y1,
    real_type theta1
  ) {

    real_type dx = x1-x0;
    real_type dy = y1-y0;
    real_type d  = hypot(dy,dx);

    real_type omega = atan2(dy,dx);

    // put in range
    real_type th0 = theta0 - omega;
    real_type th1 = theta1 - omega;

    rangeSymm(th0);
    rangeSymm(th1);

    real_type thstar = - (th0+th1)/2;

    real_type dth  = (th1 - th0)/4;
    real_type dth0 = (thstar - th0)/2;
    real_type dth1 = (thstar - th1)/2;

    real_type t  = 2 * cos( dth ) / d;
    real_type l0 = 1/(t*Sinc( dth0 ));
    real_type l1 = 1/(t*Sinc( dth1 ));
    real_type k0 = 2*t*sin( dth0 );
    real_type k1 = -2*t*sin( dth1 );

    real_type epsi = 100*d*numeric_limits<real_type>::epsilon();
    if ( l0 > epsi && l1 > epsi ) {

      m_C0.build( x0, y0, theta0, k0, l0 );

      real_type an = omega+(thstar+th0)/2;
      real_type xs = x0 + cos(an)/t;
      real_type ys = y0 + sin(an)/t;

      m_C1.build( xs, ys, omega+thstar, k1, l1 );
      return true;
    }
    return false;
  }

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

  bool
  Biarc::build_3P(
    real_type x0,
    real_type y0,
    real_type x1,
    real_type y1,
    real_type x2,
    real_type y2
  ) {

    real_type dxa   = x1-x0;
    real_type dya   = y1-y0;
    real_type dxb   = x2-x1;
    real_type dyb   = y2-y1;
    real_type La    = hypot(dya,dxa);
    real_type Lb    = hypot(dyb,dxb);
    real_type arg   = (dxa*dxb + dya * dyb)/(La*Lb);
    if      ( arg >  1 ) arg = 1;
    else if ( arg < -1 ) arg = -1;
    real_type om = acos(arg);

    real_type at = (La/(La+Lb))*om;
    real_type bt = (Lb/(La+Lb))*om;
    // find solution using Halley
    real_type Delta = 0;
    bool found = false;
    for ( int_type iter = 0; iter < 10 && !found; ++iter ) {
      real_type ga[3], gb[3];
      gfun( at+Delta, ga );
      gfun( bt-Delta, gb );
      real_type f   = ga[0]/La - gb[0]/Lb;
      real_type df  = ga[1]/La + gb[1]/Lb;
      real_type ddf = ga[2]/La - gb[2]/Lb;
      real_type h   = (df*f)/(df*df-0.5*f*ddf);
      Delta -= h;
      found = abs(h) < 1e-10 && abs(f) < 1e-10;
    }

    if ( found ) {
      at += Delta; bt -= Delta;
      real_type tha = atan2(dya,dxa);
      real_type thb = atan2(dyb,dxb);
      if ( dxa*dyb < dya*dxb ) {
        tha += at;
        thb += bt;
      } else {
        tha -= at;
        thb -= bt;
      }
      m_C0.build_G1( x0, y0, tha, x1, y1 );
      m_C1.build_G1( x1, y1, thb, x2, y2 );
    }

    return found;
  }

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

  void
  Biarc::bbox(
    real_type & xmin,
    real_type & ymin,
    real_type & xmax,
    real_type & ymax
  ) const {
    m_C0.bbox( xmin, ymin, xmax, ymax );
    real_type xmi1, ymi1, xma1, yma1;
    m_C1.bbox( xmi1, ymi1, xma1, yma1 );
    if ( xmi1 < xmin ) xmin = xmi1;
    if ( xma1 > xmax ) xmax = xma1;
    if ( ymi1 < ymin ) ymin = ymi1;
    if ( yma1 > ymax ) ymax = yma1;
  }

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

  void
  Biarc::bbox_ISO(
    real_type   offs,
    real_type & xmin,
    real_type & ymin,
    real_type & xmax,
    real_type & ymax
  ) const {
    m_C0.bbox_ISO( offs, xmin, ymin, xmax, ymax );
    real_type xmi1, ymi1, xma1, yma1;
    m_C1.bbox_ISO( offs, xmi1, ymi1, xma1, yma1 );
    if ( xmi1 < xmin ) xmin = xmi1;
    if ( xma1 > xmax ) xmax = xma1;
    if ( ymi1 < ymin ) ymin = ymi1;
    if ( yma1 > ymax ) ymax = yma1;
  }

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

  void
  Biarc::reverse() {
    CircleArc TMP(m_C0);
    m_C0 = m_C1; m_C0.reverse();
    m_C1 = TMP;  m_C1.reverse();
  }

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

  void
  Biarc::scale( real_type scl ) {
    real_type newx0 = m_C0.xBegin() + scl*(m_C1.xBegin()-m_C0.xBegin());
    real_type newy0 = m_C0.yBegin() + scl*(m_C1.yBegin()-m_C0.yBegin());
    m_C1.changeOrigin( newx0, newy0 );
    m_C1.scale( scl );
    m_C0.scale( scl );
  }

  void
  Biarc::changeOrigin( real_type newx0, real_type newy0 ) {
    m_C0.changeOrigin(newx0,newy0);
    m_C1.changeOrigin(m_C0.xEnd(),m_C0.yEnd());
  }

  void
  Biarc::trim( real_type s_begin, real_type s_end ) {
    UTILS_ASSERT(
      s_end > s_begin,
      "Biarc::trim( begin={}, s_end={} ) s_end must be > s_begin\n",
      s_begin, s_end
    );
    real_type L0 = m_C0.length();
    if ( s_end <= L0 ) {
      m_C0.trim( s_begin, s_end );
      m_C1 = m_C0;
      real_type ss = m_C0.length();
      m_C0.trim( 0, ss/2 );
      m_C1.trim( ss/2, ss );
    } else if ( s_begin >= L0 ) {
      m_C1.trim( s_begin-L0, s_end-L0 );
      m_C0 = m_C1;
      real_type ss = m_C0.length();
      m_C0.trim( 0, ss/2 );
      m_C1.trim( ss/2, ss );
    } else {
      m_C0.trim( s_begin, L0 );
      m_C1.trim( 0, s_end-L0 );
    }
  }

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

  real_type
  Biarc::theta( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.theta(s);
    else          return m_C1.theta(s-L0);
  }

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

  real_type
  Biarc::theta_D( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.m_k;
    else          return m_C1.m_k;
  }

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

  real_type
  Biarc::tx( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.tx(s);
    else          return m_C1.tx(s-L0);
  }

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

  real_type
  Biarc::tx_D( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.tx_D(s);
    else          return m_C1.tx_D(s-L0);
  }

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

  real_type
  Biarc::tx_DD( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.tx_DD(s);
    else          return m_C1.tx_DD(s-L0);
  }

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

  real_type
  Biarc::tx_DDD( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.tx_DDD(s);
    else          return m_C1.tx_DDD(s-L0);
  }

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

  real_type
  Biarc::ty( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.ty(s);
    else          return m_C1.ty(s-L0);
  }

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

  real_type
  Biarc::ty_D( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.ty_D(s);
    else          return m_C1.ty_D(s-L0);
  }

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

  real_type
  Biarc::ty_DD( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.ty_DD(s);
    else          return m_C1.ty_DD(s-L0);
  }

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

  real_type
  Biarc::ty_DDD( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.ty_DDD(s);
    else          return m_C1.ty_DDD(s-L0);
  }

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

  real_type
  Biarc::X( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.X(s);
    else          return m_C1.X(s-L0);
  }

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

  real_type
  Biarc::X_D( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.X_D(s);
    else          return m_C1.X_D(s-L0);
  }

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

  real_type
  Biarc::X_DD( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.X_DD(s);
    else          return m_C1.X_DD(s-L0);
  }

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

  real_type
  Biarc::X_DDD( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.X_DDD(s);
    else          return m_C1.X_DDD(s-L0);
  }

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

  real_type
  Biarc::Y( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.Y(s);
    else          return m_C1.Y(s-L0);
  }

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

  real_type
  Biarc::Y_D( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.Y_D(s);
    else          return m_C1.Y_D(s-L0);
  }

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

  real_type
  Biarc::Y_DD( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.Y_DD(s);
    else          return m_C1.Y_DD(s-L0);
  }

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

  real_type
  Biarc::Y_DDD( real_type s ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.Y_DDD(s);
    else          return m_C1.Y_DDD(s-L0);
  }

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

  real_type
  Biarc::X_ISO( real_type s, real_type offs ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.X_ISO(s,offs);
    else          return m_C1.X_ISO(s-L0,offs);
  }

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

  real_type
  Biarc::X_ISO_D( real_type s, real_type offs ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.X_ISO_D(s,offs);
    else          return m_C1.X_ISO_D(s-L0,offs);
  }

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

  real_type
  Biarc::X_ISO_DD( real_type s, real_type offs ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.X_ISO_DD(s,offs);
    else          return m_C1.X_ISO_DD(s-L0,offs);
  }

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

  real_type
  Biarc::X_ISO_DDD( real_type s, real_type offs ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.X_ISO_DDD(s,offs);
    else          return m_C1.X_ISO_DDD(s-L0,offs);
  }

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

  real_type
  Biarc::Y_ISO( real_type s, real_type offs ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.Y_ISO(s,offs);
    else          return m_C1.Y_ISO(s-L0,offs);
  }

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

  real_type
  Biarc::Y_ISO_D( real_type s, real_type offs ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.Y_ISO_D(s,offs);
    else          return m_C1.Y_ISO_D(s-L0,offs);
  }

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

  real_type
  Biarc::Y_ISO_DD( real_type s, real_type offs ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.Y_ISO_DD(s,offs);
    else          return m_C1.Y_ISO_DD(s-L0,offs);
  }

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

  real_type
  Biarc::Y_ISO_DDD( real_type s, real_type offs ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.Y_ISO_DDD(s,offs);
    else          return m_C1.Y_ISO_DDD(s-L0,offs);
  }

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

  void
  Biarc::tg( real_type s, real_type & tx, real_type & ty ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.tg(s,tx,ty);
    else          return m_C1.tg(s-L0,tx,ty);
  }

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

  void
  Biarc::tg_D( real_type s, real_type & tx_D, real_type & ty_D ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.tg_D(s,tx_D,ty_D);
    else          return m_C1.tg_D(s-L0,tx_D,ty_D);
  }

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

  void
  Biarc::tg_DD( real_type s, real_type & tx_DD, real_type & ty_DD ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.tg_DD(s,tx_DD,ty_DD);
    else          return m_C1.tg_DD(s-L0,tx_DD,ty_DD);
  }

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

  void
  Biarc::tg_DDD( real_type s, real_type & tx_DDD, real_type & ty_DDD ) const {
    real_type L0 = m_C0.length();
    if ( s < L0 ) return m_C0.tg_DDD(s,tx_DDD,ty_DDD);
    else          return m_C1.tg_DDD(s-L0,tx_DDD,ty_DDD);
  }

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

  void
  Biarc::evaluate(
    real_type   s,
    real_type & th,
    real_type & k,
    real_type & x,
    real_type & y
  ) const {
    if ( s < m_C0.length() ) {
      th = m_C0.theta(s);
      k  = m_C0.curvature();
      m_C0.eval(s,x,y);
    } else {
      s -= m_C0.length();
      th = m_C1.theta(s);
      k  = m_C1.curvature();
      m_C1.eval(s,x,y);
    }
  }

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

  void
  Biarc::eval(
    real_type   s,
    real_type & x,
    real_type & y
  ) const {
    if ( s < m_C0.length() ) {
      m_C0.eval(s,x,y);
    } else {
      s -= m_C0.length();
      m_C1.eval(s,x,y);
    }
  }

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

  void
  Biarc::eval_D(
    real_type   s,
    real_type & x_D,
    real_type & y_D
  ) const {
    if ( s < m_C0.length() ) {
      m_C0.eval_D(s,x_D,y_D);
    } else {
      s -= m_C0.length();
      m_C1.eval_D(s,x_D,y_D);
    }
  }

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

  void
  Biarc::eval_DD(
    real_type   s,
    real_type & x_DD,
    real_type & y_DD
  ) const {
    if ( s < m_C0.length() ) {
      m_C0.eval_DD(s,x_DD,y_DD);
    } else {
      s -= m_C0.length();
      m_C1.eval_DD(s,x_DD,y_DD);
    }
  }

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

  void
  Biarc::eval_DDD(
    real_type   s,
    real_type & x_DDD,
    real_type & y_DDD
  ) const {
    if ( s < m_C0.length() ) {
      m_C0.eval_DDD(s,x_DDD,y_DDD);
    } else {
      s -= m_C0.length();
      m_C1.eval_DDD(s,x_DDD,y_DDD);
    }
  }

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

  void
  Biarc::eval_ISO(
    real_type   s,
    real_type   offs,
    real_type & x,
    real_type & y
  ) const {
    if ( s < m_C0.length() ) {
      m_C0.eval_ISO(s,offs,x,y);
    } else {
      s -= m_C0.length();
      m_C1.eval_ISO(s,offs,x,y);
    }
  }

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

  void
  Biarc::eval_ISO_D(
    real_type   s,
    real_type   offs,
    real_type & x_D,
    real_type & y_D
  ) const {
    if ( s < m_C0.length() ) {
      m_C0.eval_ISO_D(s,offs,x_D,y_D);
    } else {
      s -= m_C0.length();
      m_C1.eval_ISO_D(s,offs,x_D,y_D);
    }
  }

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

  void
  Biarc::eval_ISO_DD(
    real_type   s,
    real_type   offs,
    real_type & x_DD,
    real_type & y_DD
  ) const {
    if ( s < m_C0.length() ) {
      m_C0.eval_ISO_DD(s,offs,x_DD,y_DD);
    } else {
      s -= m_C0.length();
      m_C1.eval_ISO_DD(s,offs,x_DD,y_DD);
    }
  }

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

  void
  Biarc::eval_ISO_DDD(
    real_type   s,
    real_type   offs,
    real_type & x_DDD,
    real_type & y_DDD
  ) const {
    if ( s < m_C0.length() ) {
      m_C0.eval_ISO_DDD(s,offs,x_DDD,y_DDD);
    } else {
      s -= m_C0.length();
      m_C1.eval_ISO_DDD(s,offs,x_DDD,y_DDD);
    }
  }

  /*\
   |   _       _                          _
   |  (_)_ __ | |_ ___ _ __ ___  ___  ___| |_
   |  | | '_ \| __/ _ \ '__/ __|/ _ \/ __| __|
   |  | | | | | ||  __/ |  \__ \  __/ (__| |_
   |  |_|_| |_|\__\___|_|  |___/\___|\___|\__|
  \*/

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

  void
  Biarc::intersect(
    Biarc const   & B,
    IntersectList & ilist,
    bool            swap_s_vals
  ) const {
    IntersectList ilist00, ilist01, ilist10, ilist11;
    m_C0.intersect( B.m_C0, ilist00, false );
    m_C0.intersect( B.m_C1, ilist01, false );
    m_C1.intersect( B.m_C0, ilist10, false );
    m_C1.intersect( B.m_C1, ilist11, false );
    real_type L  = m_C0.length();
    real_type LB = B.m_C0.length();
    IntersectList::iterator it;
    ilist.reserve( ilist.size() +
                   ilist00.size() +
                   ilist01.size() +
                   ilist10.size() +
                   ilist11.size() );
    for ( it = ilist01.begin(); it != ilist01.end(); ++it ) it->second += LB;
    for ( it = ilist10.begin(); it != ilist10.end(); ++it ) it->first  += L;
    for ( it = ilist11.begin(); it != ilist11.end(); ++it )
      { it->first += L; it->second += LB; }

    if ( swap_s_vals ) {
      for ( it = ilist00.begin(); it != ilist00.end(); ++it )
        ilist.push_back( Ipair(it->second,it->first) );
      for ( it = ilist01.begin(); it != ilist01.end(); ++it )
        ilist.push_back( Ipair(it->second,it->first) );
      for ( it = ilist10.begin(); it != ilist10.end(); ++it )
        ilist.push_back( Ipair(it->second,it->first) );
      for ( it = ilist11.begin(); it != ilist11.end(); ++it )
        ilist.push_back( Ipair(it->second,it->first) );
    } else {
      for ( it = ilist00.begin(); it != ilist00.end(); ++it ) ilist.push_back( *it );
      for ( it = ilist01.begin(); it != ilist01.end(); ++it ) ilist.push_back( *it );
      for ( it = ilist10.begin(); it != ilist10.end(); ++it ) ilist.push_back( *it );
      for ( it = ilist11.begin(); it != ilist11.end(); ++it ) ilist.push_back( *it );
    }
  }

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

  void
  Biarc::intersect_ISO(
    real_type       offs,
    Biarc const   & B,
    real_type       offs_B,
    IntersectList & ilist,
    bool            swap_s_vals
  ) const {
    IntersectList ilist00, ilist01, ilist10, ilist11;
    m_C0.intersect_ISO( offs, B.m_C0, offs_B, ilist00, false );
    m_C0.intersect_ISO( offs, B.m_C1, offs_B, ilist01, false );
    m_C1.intersect_ISO( offs, B.m_C0, offs_B, ilist10, false );
    m_C1.intersect_ISO( offs, B.m_C1, offs_B, ilist11, false );
    real_type L  = m_C0.length();
    real_type LB = B.m_C0.length();
    IntersectList::iterator it;
    ilist.reserve( ilist.size() +
                   ilist00.size() +
                   ilist01.size() +
                   ilist10.size() +
                   ilist11.size() );
    for ( it = ilist01.begin(); it != ilist01.end(); ++it ) it->second += LB;
    for ( it = ilist10.begin(); it != ilist10.end(); ++it ) it->first  += L;
    for ( it = ilist11.begin(); it != ilist11.end(); ++it )
      { it->first += L; it->second += LB; }

    if ( swap_s_vals ) {
      for ( it = ilist00.begin(); it != ilist00.end(); ++it )
        ilist.push_back( Ipair(it->second,it->first) );
      for ( it = ilist01.begin(); it != ilist01.end(); ++it )
        ilist.push_back( Ipair(it->second,it->first) );
      for ( it = ilist10.begin(); it != ilist10.end(); ++it )
        ilist.push_back( Ipair(it->second,it->first) );
      for ( it = ilist11.begin(); it != ilist11.end(); ++it )
        ilist.push_back( Ipair(it->second,it->first) );
    } else {
      for ( it = ilist00.begin(); it != ilist00.end(); ++it ) ilist.push_back( *it );
      for ( it = ilist01.begin(); it != ilist01.end(); ++it ) ilist.push_back( *it );
      for ( it = ilist10.begin(); it != ilist10.end(); ++it ) ilist.push_back( *it );
      for ( it = ilist11.begin(); it != ilist11.end(); ++it ) ilist.push_back( *it );
    }
  }

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

  int_type
  Biarc::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 x1, y1, s1, t1, dst1;
    int_type res  = m_C0.closestPoint_ISO( qx, qy, x,  y,  s,  t,  dst  );
    int_type res1 = m_C1.closestPoint_ISO( qx, qy, x1, y1, s1, t1, dst1 );
    if ( dst1 < dst ) {
      x   = x1;
      y   = y1;
      s   = s1;
      t   = t1;
      dst = dst1;
      res = res1;
    }
    return res;
  }

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

  int_type
  Biarc::closestPoint_ISO(
    real_type   qx,
    real_type   qy,
    real_type   offs,
    real_type & x,
    real_type & y,
    real_type & s,
    real_type & t,
    real_type & dst
  ) const {
    real_type x1, y1, s1, t1, dst1;
    int_type res  = m_C0.closestPoint_ISO( qx, qy, offs, x,  y,  s,  t,  dst  );
    int_type res1 = m_C1.closestPoint_ISO( qx, qy, offs, x1, y1, s1, t1, dst1 );
    if ( dst1 < dst ) {
      x   = x1;
      y   = y1;
      s   = s1;
      t   = t1;
      dst = dst1;
      res = res1;
    }
    return res;
  }

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

  bool
  build_guess_theta(
    int_type          n,
    real_type const * x,
    real_type const * y,
    real_type       * theta
  ) {
    UTILS_ASSERT0(
      n > 1, "build_guess_theta, at least 2 points are necessary\n"
    );
    Biarc b;
    if ( n == 2 ) {
      theta[0] = theta[1] = atan2( y[1] - y[0], x[1] - x[0] );
    } else {
      bool ok, ciclic = hypot( x[0]-x[n-1], y[0]-y[n-1] ) < 1e-10;
      if ( ciclic ) {
        ok = b.build_3P( x[n-2], y[n-2], x[0], y[0], x[1], y[1] );
        UTILS_ASSERT0( ok, "build_guess_theta, failed\n" );
        theta[0] = theta[n-1] = b.thetaMiddle();
      }
      for ( int_type k = 1; k < n-1; ++k ) {
        ok = b.build_3P( x[k-1], y[k-1], x[k], y[k], x[k+1], y[k+1] );
        UTILS_ASSERT0( ok, "build_guess_theta, failed\n" );
        theta[k] = b.thetaMiddle();
        if ( k == 1   && !ciclic ) theta[0]   = b.thetaBegin();
        if ( k == n-2 && !ciclic ) theta[n-1] = b.thetaEnd();
      }
    }
    return true;
  }

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

  ostream_type &
  operator << ( ostream_type & stream, Biarc const & bi ) {
    stream
      << "C0\n" << bi.m_C0
      << "C1\n" << bi.m_C1
      << "\n";
    return stream;
  }

}

///
/// eof: Biarc.cc
///
