/*
 * Class to hold a multi-segment Bezier curve. It can fit the curve to a set of 
 * digitalized points, for which it uses the algorithm implemented in Inkscape's 
 * freehand mode (which was first created by Philip J. Schneider in the late 80s).
 * 
 * Original code published in:
 *   An Algorithm for Automatically Fitting Digitized Curves
 *   by Philip J. Schneider
 *  "Graphics Gems", Academic Press, 1990
 *
 * Authors:
 *   Philip J. Schneider
 *   Lauris Kaplinski <lauris@kaplinski.com>
 *   Peter Moulder <pmoulder@mail.csse.monash.edu.au>
 *   Andres Colubri <andres.colubri@gmail.com>
 *
 * Copyright (C) 1990 Philip J. Schneider
 * Copyright (C) 2001 Lauris Kaplinski
 * Copyright (C) 2001 Ximian, Inc.
 * Copyright (C) 2003,2004 Monash University
 * Copyright (C) 2009 Andres Colubri
 *
 * This source is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This code is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * A copy of the GNU General Public License is available on the World
 * Wide Web at <http://www.gnu.org/copyleft/gpl.html>. You can also
 * obtain it by writing to the Free Software Foundation,
 * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */
 
class MultiBezier
{
    MultiBezier()
    {
        bezCurves = new Bezier[MAX_NUM_POINTS];
        for (int i = 0; i < MAX_NUM_POINTS; i++) bezCurves[i] = null;
        numSegments = 0;
        
        unconstrainedTangent = new PVector(0, 0);
        maxIterations = 4;   // Max times to try iterating.
        defError = 100.0;     // Default squared error.
        showMessages = false;
        showErrors = true;
        
        anchorsUse = new boolean[NUM_ANCHOR_POINTS];
        for (int i = 0; i < NUM_ANCHOR_POINTS; i++) anchorsUse[i] = false;
    }
    
    void clear()
    {
        for (int i = 0; i < numSegments; i++) bezCurves[i].stopPhysics();
        numSegments = 0;  
    }

    void render(boolean showControls)
    {
        for (int i = 0; i < numSegments; i++) bezCurves[i].render(showControls);
    }

    boolean selectPoint(float x, float y, float r)
    {
        boolean selected = false;
        for (int i = 0; i < numSegments; i++) 
        {
            boolean res = bezCurves[i].selectPoint(x, y, r);
            if (res) selected = true;
        }
        return selected;
    }
    
    void setSelectedPoint(float x, float y)
    {  
        for (int i = 0; i < numSegments; i++) bezCurves[i].setSelectedPoint(x, y);
    }

    void update()
    {
        for (int i = 0; i < numSegments; i++) bezCurves[i].update();  
    }

    int create(PVector[] points, int len)
    {
        return create(points, len, defError);
    }
    
    int create(PVector[] points, int len, float error)
    {
        return create(points, len, unconstrainedTangent, unconstrainedTangent, error);
    }
    
    int create(PVector[] points, int len, PVector tang0, PVector tang1, float error)
    {
        PVector[] data = copyWithoutAdjacentDuplicates(points, len);
        numSegments = 0;
        return recursiveFit(0, data, 0, data.length, tang0, tang1, error);
    }
    
    int addPoints(PVector[] points, int start, int len)
    {
        return addPoints(points, start, len, defError);
    }    
    
    int addPoints(PVector[] points, int start, int len, float error)
    {
        return addPoints(points, start, len, unconstrainedTangent, unconstrainedTangent, error);
    }    

    int addPoints(PVector[] points, int start, int len, PVector tang0, PVector tang1, float error)
    {
        PVector[] data = copyWithoutAdjacentDuplicates(points, start, len);
        return recursiveFit(numSegments, data, 0, data.length, tang0, tang1, error);
    }
    
