/*****
 * path.cc
 * Andy Hammerlindl 2002/06/06
 *
 * Stores and returns information on a predefined path.
 *
 * When changing the path algorithms, also update the corresponding
 * three-dimensional algorithms in path3.cc.
 *****/

#include "path.h"
#include "util.h"
#include "angle.h"
#include "camperror.h"
#include "mathop.h"
#include "predicates.h"
#include "rounding.h"

namespace camp {

const double Fuzz2=1000.0*DBL_EPSILON;
const double Fuzz=sqrt(Fuzz2);
const double Fuzz4=Fuzz2*Fuzz2;
const double BigFuzz=10.0*Fuzz2;
const double fuzzFactor=100.0;

const double third=1.0/3.0;

path nullpath;

const char *nopoints="nullpath has no points";

void checkEmpty(Int n) {
  if(n == 0)
    reportError(nopoints);
}

// Accurate computation of sqrt(1+x)-1.
inline double sqrt1pxm1(double x)
{
  return x/(sqrt(1.0+x)+1.0);
}
inline pair sqrt1pxm1(pair x)
{
  return x/(Sqrt(1.0+x)+1.0);
}

// Solve for the real roots of the quadratic equation ax^2+bx+c=0.
quadraticroots::quadraticroots(double a, double b, double c)
{
  // Remove roots at numerical infinity.
  if(fabs(a) <= Fuzz2*fabs(b)+Fuzz4*fabs(c)) {
    if(fabs(b) > Fuzz2*fabs(c)) {
      distinct=quadraticroots::ONE;
      roots=1;
      t1=-c/b;
    } else if(c == 0.0) {
      distinct=quadraticroots::MANY;
      roots=1;
      t1=0.0;
    } else {
      distinct=quadraticroots::NONE;
      roots=0;
    }
  } else {
    double factor=0.5*b/a;
    double denom=b*factor;
    if(fabs(denom) <= Fuzz2*fabs(c)) {
      double x=-c/a;
      if(x >= 0.0) {
        distinct=quadraticroots::TWO;
        roots=2;
        t2=sqrt(x);
        t1=-t2;
      } else {
        distinct=quadraticroots::NONE;
        roots=0;
      }
    } else {
      double x=-2.0*c/denom;
      if(x > -1.0) {
        distinct=quadraticroots::TWO;
        roots=2;
        double r2=factor*sqrt1pxm1(x);
        double r1=-r2-2.0*factor;
        if(r1 <= r2) {
          t1=r1;
          t2=r2;
        } else {
          t1=r2;
          t2=r1;
        }
      } else if(x == -1.0) {
        distinct=quadraticroots::ONE;
        roots=2;
        t1=t2=-factor;
      } else {
        distinct=quadraticroots::NONE;
        roots=0;
      }
    }
  }
}

// Solve for the complex roots of the quadratic equation ax^2+bx+c=0.
Quadraticroots::Quadraticroots(pair a, pair b, pair c)
{
  if(a == 0.0) {
    if(b != 0.0) {
      roots=1;
      z1=-c/b;
    } else if(c == 0.0) {
      roots=1;
      z1=0.0;
    } else
      roots=0;
  } else {
    roots=2;
    pair factor=0.5*b/a;
    pair denom=b*factor;
    if(denom == 0.0) {
      z1=Sqrt(-c/a);
      z2=-z1;
    } else {
      z1=factor*sqrt1pxm1(-2.0*c/denom);
      z2=-z1-2.0*factor;
    }
  }
}

inline bool goodroot(double a, double b, double c, double t)
{
  return goodroot(t) && quadratic(a,b,c,t) >= 0.0;
}

// Accurate computation of cbrt(sqrt(1+x)+1)-cbrt(sqrt(1+x)-1).
inline double cbrtsqrt1pxm(double x)
{
  double s=sqrt1pxm1(x);
  return 2.0/(cbrt(x+2.0*(sqrt(1.0+x)+1.0))+cbrt(x)+cbrt(s*s));
}

// Taylor series of cos((atan(1.0/w)+pi)/3.0).
static inline double costhetapi3(double w)
{
  static const double c1=1.0/3.0;
  static const double c3=-19.0/162.0;
  static const double c5=425.0/5832.0;
  static const double c7=-16829.0/314928.0;
  double w2=w*w;
  double w3=w2*w;
  double w5=w3*w2;
  return c1*w+c3*w3+c5*w5+c7*w5*w2;
}

// Solve for the real roots of the cubic equation ax^3+bx^2+cx+d=0.
cubicroots::cubicroots(double a, double b, double c, double d)
{
  static const double ninth=1.0/9.0;
  static const double fiftyfourth=1.0/54.0;

  // Remove roots at numerical infinity.
  if(fabs(a) <= Fuzz2*(fabs(b)+fabs(c)*Fuzz2+fabs(d)*Fuzz4)) {
    quadraticroots q(b,c,d);
    roots=q.roots;
    if(q.roots >= 1) t1=q.t1;
    if(q.roots == 2) t2=q.t2;
    return;
  }

  // Detect roots at numerical zero.
  if(fabs(d) <= Fuzz2*(fabs(c)+fabs(b)*Fuzz2+fabs(a)*Fuzz4)) {
    quadraticroots q(a,b,c);
    roots=q.roots+1;
    t1=0;
    if(q.roots >= 1) t2=q.t1;
    if(q.roots == 2) t3=q.t2;
    return;
  }

  b /= a;
  c /= a;
  d /= a;

  double b2=b*b;
  double Q=3.0*c-b2;
  if(fabs(Q) < Fuzz2*(3.0*fabs(c)+fabs(b2)))
    Q=0.0;

  double R=(3.0*Q+b2)*b-27.0*d;
  if(fabs(R) < Fuzz2*((3.0*fabs(Q)+fabs(b2))*fabs(b)+27.0*fabs(d)))
    R=0.0;

  Q *= ninth;
  R *= fiftyfourth;

  double Q3=Q*Q*Q;
  double R2=R*R;
  double D=Q3+R2;
  double mthirdb=-b*third;

  if(D > 0.0) {
    roots=1;
    t1=mthirdb;
    if(R2 != 0.0) t1 += cbrt(R)*cbrtsqrt1pxm(Q3/R2);
  } else {
    roots=3;
    double v=0.0,theta;
    if(R2 > 0.0) {
      v=sqrt(-D/R2);
      theta=atan(v);
    } else theta=0.5*PI;
    double factor=2.0*sqrt(-Q)*(R >= 0 ? 1 : -1);

    t1=mthirdb+factor*cos(third*theta);
    t2=mthirdb-factor*cos(third*(theta-PI));
    t3=mthirdb;
    if(R2 > 0.0)
      t3 -= factor*((v < 100.0) ? cos(third*(theta+PI)) : costhetapi3(1.0/v));
  }
}

pair path::point(double t) const
{
  checkEmpty(n);

  Int i = Floor(t);
  Int iplus;
  t = fmod(t,1);
  if (t < 0) t += 1;

  if (cycles) {
    i = imod(i,n);
    iplus = imod(i+1,n);
  }
  else if (i < 0)
    return nodes[0].point;
  else if (i >= n-1)
    return nodes[n-1].point;
  else
    iplus = i+1;

  double one_t = 1.0-t;

  pair a = nodes[i].point,
    b = nodes[i].post,
    c = nodes[iplus].pre,
    d = nodes[iplus].point,
    ab   = one_t*a   + t*b,
    bc   = one_t*b   + t*c,
    cd   = one_t*c   + t*d,
    abc  = one_t*ab  + t*bc,
    bcd  = one_t*bc  + t*cd,
    abcd = one_t*abc + t*bcd;

  return abcd;
}

pair path::precontrol(double t) const
{
  checkEmpty(n);

  Int i = Floor(t);
  Int iplus;
  t = fmod(t,1);
  if (t < 0) t += 1;

  if (cycles) {
    i = imod(i,n);
    iplus = imod(i+1,n);
  }
  else if (i < 0)
    return nodes[0].pre;
  else if (i >= n-1)
    return nodes[n-1].pre;
  else
    iplus = i+1;

  double one_t = 1.0-t;

  pair a = nodes[i].point,
    b = nodes[i].post,
    c = nodes[iplus].pre,
    ab   = one_t*a   + t*b,
    bc   = one_t*b   + t*c,
    abc  = one_t*ab  + t*bc;

  return (abc == a) ? nodes[i].pre : abc;
}


pair path::postcontrol(double t) const
{
  checkEmpty(n);

  Int i = Floor(t);
  Int iplus;
  t = fmod(t,1);
  if (t < 0) t += 1;

  if (cycles) {
    i = imod(i,n);
    iplus = imod(i+1,n);
  }
  else if (i < 0)
    return nodes[0].post;
  else if (i >= n-1)
    return nodes[n-1].post;
  else
    iplus = i+1;

  double one_t = 1.0-t;

  pair b = nodes[i].post,
    c = nodes[iplus].pre,
    d = nodes[iplus].point,
    bc   = one_t*b   + t*c,
    cd   = one_t*c   + t*d,
    bcd  = one_t*bc  + t*cd;

  return (bcd == d) ? nodes[iplus].post : bcd;
}

path path::reverse() const
{
  mem::vector<solvedKnot> nodes(n);
  Int len=length();
  for (Int i = 0, j = len; i < n; i++, j--) {
    nodes[i].pre = postcontrol(j);
    nodes[i].point = point(j);
    nodes[i].post = precontrol(j);
    nodes[i].straight = straight(j-1);
  }
  return path(nodes, n, cycles);
}

path path::subpath(Int a, Int b) const
{
  if(empty()) return path();

  if (a > b) {
    const path &rp = reverse();
    Int len=length();
    path result = rp.subpath(len-a, len-b);
    return result;
  }

  if (!cycles) {
    if (a < 0) {
      a = 0;
      if(b < 0)
        b = 0;
    }
    if (b > n-1) {
      b = n-1;
      if(a > b)
        a = b;
    }
  }

  Int sn = b-a+1;
  mem::vector<solvedKnot> nodes(sn);

  for (Int i = 0, j = a; j <= b; i++, j++) {
    nodes[i].pre = precontrol(j);
    nodes[i].point = point(j);
    nodes[i].post = postcontrol(j);
    nodes[i].straight = straight(j);
  }
  nodes[0].pre = nodes[0].point;
  nodes[sn-1].post = nodes[sn-1].point;

  return path(nodes, sn);
}

inline pair split(double t, const pair& x, const pair& y) { return x+(y-x)*t; }

inline void splitCubic(solvedKnot sn[], double t, const solvedKnot& left_,
                       const solvedKnot& right_)
{
  solvedKnot &left=(sn[0]=left_), &mid=sn[1], &right=(sn[2]=right_);
  if(left.straight) {
    mid.point=split(t,left.point,right.point);
    pair deltaL=third*(mid.point-left.point);
    left.post=left.point+deltaL;
    mid.pre=mid.point-deltaL;
    pair deltaR=third*(right.point-mid.point);
    mid.post=mid.point+deltaR;
    right.pre=right.point-deltaR;
    mid.straight=true;
  } else {
    pair x=split(t,left.post,right.pre); // m1
    left.post=split(t,left.point,left.post); // m0
    right.pre=split(t,right.pre,right.point); // m2
    mid.pre=split(t,left.post,x); // m3
    mid.post=split(t,x,right.pre); // m4
    mid.point=split(t,mid.pre,mid.post); // m5
  }
}

path path::subpath(double a, double b) const
{
  if(empty()) return path();

  if (a > b) {
    const path &rp = reverse();
    Int len=length();
    return rp.subpath(len-a, len-b);
  }

  solvedKnot aL, aR, bL, bR;
  if (!cycles) {
    if (a < 0) {
      a = 0;
      if (b < 0)
        b = 0;
    }
    if (b > n-1) {
      b = n-1;
      if (a > b)
        a = b;
    }
    aL = nodes[(Int)floor(a)];
    aR = nodes[(Int)ceil(a)];
    bL = nodes[(Int)floor(b)];
    bR = nodes[(Int)ceil(b)];
  } else {
    if(run::validInt(a) && run::validInt(b)) {
      aL = nodes[imod((Int) floor(a),n)];
      aR = nodes[imod((Int) ceil(a),n)];
      bL = nodes[imod((Int) floor(b),n)];
      bR = nodes[imod((Int) ceil(b),n)];
    } else reportError("invalid path index");
  }

  if (a == b) return path(point(a));

  solvedKnot sn[3];
  path p = subpath(Ceil(a), Floor(b));
  if (a > floor(a)) {
    if (b < ceil(a)) {
      splitCubic(sn,a-floor(a),aL,aR);
      splitCubic(sn,(b-a)/(ceil(b)-a),sn[1],sn[2]);
      return path(sn[0],sn[1]);
    }
    splitCubic(sn,a-floor(a),aL,aR);
    p=concat(path(sn[1],sn[2]),p);
  }
  if (ceil(b) > b) {
    splitCubic(sn,b-floor(b),bL,bR);
    p=concat(p,path(sn[0],sn[1]));
  }
  return p;
}

// Special case of subpath for paths of length 1 used by intersect.
void path::halve(path &first, path &second) const
{
  solvedKnot sn[3];
  splitCubic(sn,0.5,nodes[0],nodes[1]);
  first=path(sn[0],sn[1]);
  second=path(sn[1],sn[2]);
}

// Calculate the coefficients of a Bezier derivative divided by 3.
static inline void derivative(pair& a, pair& b, pair& c,
                              const pair& z0, const pair& c0,
                              const pair& c1, const pair& z1)
{
  a=z1-z0+3.0*(c0-c1);
  b=2.0*(z0+c1)-4.0*c0;
  c=c0-z0;
}

bbox path::bounds() const
{
  if(!box.empty) return box;

  if (empty()) {
    // No bounds
    return bbox();
  }

  Int len=length();
  box.add(point(len));
  times=bbox(len,len,len,len);

  for (Int i = 0; i < len; i++) {
    addpoint(box,i);
    if(straight(i)) continue;

    pair a,b,c;
    derivative(a,b,c,point(i),postcontrol(i),precontrol(i+1),point(i+1));

    // Check x coordinate
    quadraticroots x(a.getx(),b.getx(),c.getx());
    if(x.distinct != quadraticroots::NONE && goodroot(x.t1))
      addpoint(box,i+x.t1);
    if(x.distinct == quadraticroots::TWO && goodroot(x.t2))
      addpoint(box,i+x.t2);

    // Check y coordinate
    quadraticroots y(a.gety(),b.gety(),c.gety());
    if(y.distinct != quadraticroots::NONE && goodroot(y.t1))
      addpoint(box,i+y.t1);
    if(y.distinct == quadraticroots::TWO && goodroot(y.t2))
      addpoint(box,i+y.t2);
  }
  return box;
}

bbox path::bounds(double min, double max) const
{
  bbox box;

  Int len=length();
  for (Int i = 0; i < len; i++) {
    addpoint(box,i,min,max);
    if(straight(i)) continue;

    pair a,b,c;
    derivative(a,b,c,point(i),postcontrol(i),precontrol(i+1),point(i+1));

    // Check x coordinate
    quadraticroots x(a.getx(),b.getx(),c.getx());
    if(x.distinct != quadraticroots::NONE && goodroot(x.t1))
      addpoint(box,i+x.t1,min,max);

    if(x.distinct == quadraticroots::TWO && goodroot(x.t2))
      addpoint(box,i+x.t2,min,max);

    // Check y coordinate
    quadraticroots y(a.gety(),b.gety(),c.gety());
    if(y.distinct != quadraticroots::NONE && goodroot(y.t1))
      addpoint(box,i+y.t1,min,max);
    if(y.distinct == quadraticroots::TWO && goodroot(y.t2))
      addpoint(box,i+y.t2,min,max);
  }
  addpoint(box,len,min,max);
  return box;
}

inline void add(bbox& box, const pair& z, const pair& min, const pair& max)
{
  box += z+min;
  box += z+max;
}

bbox path::internalbounds(const bbox& padding) const
{
  bbox box;

  // Check interior nodes.
  Int len=length();

  // Check interior segments.
  for (Int i = 0; i < len; i++) {
    if(straight(i)) continue;

    pair a,b,c;
    derivative(a,b,c,point(i),postcontrol(i),precontrol(i+1),point(i+1));

    // Check x coordinate
    quadraticroots x(a.getx(),b.getx(),c.getx());
    if(x.distinct != quadraticroots::NONE && goodroot(x.t1))
      add(box,point(i+x.t1),padding.left,padding.right);
    if(x.distinct == quadraticroots::TWO && goodroot(x.t2))
      add(box,point(i+x.t2),padding.left,padding.right);

    // Check y coordinate
    quadraticroots y(a.gety(),b.gety(),c.gety());
    if(y.distinct != quadraticroots::NONE && goodroot(y.t1))
      add(box,point(i+y.t1),pair(0,padding.bottom),pair(0,padding.top));
    if(y.distinct == quadraticroots::TWO && goodroot(y.t2))
      add(box,point(i+y.t2),pair(0,padding.bottom),pair(0,padding.top));
  }
  return box;
}

// {{{ Arclength Calculations

static pair a,b,c;

static double ds(double t)
{
  double dx=quadratic(a.getx(),b.getx(),c.getx(),t);
  double dy=quadratic(a.gety(),b.gety(),c.gety(),t);
  return sqrt(dx*dx+dy*dy);
}

// Calculates arclength of a cubic Bezier curve using adaptive Simpson
// integration.
double arcLength(const pair& z0, const pair& c0, const pair& c1,
                 const pair& z1)
{
  double integral;
  derivative(a,b,c,z0,c0,c1,z1);

  if(!simpson(integral,ds,0.0,1.0,DBL_EPSILON,1.0))
    reportError("nesting capacity exceeded in computing arclength");
  return integral;
}

double path::cubiclength(Int i, double goal) const
{
  const pair& z0=point(i);
  const pair& z1=point(i+1);
  double L;
  if(straight(i)) {
    L=(z1-z0).length();
    return (goal < 0 || goal >= L) ? L : -goal/L;
  }

  double integral=arcLength(z0,postcontrol(i),precontrol(i+1),z1);

  L=3.0*integral;
  if(goal < 0 || goal >= L) return L;

  double t=goal/L;
  goal *= third;
  static double dxmin=sqrt(DBL_EPSILON);
  if(!unsimpson(goal,ds,0.0,t,100.0*DBL_EPSILON,integral,1.0,dxmin))
    reportError("nesting capacity exceeded in computing arctime");
  return -t;
}

double path::arclength() const
{
  if (cached_length != -1) return cached_length;

  double L=0.0;
  for (Int i = 0; i < n-1; i++) {
    L += cubiclength(i);
  }
  if(cycles) L += cubiclength(n-1);
  cached_length = L;
  return cached_length;
}

double path::arctime(double goal) const
{
  if (cycles) {
    if (goal == 0 || cached_length == 0) return 0;
    if (goal < 0)  {
      const path &rp = this->reverse();
      double result = -rp.arctime(-goal);
      return result;
    }
    if (cached_length > 0 && goal >= cached_length) {
      Int loops = (Int)(goal / cached_length);
      goal -= loops*cached_length;
      return loops*n+arctime(goal);
    }
  } else {
    if (goal <= 0)
      return 0;
    if (cached_length > 0 && goal >= cached_length)
      return n-1;
  }

  double l,L=0;
  for (Int i = 0; i < n-1; i++) {
    l = cubiclength(i,goal);
    if (l < 0)
      return (-l+i);
    else {
      L += l;
      goal -= l;
      if (goal <= 0)
        return i+1;
    }
  }
  if (cycles) {
    l = cubiclength(n-1,goal);
    if (l < 0)
      return -l+n-1;
    if (cached_length > 0 && cached_length != L+l) {
      reportError("arclength != length.\n"
                  "path::arclength(double) must have broken semantics.\n"
                  "Please report this error.");
    }
    cached_length = L += l;
    goal -= l;
    return arctime(goal)+n;
  }
  else {
    cached_length = L;
    return length();
  }
}

// }}}

// {{{ Direction Time Calulation
// Algorithm Stolen from Knuth's MetaFont
inline double cubicDir(const solvedKnot& left, const solvedKnot& right,
                       const pair& rot)
{
  pair a,b,c;
  derivative(a,b,c,left.point,left.post,right.pre,right.point);
  a *= rot; b *= rot; c *= rot;

  quadraticroots ret(a.gety(),b.gety(),c.gety());
  switch(ret.distinct) {
    case quadraticroots::MANY:
    case quadraticroots::ONE:
    {
      if(goodroot(a.getx(),b.getx(),c.getx(),ret.t1)) return ret.t1;
    } break;

    case quadraticroots::TWO:
    {
      if(goodroot(a.getx(),b.getx(),c.getx(),ret.t1)) return ret.t1;
      if(goodroot(a.getx(),b.getx(),c.getx(),ret.t2)) return ret.t2;
    } break;

    case quadraticroots::NONE:
      break;
  }

  return -1;
}

// TODO: Check that we handle corner cases.
// Velocity(t) == (0,0)
double path::directiontime(const pair& dir) const {
  if (dir == pair(0,0)) return 0;
  pair rot = pair(1,0)/unit(dir);

  double t; double pre,post;
  for (Int i = 0; i < n-1+cycles; ) {
    t = cubicDir(this->nodes[i],(cycles && i==n-1) ? nodes[0]:nodes[i+1],rot);
    if (t >= 0) return i+t;
    i++;
    if (cycles || i != n-1) {
      pair Pre = (point(i)-precontrol(i))*rot;
      pair Post = (postcontrol(i)-point(i))*rot;
      static pair zero(0.0,0.0);
      if(Pre != zero && Post != zero) {
        pre = angle(Pre);
        post = angle(Post);
        if ((pre <= 0 && post >= 0 && pre >= post - PI) ||
            (pre >= 0 && post <= 0 && pre <= post + PI))
          return i;
      }
    }
  }

  return -1;
}
// }}}

// {{{ Path Intersection Calculations

const unsigned maxdepth=DBL_MANT_DIG;
const unsigned mindepth=maxdepth-16;

void roots(std::vector<double> &roots, double a, double b, double c, double d)
{
  cubicroots r(a,b,c,d);
  if(r.roots >= 1) roots.push_back(r.t1);
  if(r.roots >= 2) roots.push_back(r.t2);
  if(r.roots == 3) roots.push_back(r.t3);
}

void roots(std::vector<double> &r, double x0, double c0, double c1, double x1,
           double x)
{
  double a=x1-x0+3.0*(c0-c1);
  double b=3.0*(x0+c1)-6.0*c0;
  double c=3.0*(c0-x0);
  double d=x0-x;
  roots(r,a,b,c,d);
}

// Return all intersection times of path g with the pair z.
void intersections(std::vector<double>& T, const path& g, const pair& z,
                   double fuzz)
{
  double fuzz2=fuzz*fuzz;
  Int n=g.length();
  bool cycles=g.cyclic();
  for(Int i=0; i < n; ++i) {
    // Check both directions to circumvent degeneracy.
    std::vector<double> r;
    roots(r,g.point(i).getx(),g.postcontrol(i).getx(),
          g.precontrol(i+1).getx(),g.point(i+1).getx(),z.getx());
    roots(r,g.point(i).gety(),g.postcontrol(i).gety(),
          g.precontrol(i+1).gety(),g.point(i+1).gety(),z.gety());

    size_t m=r.size();
    for(size_t j=0 ; j < m; ++j) {
      double t=r[j];
      if(t >= -Fuzz2 && t <= 1.0+Fuzz2) {
        double s=i+t;
        if((g.point(s)-z).abs2() <= fuzz2) {
          if(cycles && s >= n-Fuzz2) s=0;
          T.push_back(s);
        }
      }
    }
  }
}

inline bool online(const pair&p, const pair& q, const pair& z, double fuzz)
{
  if(p == q) return (z-p).abs2() <= fuzz*fuzz;
  return (z.getx()-p.getx())*(q.gety()-p.gety()) ==
    (q.getx()-p.getx())*(z.gety()-p.gety());
}

// Return all intersection times of path g with the (infinite)
// line through p and q; if there are an infinite number of intersection points,
// the returned list is guaranteed to include the endpoint times of
// the intersection if endpoints=true.
void lineintersections(std::vector<double>& T, const path& g,
                       const pair& p, const pair& q, double fuzz,
                       bool endpoints=false)
{
  Int n=g.length();
  if(n == 0) {
    if(online(p,q,g.point((Int) 0),fuzz)) T.push_back(0.0);
    return;
  }
  bool cycles=g.cyclic();
  double dx=q.getx()-p.getx();
  double dy=q.gety()-p.gety();
  double det=p.gety()*q.getx()-p.getx()*q.gety();
  for(Int i=0; i < n; ++i) {
    pair z0=g.point(i);
    pair c0=g.postcontrol(i);
    pair c1=g.precontrol(i+1);
    pair z1=g.point(i+1);
    pair t3=z1-z0+3.0*(c0-c1);
    pair t2=3.0*(z0+c1)-6.0*c0;
    pair t1=3.0*(c0-z0);
    double a=dy*t3.getx()-dx*t3.gety();
    double b=dy*t2.getx()-dx*t2.gety();
    double c=dy*t1.getx()-dx*t1.gety();
    double d=dy*z0.getx()-dx*z0.gety()+det;
    std::vector<double> r;
    if(max(max(max(a*a,b*b),c*c),d*d) >
       Fuzz4*max(max(max(z0.abs2(),z1.abs2()),c0.abs2()),c1.abs2()))
      roots(r,a,b,c,d);
    else r.push_back(0.0);
    if(endpoints) {
      path h=g.subpath(i,i+1);
      intersections(r,h,p,fuzz);
      intersections(r,h,q,fuzz);
      if(online(p,q,z0,fuzz)) r.push_back(0.0);
      if(online(p,q,z1,fuzz)) r.push_back(1.0);
    }
    size_t m=r.size();
    for(size_t j=0 ; j < m; ++j) {
      double t=r[j];
      if(t >= -Fuzz2 && t <= 1.0+Fuzz2) {
        double s=i+t;
        if(cycles && s >= n-Fuzz2) s=0;
        T.push_back(s);
      }
    }
  }
}

// An optimized implementation of intersections(g,p--q);
// if there are an infinite number of intersection points, the returned list is
// only guaranteed to include the endpoint times of the intersection.
void intersections(std::vector<double>& S, std::vector<double>& T,
                   const path& g, const pair& p, const pair& q, double fuzz)
{
  double length2=(q-p).abs2();
  if(length2 == 0.0) {
    std::vector<double> S1;
    intersections(S1,g,p,fuzz);
    size_t n=S1.size();
    for(size_t i=0; i < n; ++i) {
      S.push_back(S1[i]);
      T.push_back(0.0);
    }
  } else {
    pair factor=(q-p)/length2;
    std::vector<double> S1;
    lineintersections(S1,g,p,q,fuzz,true);
    size_t n=S1.size();
    for(size_t i=0; i < n; ++i) {
      double s=S1[i];
      double t=dot(g.point(s)-p,factor);
      if(t >= -Fuzz2 && t <= 1.0+Fuzz2) {
        S.push_back(s);
        T.push_back(t);
      }
    }
  }
}

void add(std::vector<double>& S, double s, const path& p, double fuzz2)
{
  pair z=p.point(s);
  size_t n=S.size();
  for(size_t i=0; i < n; ++i)
    if((p.point(S[i])-z).abs2() <= fuzz2) return;
  S.push_back(s);
}

void add(std::vector<double>& S, std::vector<double>& T, double s, double t,
         const path& p, double fuzz2)
{
  pair z=p.point(s);
  size_t n=S.size();
  for(size_t i=0; i < n; ++i)
    if((p.point(S[i])-z).abs2() <= fuzz2) return;
  S.push_back(s);
  T.push_back(t);
}

void add(double& s, double& t, std::vector<double>& S, std::vector<double>& T,
         std::vector<double>& S1, std::vector<double>& T1,
         double pscale, double qscale, double poffset, double qoffset,
         const path& p, double fuzz2, bool single)
{
  if(single) {
    s=s*pscale+poffset;
    t=t*qscale+qoffset;
  } else {
    size_t n=S1.size();
    for(size_t i=0; i < n; ++i)
      add(S,T,pscale*S1[i]+poffset,qscale*T1[i]+qoffset,p,fuzz2);
  }
}

void add(double& s, double& t, std::vector<double>& S, std::vector<double>& T,
         std::vector<double>& S1, std::vector<double>& T1,
         const path& p, double fuzz2, bool single)
{
  size_t n=S1.size();
  if(single) {
    if(n > 0) {
      s=S1[0];
      t=T1[0];
    }
  } else {
    for(size_t i=0; i < n; ++i)
      add(S,T,S1[i],T1[i],p,fuzz2);
  }
}

void intersections(std::vector<double>& S, path& g,
                   const pair& p, const pair& q, double fuzz)
{
  double fuzz2=max(fuzzFactor*fuzz*fuzz,Fuzz2);
  std::vector<double> S1;
  lineintersections(S1,g,p,q,fuzz);
  size_t n=S1.size();
  for(size_t i=0; i < n; ++i)
    add(S,S1[i],g,fuzz2);
}

bool intersections(double &s, double &t, std::vector<double>& S,
                   std::vector<double>& T, path& p, path& q,
                   double fuzz, bool single, bool exact, unsigned depth)
{
  if(errorstream::interrupt) throw interrupted();

  double fuzz2=max(fuzzFactor*fuzz*fuzz,Fuzz2);

  Int lp=p.length();
  if(((lp == 1 && p.straight(0)) || lp == 0) && exact) {
    std::vector<double> T1,S1;
    intersections(T1,S1,q,p.point((Int) 0),p.point(lp),fuzz);
    add(s,t,S,T,S1,T1,p,fuzz2,single);
    return S1.size() > 0;
  }

  Int lq=q.length();
  if(((lq == 1 && q.straight(0)) || lq == 0) && exact) {
    std::vector<double> S1,T1;
    intersections(S1,T1,p,q.point((Int) 0),q.point(lq),fuzz);
    add(s,t,S,T,S1,T1,p,fuzz2,single);
    return S1.size() > 0;
  }

  pair maxp=p.max();
  pair minp=p.min();
  pair maxq=q.max();
  pair minq=q.min();

  if(maxp.getx()+fuzz >= minq.getx() &&
     maxp.gety()+fuzz >= minq.gety() &&
     maxq.getx()+fuzz >= minp.getx() &&
     maxq.gety()+fuzz >= minp.gety()) {
    // Overlapping bounding boxes

    --depth;
//    fuzz *= 2;
//    fuzz2=max(fuzzFactor*fuzz*fuzz,Fuzz2);

    if((maxp-minp).length()+(maxq-minq).length() <= fuzz || depth == 0) {
      if(single) {
        s=0.5;
        t=0.5;
      } else {
        S.push_back(0.5);
        T.push_back(0.5);
      }
      return true;
    }

    path p1,p2;
    double pscale,poffset;
    std::vector<double> S1,T1;

    if(lp <= 1) {
      if(lp == 1) p.halve(p1,p2);
      if(lp == 0 || p1 == p || p2 == p) {
        intersections(T1,S1,q,p.point((Int) 0),p.point((Int) 0),fuzz);
        add(s,t,S,T,S1,T1,p,fuzz2,single);
        return S1.size() > 0;
      }
      pscale=poffset=0.5;
    } else {
      Int tp=lp/2;
      p1=p.subpath(0,tp);
      p2=p.subpath(tp,lp);
      poffset=tp;
      pscale=1.0;
    }

    path q1,q2;
    double qscale,qoffset;

    if(lq <= 1) {
      if(lq == 1) q.halve(q1,q2);
      if(lq == 0 || q1 == q || q2 == q) {
        intersections(S1,T1,p,q.point((Int) 0),q.point((Int) 0),fuzz);
        add(s,t,S,T,S1,T1,p,fuzz2,single);
        return S1.size() > 0;
      }
      qscale=qoffset=0.5;
    } else {
      Int tq=lq/2;
      q1=q.subpath(0,tq);
      q2=q.subpath(tq,lq);
      qoffset=tq;
      qscale=1.0;
    }

    bool Short=lp == 1 && lq == 1;

    static size_t maxcount=9;
    size_t count=0;

    if(intersections(s,t,S1,T1,p1,q1,fuzz,single,exact,depth)) {
      add(s,t,S,T,S1,T1,pscale,qscale,0.0,0.0,p,fuzz2,single);
      if(single || depth <= mindepth)
        return true;
      count += S1.size();
      if(Short && count > maxcount) return true;
    }

    S1.clear();
    T1.clear();
    if(intersections(s,t,S1,T1,p1,q2,fuzz,single,exact,depth)) {
      add(s,t,S,T,S1,T1,pscale,qscale,0.0,qoffset,p,fuzz2,single);
      if(single || depth <= mindepth)
        return true;
      count += S1.size();
      if(Short && count > maxcount) return true;
    }

    S1.clear();
    T1.clear();
    if(intersections(s,t,S1,T1,p2,q1,fuzz,single,exact,depth)) {
      add(s,t,S,T,S1,T1,pscale,qscale,poffset,0.0,p,fuzz2,single);
      if(single || depth <= mindepth)
        return true;
      count += S1.size();
      if(Short && count > maxcount) return true;
    }

    S1.clear();
    T1.clear();
    if(intersections(s,t,S1,T1,p2,q2,fuzz,single,exact,depth)) {
      add(s,t,S,T,S1,T1,pscale,qscale,poffset,qoffset,p,fuzz2,single);
      if(single || depth <= mindepth)
        return true;
      count += S1.size();
      if(Short && count > maxcount) return true;
    }

    return S.size() > 0;
  }
  return false;
}

// }}}

ostream& operator<< (ostream& out, const path& p)
{
  Int n = p.length();
  if(n < 0)
    out << "<nullpath>";
  else {
    for(Int i = 0; i < n; i++) {
      out << p.point(i);
      if(p.straight(i)) out << "--";
      else
        out << ".. controls " << p.postcontrol(i) << " and "
            << p.precontrol(i+1) << newl << " ..";
    }
    if(p.cycles)
      out << "cycle";
    else
      out << p.point(n);
  }
  return out;
}

path concat(const path& p1, const path& p2)
{
  Int n1 = p1.length(), n2 = p2.length();

  if (n1 == -1) return p2;
  if (n2 == -1) return p1;

  mem::vector<solvedKnot> nodes(n1+n2+1);

  Int i = 0;
  nodes[0].pre = p1.point((Int) 0);
  for (Int j = 0; j < n1; j++) {
    nodes[i].point = p1.point(j);
    nodes[i].straight = p1.straight(j);
    nodes[i].post = p1.postcontrol(j);
    nodes[i+1].pre = p1.precontrol(j+1);
    i++;
  }
  for (Int j = 0; j < n2; j++) {
    nodes[i].point = p2.point(j);
    nodes[i].straight = p2.straight(j);
    nodes[i].post = p2.postcontrol(j);
    nodes[i+1].pre = p2.precontrol(j+1);
    i++;
  }
  nodes[i].point = nodes[i].post = p2.point(n2);

  return path(nodes, i+1);
}

// Interface to orient2d predicate optimized for pairs.
double orient2d(const pair& a, const pair& b, const pair& c)
{
  double detleft, detright, det;
  double detsum, errbound;
  double orient;

  FPU_ROUND_DOUBLE;

  detleft = (a.getx() - c.getx()) * (b.gety() - c.gety());
  detright = (a.gety() - c.gety()) * (b.getx() - c.getx());
  det = detleft - detright;

  if (detleft > 0.0) {
    if (detright <= 0.0) {
      FPU_RESTORE;
      return det;
    } else {
      detsum = detleft + detright;
    }
  } else if (detleft < 0.0) {
    if (detright >= 0.0) {
      FPU_RESTORE;
      return det;
    } else {
      detsum = -detleft - detright;
    }
  } else {
    FPU_RESTORE;
    return det;
  }

  errbound = ccwerrboundA * detsum;
  if ((det >= errbound) || (-det >= errbound)) {
    FPU_RESTORE;
    return det;
  }

  double pa[]={a.getx(),a.gety()};
  double pb[]={b.getx(),b.gety()};
  double pc[]={c.getx(),c.gety()};

  orient = orient2dadapt(pa, pb, pc, detsum);
  FPU_RESTORE;
  return orient;
}

// Returns true iff the point z lies in or on the bounding box
// of a,b,c, and d.
bool insidebbox(const pair& a, const pair& b, const pair& c, const pair& d,
                const pair& z)
{
  bbox B(a);
  B.addnonempty(b);
  B.addnonempty(c);
  B.addnonempty(d);
  return B.left <= z.getx() && z.getx() <= B.right && B.bottom <= z.gety()
    && z.gety() <= B.top;
}

inline bool inrange(double x0, double x1, double x)
{
  return (x0 <= x && x <= x1) || (x1 <= x && x <= x0);
}

// Return true if point z is on z0--z1; otherwise compute contribution to
// winding number.
bool checkstraight(const pair& z0, const pair& z1, const pair& z, Int& count)
{
  if(z0.gety() <= z.gety() && z.gety() <= z1.gety()) {
    double side=orient2d(z0,z1,z);
    if(side == 0.0 && inrange(z0.getx(),z1.getx(),z.getx()))
      return true;
    if(z.gety() < z1.gety() && side > 0) ++count;
  } else if(z1.gety() <= z.gety() && z.gety() <= z0.gety()) {
    double side=orient2d(z0,z1,z);
    if(side == 0.0 && inrange(z0.getx(),z1.getx(),z.getx()))
      return true;
    if(z.gety() < z0.gety() && side < 0) --count;
  }
  return false;
}

// returns true if point is on curve; otherwise compute contribution to
// winding number.
bool checkcurve(const pair& z0, const pair& c0, const pair& c1,
                const pair& z1, const pair& z, Int& count, unsigned depth)
{
  if(depth == 0) return true;
  --depth;

  if(insidebbox(z0,c0,c1,z1,z)) {
    const pair m0=0.5*(z0+c0);
    const pair m1=0.5*(c0+c1);
    const pair m2=0.5*(c1+z1);
    const pair m3=0.5*(m0+m1);
    const pair m4=0.5*(m1+m2);
    const pair m5=0.5*(m3+m4);
    if(checkcurve(z0,m0,m3,m5,z,count,depth) ||
       checkcurve(m5,m4,m2,z1,z,count,depth)) return true;
  } else
    if(checkstraight(z0,z1,z,count)) return true;
  return false;
}

// Return the winding number of the region bounded by the (cyclic) path
// relative to the point z, or the largest odd integer if the point lies on
// the path.
Int path::windingnumber(const pair& z) const
{
  static const Int undefined=Int_MAX % 2 ? Int_MAX : Int_MAX-1;

  if(!cycles)
    reportError("path is not cyclic");

  bbox b=bounds();

  if(z.getx() < b.left || z.getx() > b.right ||
     z.gety() < b.bottom || z.gety() > b.top) return 0;

  Int count=0;
  for(Int i=0; i < n; ++i)
    if(straight(i)) {
      if(checkstraight(point(i),point(i+1),z,count))
        return undefined;
    } else
      if(checkcurve(point(i),postcontrol(i),precontrol(i+1),point(i+1),z,count,
                    maxdepth)) return undefined;
  return count;
}

path path::transformed(const transform& t) const
{
  mem::vector<solvedKnot> nodes(n);

  for (Int i = 0; i < n; ++i) {
    nodes[i].pre = t * this->nodes[i].pre;
    nodes[i].point = t * this->nodes[i].point;
    nodes[i].post = t * this->nodes[i].post;
    nodes[i].straight = this->nodes[i].straight;
  }

  path p(nodes, n, cyclic());
  return p;
}

path transformed(const transform& t, const path& p)
{
  Int n = p.size();
  mem::vector<solvedKnot> nodes(n);

  for (Int i = 0; i < n; ++i) {
    nodes[i].pre = t * p.precontrol(i);
    nodes[i].point = t * p.point(i);
    nodes[i].post = t * p.postcontrol(i);
    nodes[i].straight = p.straight(i);
  }

  return path(nodes, n, p.cyclic());
}

path nurb(pair z0, pair z1, pair z2, pair z3,
          double w0, double w1, double w2, double w3, Int m)
{
  mem::vector<solvedKnot> nodes(m+1);

  if(m < 1) reportError("invalid sampling interval");

  double step=1.0/m;
  for(Int i=0; i <= m; ++i) {
    double t=i*step;
    double t2=t*t;
    double onemt=1.0-t;
    double onemt2=onemt*onemt;
    double W0=w0*onemt2*onemt;
    double W1=w1*3.0*t*onemt2;
    double W2=w2*3.0*t2*onemt;
    double W3=w3*t2*t;
    nodes[i].point=(W0*z0+W1*z1+W2*z2+W3*z3)/(W0+W1+W2+W3);
  }

  static const double twothirds=2.0/3.0;
  pair z=nodes[0].point;
  nodes[0].pre=z;
  nodes[0].post=twothirds*z+third*nodes[1].point;
  for(int i=1; i < m; ++i) {
    pair z0=nodes[i].point;
    pair zm=nodes[i-1].point;
    pair zp=nodes[i+1].point;
    pair pre=twothirds*z0+third*zm;
    pair pos=twothirds*z0+third*zp;
    pair dir=unit(pos-pre);
    nodes[i].pre=z0-length(z0-pre)*dir;
    nodes[i].post=z0+length(pos-z0)*dir;
  }
  z=nodes[m].point;
  nodes[m].pre=twothirds*z+third*nodes[m-1].point;
  nodes[m].post=z;
  return path(nodes,m+1);
}

} //namespace camp