#include <2geom/solver.h>

#include <2geom/point.h>

#include <algorithm>

namespace Geom{

#define SGN(a) (((a)<0) ? -1 : 1)

static Geom::Point

Bezier(Geom::Point const *V,

unsigned degree,

double t,

Geom::Point *Left,

Geom::Point *Right);

crossing_count(Geom::Point const *V, unsigned degree);

static unsigned

control_poly_flat_enough(Geom::Point const *V, unsigned degree);

static double

compute_x_intercept(Geom::Point const *V, unsigned degree);

const unsigned MAXDEPTH = 64; /* Maximum depth for recursion */

const double BEPSILON = ldexp(1.0,-MAXDEPTH-1); /*Flatness control value */

unsigned total_steps, total_subs;

find_parametric_bezier_roots(Geom::Point const *w, /* The control points */

unsigned degree, /* The degree of the polynomial */

std::vector<double> &solutions, /* RETURN candidate t-values */

unsigned depth) /* The depth of the recursion */

{

total_steps++;

const unsigned max_crossings = crossing_count(w, degree);

switch (max_crossings) {

case 0: /* No solutions here */

return;

case 1:

/* Unique solution */

/* Stop recursion when the tree is deep enough */

/* if deep enough, return 1 solution at midpoint */

if (depth >= MAXDEPTH) {

solutions.push_back((w[0][Geom::X] + w[degree][Geom::X]) / 2.0);

return;

}

// I thought secant method would be faster here, but it'aint. -- njh

if (control_poly_flat_enough(w, degree)) {

solutions.push_back(compute_x_intercept(w, degree));

return;

}

break;

}

/* Otherwise, solve recursively after subdividing control polygon */

//Geom::Point Left[degree+1], /* New left and right */

// Right[degree+1]; /* control polygons */

std::vector<Geom::Point> Left( degree+1 ), Right(degree+1);

Bezier(w, degree, 0.5, Left.data(), Right.data());

total_subs ++;

find_parametric_bezier_roots(Left.data(), degree, solutions, depth+1);

find_parametric_bezier_roots(Right.data(), degree, solutions, depth+1);

}

crossing_count(Geom::Point const *V, /* Control pts of Bezier curve */

unsigned degree) /* Degree of Bezier curve */

{

unsigned n_crossings = 0; /* Number of zero-crossings */

int old_sign = SGN(V[0][Geom::Y]);

for (unsigned i = 1; i <= degree; i++) {

int sign = SGN(V[i][Geom::Y]);

if (sign != old_sign)

n_crossings++;

old_sign = sign;

}

return n_crossings;

}

static unsigned

control_poly_flat_enough(Geom::Point const *V, /* Control points */

unsigned degree) /* Degree of polynomial */

{

/* Find the perpendicular distance from each interior control point to line connecting V[0] and

/* Derive the implicit equation for line connecting first */

/* and last control points */

const double a = V[0][Geom::Y] - V[degree][Geom::Y];

const double b = V[degree][Geom::X] - V[0][Geom::X];

const double c = V[0][Geom::X] * V[degree][Geom::Y] - V[degree][Geom::X] * V[0][Geom::Y];

const double abSquared = (a * a) + (b * b);

//double distance[degree]; /* Distances from pts to line */

std::vector<double> distance(degree); /* Distances from pts to line */

for (unsigned i = 1; i < degree; i++) {

/* Compute distance from each of the points to that line */

double & dist(distance[i-1]);

const double d = a * V[i][Geom::X] + b * V[i][Geom::Y] + c;

dist = d*d / abSquared;

if (d < 0.0)

dist = -dist;

}

// Find the largest distance

double max_distance_above = 0.0;

double max_distance_below = 0.0;

for (unsigned i = 0; i < degree-1; i++) {

const double d = distance[i];

if (d < 0.0)

max_distance_below = std::min(max_distance_below, d);

if (d > 0.0)

max_distance_above = std::max(max_distance_above, d);

}

const double intercept_1 = (c + max_distance_above) / -a;

const double intercept_2 = (c + max_distance_below) / -a;

/* Compute bounding interval*/

const double left_intercept = std::min(intercept_1, intercept_2);

const double right_intercept = std::max(intercept_1, intercept_2);

const double error = 0.5 * (right_intercept - left_intercept);

if (error < BEPSILON)

return 1;

return 0;

}

static double

compute_x_intercept(Geom::Point const *V, /* Control points */

unsigned degree) /* Degree of curve */

{

const Geom::Point A = V[degree] - V[0];

return (A[Geom::X]*V[0][Geom::Y] - A[Geom::Y]*V[0][Geom::X]) / -A[Geom::Y];

}

static Geom::Point

Bezier(Geom::Point const *V, /* Control pts */

unsigned degree, /* Degree of bezier curve */

double t, /* Parameter value */

Geom::Point *Left, /* RETURN left half ctl pts */

Geom::Point *Right) /* RETURN right half ctl pts */

{

//Geom::Point Vtemp[degree+1][degree+1];

std::vector<std::vector<Geom::Point> > Vtemp(degree+1);

for ( size_t i = 0; i < degree + 1; ++i )

Vtemp.reserve(degree+1);

/* Copy control points */

std::copy(V, V+degree+1, Vtemp[0].begin());

/* Triangle computation */

for (unsigned i = 1; i <= degree; i++) {

for (unsigned j = 0; j <= degree - i; j++) {

Vtemp[i][j] = lerp(t, Vtemp[i-1][j], Vtemp[i-1][j+1]);

}

}

for (unsigned j = 0; j <= degree; j++)

Left[j] = Vtemp[j][0];

for (unsigned j = 0; j <= degree; j++)

Right[j] = Vtemp[degree-j][j];

return (Vtemp[degree][0]);

}

};