    // Recursively fits the multi-segment Bezier curve to a set of digitized points.
    int recursiveFit(int segment, PVector[] data, int start, int len, PVector tHat1, PVector tHat2, float error)
    {
        Bezier bezCurve;

        if (len < 2) return 0;

        if (len == 2)
        {
            // We have 2 points, which can be fitted trivially.
            PVector p0, p1, p2, p3;
            
            p0 = data[start];
            p3 = data[start + len - 1];
            
            float d = p0.dist(p3) / 3.0;
            if ((new Float(d)).equals(Float.NaN))
            {
                // Numerical problem, fall back to straight line segment.
                p1 = p0;
                p2 = p3;
            }
            else
            {
                p1 = isZero(tHat1) ? new PVector((2.0 * p0.x + p3.x) / 3.0, (2.0 * p0.y + p3.y) / 3.0)
                                   : new PVector(p0.x + d * tHat1.x, p0.y + d * tHat1.y);
                p2 = isZero(tHat2) ? new PVector((p0.x + 2.0 * p3.x) / 3.0, (p0.y + 2.0 * p3.y) / 3.0)
                                   : new PVector(p3.x + d * tHat2.x, p3.y + d * tHat2.y);
            }
            
            bezCurve = new Bezier(p0.x, p0.y, p1.x, p1.y, p2.x, p2.y, p3.x, p3.y);
            addNewSegment(bezCurve, segment);
            
            return 1;
        }

        //  Parameterize points, and attempt to fit curve.
        int splitPoint; // Point to split point set at.
        int[] splitPointArray = { 0 }; // The array is used to get the split point value from computeMaxErrorRatio.
        boolean is_corner;

        float[] u = new float[len];
        chordLengthParameterize(data, u, start, len);
        if (u[len - 1] == 0.0) 
        {
            // Zero-length path: every point in data[] is the same.
            return 0;
        }

        bezCurve = new Bezier();
        generateBezier(bezCurve, data, u, start, len, tHat1, tHat2, error);
        reparameterize(data, start, len, u, bezCurve);

        // Find max deviation of points to fitted curve.
        float tolerance = sqrt(error + 1E-9);
        float maxErrorRatio = computeMaxErrorRatio(data, u, start, len, bezCurve, tolerance, splitPointArray);
        splitPoint = splitPointArray[0];

        if (abs(maxErrorRatio) <= 1.0) 
        {
            addNewSegment(bezCurve, segment);
            return 1;
        }

        // If error not too large, then try some reparameterization and iteration.
        if (0.0 <= maxErrorRatio && maxErrorRatio <= 3.0 ) 
        {
            for (int i = 0; i < maxIterations; i++) 
            {
                generateBezier(bezCurve, data, u, start, len, tHat1, tHat2, error);
                reparameterize(data, start, len, u, bezCurve);
                maxErrorRatio = computeMaxErrorRatio(data, u, start, len, bezCurve, tolerance, splitPointArray);
                splitPoint = splitPointArray[0];
                if (abs(maxErrorRatio) <= 1.0) 
                {
                    addNewSegment(bezCurve, segment);
                    return 1;
                }
            }
        }
        is_corner = (maxErrorRatio < 0);

        if (is_corner) 
        {
            if (!(splitPoint < len))
            {
                if (showErrors) println("Error: splitPoint >= len.");
                return 0;  
            }
            
            if (splitPoint == 0) 
            {
                if (isZero(tHat1)) 
                {
                    // Got spike even with unconstrained initial tangent.
                    ++splitPoint;
                } 
                else 
                {
                    return recursiveFit(segment, data, start, len, unconstrainedTangent, tHat2, error);
                }
            } 
            else if (splitPoint == len - 1) 
            {
                if (isZero(tHat2)) 
                {
                    // Got spike even with unconstrained final tangent.
                    --splitPoint;
                } 
                else 
                {
                    return recursiveFit(segment, data, start, len, tHat1, unconstrainedTangent, error);
                }
            }
        }

        //  Fitting failed -- split at max error point and fit recursively
        PVector recTHat2 = new PVector();
        PVector recTHat1 = new PVector();
        if (is_corner) 
        {
            if (!(0 < splitPoint && splitPoint < len - 1)) return -1;
            recTHat1.set(unconstrainedTangent); 
            recTHat2.set(unconstrainedTangent); 
        } 
        else 
        {
            // Unit tangent vector at splitPoint.
            recTHat2 = spDarrayCenterTangent(data, start, splitPoint, len);
            recTHat1.set(-recTHat2.x, -recTHat2.y, 0);
        }
        int nsegs1 = recursiveFit(segment, data, start, splitPoint + 1, tHat1, recTHat2, error);
        if ( nsegs1 < 0 ) 
        {
            if (showErrors) println("Error: recursive fit 1 failed.");
            return -1;
        }
        
        int nsegs2 = recursiveFit(segment + nsegs1, data, start + splitPoint, len - splitPoint, recTHat1, tHat2, error);
        if ( nsegs2 < 0 ) 
        {
            if (showErrors) println("Error: recursive fit 2 failed.");
            return -1;
        }

        if (showMessages) println("Success: recursive fit with "+ nsegs1 + "+" + nsegs2 + "=" + int(nsegs1 + nsegs2) + " segments");
        return nsegs1 + nsegs2;
    }

    // Remove adjacent points with equal x and y.
    PVector[] copyWithoutAdjacentDuplicates(PVector[] points, int len)
    {
        return copyWithoutAdjacentDuplicates(points, 0, len);
    }

    // Remove adjacent points with equal x and y.
    PVector[] copyWithoutAdjacentDuplicates(PVector[] points, int start, int len)
    {
        boolean[] use = new boolean[len];
        
        int ndiff = 1;
        use[0] = true;
        for (int i = len - 1; i >= 1; i--)
        {         
            if (!areEqual(points[start + i], points[start + i - 1]))
            {
                use[i] = true;
                ndiff++;
            }
            else use[i] = false;
        }
        
        PVector[] data = new PVector[ndiff];
        int n = 0;        
        for (int i = 0; i < len; i++)
            if (use[i])
            {
                data[n] = points[start + i];
                n++;
            }

        return data;
    }
    
    // Assign parameter values to digitized points using relative distances between points.
    void chordLengthParameterize(PVector[] d, float[] u, int start, int len)
    {
        // First let u[i] equal the distance travelled along the path from d[start + 0] to d[start + i].
        u[0] = 0.0;
        for (int i = 1; i < len; i++) 
        {
            float r = d[start + i].dist(d[start + i - 1]);
            u[i] = u[i - 1] + r;
        }

        // Then scale to [0.0 .. 1.0].
        float totLen = u[len - 1];
        for (int i = 1; i < len - 1; i++) 
        {
            u[i] /= totLen;
        }
        u[len - 1] = 1.0;   
    }

    // Returns a bezier based on the given data and tangent requirements, using
    // a least-squares fit.
    // Each of tHat1 and tHat2 should be either a zero vector or a unit vector.
    // If it is zero, then bezier[1 or 2] is estimated without constraint; otherwise,
    // it bezier[1 or 2] is placed in the specified direction from bezier[0 or 3].
    // Parameter tolerance_sq is used only for an initial guess as to tangent directions
    // when tHat1 or tHat2 is zero.
    void generateBezier(Bezier bezCurve, PVector[] d, float[] u, int start, int len, PVector tHat0, PVector tHat1, float toleranceSq)
    {
        boolean est0 = isZero(tHat0);
        boolean est1 = isZero(tHat1);
        PVector est_tHat0 = est0 ? spDarrayLeftTangent(d, start, len, toleranceSq)
                                 : new PVector(tHat0.x, tHat0.y);
        PVector est_tHat1 = est1 ? spDarrayRightTangent(d, start, len, toleranceSq)
                                 : new PVector(tHat1.x, tHat1.y);
                                 
        estimateLengths(bezCurve, d, u, start, len, est_tHat0, est_tHat1);
        
        // spDarrayRightTangent tends to produce better results in the Inkscape's
        // freehand tool than full estimation.
        if (est0) 
        {
            estimateBi(bezCurve, 1, d, u, start, len);
            if (!areEqual(bezCurve.points[0], bezCurve.points[1]))
            {
               est_tHat0 = PVector.sub(bezCurve.points[1], bezCurve.points[0]);
               est_tHat0.normalize();
            }
            estimateLengths(bezCurve, d, u, start, len, est_tHat0, est_tHat1);
        }
    }

    // Estimate the (forward) tangent at point d[start + first + 0.5].
    // Unlike the center and right versions, this calculates the tangent in 
    // the way one might expect, i.e., wrt increasing index into d.
    PVector spDarrayLeftTangent(PVector[] d, int start, int len)
    {
        PVector res = PVector.sub(d[start + 1], d[start + 0]);
        res.normalize();
        return res;
    }

    // Estimates the (backward) tangent at d[start + last - 0.5].
    // Note: The tangent is "backwards", i.e. it is with respect to 
    // decreasing index rather than increasing index.
    PVector spDarrayRightTangent(PVector[] d, int start, int len)
    {
        int last = len - 1;
        int prev = last - 1;
        PVector res = PVector.sub(d[start + prev], d[start + last]);
        res.normalize();
        return res;
    }
    
    // Estimate the (forward) tangent at point d[start + 0].
    // Unlike the center and right versions, this calculates the tangent in 
    // the way one might expect, i.e., wrt increasing index into d.
    PVector spDarrayLeftTangent(PVector[] d, int start, int len, float toleranceSq)
    {
        int i = 0;
        while (true) 
        {
            PVector pi = d[start + i];
            PVector t = PVector.sub(pi, d[start + 0]);
            float distsq = t.dot(t);
            if (toleranceSq < distsq) 
            {
                t.normalize();
                return t;
            }
            ++i;
            if (i == len) 
            {
                if (distsq == 0) return spDarrayLeftTangent(d, start, len);
                else 
                {
                    t.normalize();
                    return t;                  
                }
            }
        }
    }

    // Estimates the (backward) tangent at d[start + last].
    // Note: The tangent is "backwards", i.e. it is with respect to 
    // decreasing index rather than increasing index.
    PVector spDarrayRightTangent(PVector[] d, int start, int len, float toleranceSq)
    {
        int last = len - 1;
        int i = last - 1;
        while (true) 
        {
            PVector pi = d[start + i];
            PVector t =  PVector.sub(pi, d[start + last]);
            float distsq = t.dot(t);
            if (toleranceSq < distsq) 
            {
                t.normalize();
                return t;
            }
            if (i == 0) 
            {
                if (distsq == 0) return spDarrayRightTangent(d, start, len);
                else 
                {
                    t.normalize();
                    return t;                  
                }
            }
            i--;
        }
    }

    // Estimates the (backward) tangent at d[start + center], by averaging the two 
    // segments connected to d[start + center] (and then normalizing the result).
    // Note: The tangent is "backwards", i.e. it is with respect to 
    // decreasing index rather than increasing index.
    PVector spDarrayCenterTangent(PVector[] d, int start, int center, int len)
    {
        PVector res = new PVector();
        if (areEqual(d[start + center + 1], d[start + center - 1])) 
        {
            // Rotate 90 degrees in an arbitrary direction.
            PVector diff = PVector.sub(d[start + center], d[start + center - 1]);
            res.set(-diff.y, diff.x, 0);
        } 
        else 
        {
            res.set(d[start + center - 1].x - d[start + center + 1].x, d[start + center - 1].y - d[start + center + 1].y, 0);
        }
        res.normalize();
        return res;
    }

    void estimateLengths(Bezier bezCurve, PVector[] data, float[] uPrime, int start, int len, PVector tHat1, PVector tHat2)
    {
        // Create the C and X matrices.
        float[][] C = {{0, 0}, {0, 0}};  // Matrix C.
        float[]   X = {0, 0};            // Matrix X.

        // First and last control points of the Bezier curve are positioned exactly at the first and
        // last data points.
        bezCurve.points[0].set(data[start + 0]);
        bezCurve.points[3].set(data[start + len - 1]);

        for (int i = 0; i < len; i++) 
        {
            // Bezier control point coefficients.
            float b0 = B0(uPrime[i]);
            float b1 = B1(uPrime[i]);
            float b2 = B2(uPrime[i]);
            float b3 = B3(uPrime[i]);

            // rhs for eqn
            PVector a1 = PVector.mult(tHat1, b1);
            PVector a2 = PVector.mult(tHat2, b2);

            C[0][0] += a1.dot(a1);
            C[0][1] += a1.dot(a2);
            C[1][0] = C[0][1];
            C[1][1] += a2.dot(a2);

            // Additional offset to the data point from the predicted point if we were to set bezier[1]
            // to bezier[0] and bezier[2] to bezier[3]. */
            PVector shortfall = new PVector(data[start + i].x - ((b0 + b1) * bezCurve.points[0].x) - ((b2 + b3) * bezCurve.points[3].x),
                                            data[start + i].y - ((b0 + b1) * bezCurve.points[0].y) - ((b2 + b3) * bezCurve.points[3].y));
            X[0] += shortfall.dot(a1);
            X[1] += shortfall.dot(a2);
        }

        // We've constructed a pair of equations in the form of a matrix product C * alpha = X.
        // Now solve for alpha.
        float alpha_l, alpha_r;

        // Compute the determinants of C and X.
        float det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
        if ( det_C0_C1 != 0 ) 
        {
            // Apparently Kramer's rule.
            float det_C0_X  = C[0][0] * X[1]    - C[0][1] * X[0];
            float det_X_C1  = X[0]    * C[1][1] - X[1]    * C[0][1];
            alpha_l = det_X_C1 / det_C0_C1;
            alpha_r = det_C0_X / det_C0_C1;
        } 
        else 
        {
            // The matrix is under-determined.  Try requiring alpha_l == alpha_r.
            //  One way of implementing the constraint alpha_l == alpha_r is to treat them as the same
            //  variable in the equations.  We can do this by adding the columns of C to form a single
            //  column, to be multiplied by alpha to give the column vector X.
            //  We try each row in turn.
            float c0 = C[0][0] + C[0][1];
            if (c0 != 0) 
            {
                alpha_l = alpha_r = X[0] / c0;
            } 
            else 
            {
                float c1 = C[1][0] + C[1][1];
                if (c1 != 0) 
                {
                    alpha_l = alpha_r = X[1] / c1;
                } 
                else 
                {
                    // Let the below code handle this.
                    alpha_l = alpha_r = 0.;
                }
            }
        }

        // If alpha negative, use the Wu/Barsky heuristic (see text).  (If alpha is 0, you get
        // coincident control points that lead to divide by zero in any subsequent
        // NewtonRaphsonRootFind() call.)
        // todo Check whether this special-casing is necessary now that 
        // NewtonRaphsonRootFind handles non-positive denominator.
        if (alpha_l < 1.0e-6 ||
            alpha_r < 1.0e-6)
        {
            alpha_l = alpha_r = data[start + len - 1].dist(data[start + 0]) / 3.0;
        }

        // Control points 1 and 2 are positioned an alpha distance out on the tangent vectors, left and
        // right, respectively.
        bezCurve.points[1].set(alpha_l * tHat1.x + bezCurve.points[0].x, alpha_l * tHat1.y + bezCurve.points[0].y, 0);
        bezCurve.points[2].set(alpha_r * tHat2.x + bezCurve.points[3].x, alpha_r * tHat2.y + bezCurve.points[3].y, 0);
    }

    // ei must be 1 or 2.
    void estimateBi(Bezier bezCurve, int ei, PVector[] data, float[] u, int start, int len)
    {
        int oi = 3 - ei;
        float[] num = {0.0, 0.0};
        float den = 0.0;
        for (int i = 0; i < len; ++i) 
        {
            float ui = u[i];
            float[] b = { B0(ui), B1(ui), B2(ui), B3(ui) };

            for (int d = 0; d < 2; ++d) 
            {
                num[d] += b[ei] * (b[0]  * bezCurve.getPointCoord(0, d) +
                                   b[oi] * bezCurve.getPointCoord(oi, d) +
                                   b[3]  * bezCurve.getPointCoord(3, d) +
                                   - (d == 0 ? data[start + i].x : data[start + i].y));
            }
            den -= b[ei] * b[ei];
        }

        if (den != 0.0) 
        {
            for (int d = 0; d < 2; ++d) 
            {
                bezCurve.setPointCoord(ei, d, num[d] / den);
            }
        } 
        else 
        {
            bezCurve.setPoint(ei, (oi * bezCurve.points[0].x + ei * bezCurve.points[3].x ) / 3.0,
                                  (oi * bezCurve.points[0].y + ei * bezCurve.points[3].y ) / 3.0);
        }
    }

    // Given set of points and their parameterization, try to find a better assignment of parameter
    // values for the points.
    void reparameterize(PVector[] d, int start, int len, float u[], Bezier bezCurve)
    {
        int last = len - 1;
        for (int i = 1; i < last; i++) 
        {
            u[i] = newtonRaphsonRootFind(bezCurve, d[start + i], u[i]);
        }
    }

    //  Use Newton-Raphson iteration to find better root. Takes parameter value u for point P, and
    // returns improved u.
    float newtonRaphsonRootFind(Bezier Q, PVector P, float u)
    {
        PVector Q_u = new PVector();
        PVector Q1_u = new PVector();
        PVector Q2_u = new PVector();

        // Compute Q(u), Q'(u) and Q''(u).
        Q.eval(0, Q_u, u);
        Q.eval(1, Q1_u, u);
        Q.eval(2, Q2_u, u);

        // Compute f(u)/f'(u), where f is the derivative wrt u of distsq(u) = 0.5 * the square of the
        // distance from P to Q(u).  Here we're using Newton-Raphson to find a stationary point in the
        // distsq(u), hopefully corresponding to a local minimum in distsq (and hence a local minimum
        // distance from P to Q(u)).
        PVector diff = PVector.sub(Q_u, P);
        float numerator = diff.dot(Q1_u);
        float denominator = Q1_u.dot(Q1_u) + diff.dot(Q2_u);

        float improved_u = 0.0;
        if (denominator > 0.0) 
        {
            // One iteration of Newton-Raphson:
            // improved_u = u - f(u)/f'(u)
            improved_u = u - (numerator / denominator);
        } 
        else 
        {
            // Using Newton-Raphson would move in the wrong direction (towards a local maximum rather
            //   than local minimum), so we move an arbitrary amount in the right direction.
            if (numerator > 0.0) 
            {
                improved_u = u * .98 - .01;
            } 
            else if (numerator < 0.0) 
            {
                // Deliberately asymmetrical, to reduce the chance of cycling.
                improved_u = 0.031 + u * 0.98;
            } 
            else 
            {
                improved_u = u;
            }
        }

        // This code is not used for performance issues:
        //if (!(new Float(improved_u)).equals(Float.NaN)) improved_u = u;
        //else if (improved_u < 0.0) improved_u = 0.0;
        //else if (improved_u > 1.0) improved_u = 1.0;
        // More details on how to deal with NaN and Infinity in Java:
        // http://www.concentric.net/~Ttwang/tech/javafloat.htm

        // Ensure that improved_u isn't actually worse.
        float diff_lensq = diff.dot(diff);
        float proportion = 0.125;
        PVector Q_improved_u = new PVector();
        while (true) 
        {
            Q.eval(0, Q_improved_u, improved_u);
            Q_improved_u.sub(P);
            if (Q_improved_u.dot(Q_improved_u) > diff_lensq) 
            {
                if (proportion > 1.0) 
                {
                    improved_u = u;
                    break;
                }
                improved_u = (1 - proportion) * improved_u  + proportion * u;
            } 
            else 
            {
                break;
            }
                
            proportion += 0.125;
        }

        return improved_u;
    }

    // Find the maximum squared distance of digitized points to fitted curve, and (if this maximum
    // error is non-zero) set splitPoint to the corresponding index.
    float computeMaxErrorRatio(PVector[] d, float[] u, int start, int len, Bezier bezCurve, float tolerance, int[] splitPointArray)
    {
        float maxDistsq = 0.0; // Maximum error.
        float max_hook_ratio = 0.0;
        int snap_end = 0;
        
        int splitPoint = 0;
        int last = len - 1;
        PVector prev = bezCurve.points[0];        
        PVector curr = new PVector();
        for (int i = 1; i <= last; i++) 
        {
            bezCurve.eval(0, curr, u[i]);
            PVector diff = PVector.sub(curr, d[start + i]);
            float distsq = diff.dot(diff);
            if (distsq > maxDistsq)
            {
                maxDistsq = distsq;
                splitPoint = i;
            }
            float hook_ratio = computeHook(prev, curr, 0.5 * (u[i - 1] + u[i]), bezCurve, tolerance);
            if (max_hook_ratio < hook_ratio) 
            {
                max_hook_ratio = hook_ratio;
                snap_end = i;
            }
            prev = curr;
        }

        float dist_ratio = sqrt(maxDistsq) / tolerance;
        float ret;
        if (max_hook_ratio <= dist_ratio) 
        {
            ret = dist_ratio;
        } 
        else 
        {
            ret = -max_hook_ratio;
            splitPoint = snap_end - 1;
        }
        splitPointArray[0] = splitPoint;
        
        splitPoint = int(constrain(splitPoint, 0, last));
        
        if (!(ret == 0.0 || ((splitPoint < last) && ( splitPoint != 0 || ret < 0.0))))
        {
            if (showErrors) println("Error determining MaxErrorRatio");
            splitPoint = 0;
            ret = 0.0;
        }

        return ret;
    }
 
    // Whereas computeMaxErrorRatio() checks for itself that each data point 
    // is near some point on the curve, this function checks that each point on 
    // the curve is near some data point (or near some point on the polyline 
    // defined by the data points, or something like that: we allow for a
    // "reasonable curviness" from such a polyline).  "Reasonable curviness" 
    // means we draw a circle centred at the midpoint of a..b, of radius 
    // proportional to the length |a - b|, and require that each point on the 
    // segment of bezCurve between the parameters of a and b be within that circle.
    // If any point P on the bezCurve segment is outside of that allowable 
    // region (circle), then we return some metric that increases with the 
    // distance from P to the circle.
    //
    //  Given that this is a fairly arbitrary criterion for finding appropriate 
    //  places for sharp corners, we test only one point on bezCurve, namely 
    //  the point on bezCurve with parameter halfway between our estimated 
    //  parameters for a and b.  (Alternatives are taking the farthest of a
    //  few parameters between those of a and b, or even using a variant of 
    //  NewtonRaphsonFindRoot() for finding the maximum rather than minimum 
    //  distance.)
    float computeHook(PVector a, PVector b, float u, Bezier bezCurve, float tolerance)
    {
        PVector P = new PVector();        
        bezCurve.eval(0, P, u);
        
        PVector diffP = PVector.add(a, b);
        PVector diffAB = PVector.sub(b, a);
        diffP.mult(0.5);
        diffP.sub(P);
        
        float d = diffP.mag();
        if (d < tolerance) {
            return 0;
        }
        float allowed = diffAB.mag() + tolerance;
        return d / allowed;
        // todo (from Inskcape):
        // effic: Hooks are very rare.  We could start by comparing 
        // distsq, only resorting to the more expensive L2 in cases of 
        // uncertainty.
    }
    
    void addNewSegment(Bezier bezCurve, int sgt)
    {
        if (sgt < MAX_NUM_POINTS)
        {
            if (bezCurves[sgt] == null)  
            {
                bezCurves[sgt] = new Bezier(bezCurve);
            }
            else
            {
                bezCurves[sgt].copyFrom(bezCurve);
            }
            setupPhysics(sgt);
            if (numSegments <= sgt) numSegments = sgt + 1;            
        }
    }

    void setupPhysics()
    {
        for (int i = 0; i < numSegments; i++) setupPhysics(i);
    }
    
    void setupPhysics(int i)
    {
        PParticle pt, prevPt;
        for (int n = 0; n < 4; n++)
        {
            pt = bezCurves[i].points[n];
            pt.moveTo(pt.x, pt.y);
                
            for (int j = 0; j < i; j++)
                for (int m = 0; m < 4; m++)
                {
                    prevPt = bezCurves[j].points[m];
                    physics.makeSpring(pt.particle, prevPt.particle, SPRING_CONSTANT, DAMP_CONSTANT, pt.dist(prevPt));
                }

            for (int m = 0; m < n; m++)
            {
                prevPt = bezCurves[i].points[m];
                physics.makeSpring(pt.particle, prevPt.particle, SPRING_CONSTANT, DAMP_CONSTANT, pt.dist(prevPt));                  
            }
        }
    }
    
    void addAnchor(int idx)
    {
        anchorsUse[idx] = true;  
    }
    
    void setupAnchors(PParticle[] anchorsList)
    {
        PParticle anchor, pt;
        for (int idx = 0; idx < anchorsList.length; idx++)
            if (anchorsUse[idx])
            {
                anchor = anchorsList[idx];
                for (int i = 0; i < numSegments; i++)
                    for (int n = 0; n < 4; n++)
                    {
                        pt = bezCurves[i].points[n];
                        physics.makeSpring(pt.particle, anchor.particle, SPRING_CONSTANT, DAMP_CONSTANT, pt.dist(anchor));
                    }
            }      
    }
 
    boolean areEqual(PVector v1, PVector v2)
    {
        return abs(v1.x - v2.x) + abs(v1.y - v2.y) < 1E-8;
    }
    
    boolean isZero(PVector v)
    {
        return abs(v.x) + abs(v.y) < 1E-8;      
    }
 
    float B0(float u) { return ( 1.0 - u )  *  ( 1.0 - u )  *  ( 1.0 - u ); }
    float B1(float u) { return 3 * u  *  ( 1.0 - u )  *  ( 1.0 - u ); }
    float B2(float u) { return 3 * u * u  *  ( 1.0 - u ); }
    float B3(float u) { return u * u * u; }

    Bezier[] bezCurves;
    int numSegments;
    int maxIterations;
    float defError;
    PVector unconstrainedTangent;
    boolean showMessages;
    boolean showErrors;
    boolean[] anchorsUse;
}