/*
 *  Copyright (c) 2024 - 2025 by Paul Hertz <ignotus@gmail.com>
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU Library General Public License as published
 *   by the Free Software Foundation; either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   This program 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 Library General Public License for more details.
 *
 *   You should have received a copy of the GNU Library General Public
 *   License along with this program; if not, write to the Free Software
 *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

package net.paulhertz.pixelaudio.curves;

import java.util.ArrayList;
import java.util.List;
import java.util.ListIterator;

import processing.core.PVector;
import processing.core.PApplet;
import processing.core.PGraphics;

import net.paulhertz.pixelaudio.schedule.GestureSchedule;

/**
 *  <p>A class to maintain static versions of point reduction and curve-modeling methods.
 *  Called by PACurveMaker and other curve-modeling applications. Includes an implementation
 *  of the Ramer-Douglas-Peucker (RDP) point reduction algorithm, Bezier curve generation 
 *  methods, and a simple brush stroke modeling method.</p>
 *  
 * <p>Drawing methods are supplied to simplify drawing calls for the objects generated by 
 * PACurveUtility and PACurveMaker. The curve and shape drawing methods in particular
 * can be handled by the PABezShape class, which has its own fill and stroke properties
 * for curves (Bezier paths with no fill) and shapes (Bezier paths that may be filled), 
 * which are not distinguished within PABezShape. When stroke, fill, and weight are 
 * supplied in method arguments for PABezShape instances, the methods will call 
 * PABezShape's drawQuick() methods, which use the current drawing context to determine
 * fill, stroke, and weight properties.</p>
 *  
 *  @see <a href="https://thecodingtrain.com/CodingChallenges/152-rdp-algorithm.html">https://thecodingtrain.com/CodingChallenges/152-rdp-algorithm.html</a> 
 *  @see <a href="http://www.particleincell.com/2012/bezier-splines/">http://www.particleincell.com/2012/bezier-splines/</a>
 *  @see <a href="https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm">https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm</a>
 */
public class PACurveUtility {
	
	private PACurveUtility() {}
	
	/* ------------- BEGIN CODE FROM CODING TRAIN ------------- */
	
	/* See Coding Challenge on RDP Line Simplification at 
	 * https://thecodingtrain.com/CodingChallenges/152-rdp-algorithm.html 
	 */

	// TODO Maybe a distance squared evaluation instead of a distance evaluation would speed things up?
	// DONE A version that also sifts time data to generate a sequence of timed PVectors: see PAGestureParametric
	// DONE version with an int[] rdpIndices argument passed to accumulate index numbers for rdpPoints
	// we can use this to get time stamps from dragPoints in PACurveMaker and so provide velocity for
	// events that are triggered from RDP data -- Bezier curves could just be a representation
	// for interface purposes, or we *could* figure out how to provide them with timing, too. See indexedRDP().
	
	/**
	 * Ramer-Douglas-Peucker point reduction algorithm (RDP), reduces points in allPoints and 
	 * returns the result in rdpPoints.
	 * 
	 * @param startIndex	start index in allPoints (usually 0 to begin with)
	 * @param endIndex		end index in allPoints (usually allPoints.size()-1 to begin with)
	 * @param allPoints		ArrayList of dense points, for example a hand-drawn line
	 * @param rdpPoints		an empty ArrayList for accumulating reduced points
	 * @param epsilon       the expected distance between reduced points
	 */
	public static void rdp(int startIndex, int endIndex, ArrayList<PVector> allPoints, ArrayList<PVector> rdpPoints, float epsilon) {
	  int nextIndex = findFurthest(allPoints, startIndex, endIndex, epsilon);
	  if (nextIndex > 0) {
	    if (startIndex != nextIndex) {
	      rdp(startIndex, nextIndex, allPoints, rdpPoints, epsilon);
	    }
	    rdpPoints.add(allPoints.get(nextIndex));
	    if (endIndex != nextIndex) {
	      rdp(nextIndex, endIndex, allPoints, rdpPoints, epsilon);
	    }
	  }
	}
	
	/**
	 * Ramer-Douglas-Peucker point reduction algorithm (RDP), reduces points in allPoints and 
	 * returns the result in rdpPoints.
	 * 
	 * @param startIndex	start index in allPoints (usually 0 to begin with)
	 * @param endIndex		end index in allPoints (usually allPoints.size()-1 to begin with)
	 * @param allPoints		ArrayList of dense points, for example a hand-drawn line
	 * @param rdpPoints		an empty ArrayList for accumulating reduced points
	 * @param epsilon       the expected distance between reduced points
	 */
	public static void indexedRDP(int startIndex, int endIndex, ArrayList<PVector> allPoints, 
			ArrayList<PVector> rdpPoints, ArrayList<Integer> rdpIndices, float epsilon) {
	  int nextIndex = findFurthest(allPoints, startIndex, endIndex, epsilon);
	  if (nextIndex > 0) {
	    if (startIndex != nextIndex) {
	    	indexedRDP(startIndex, nextIndex, allPoints, rdpPoints, rdpIndices, epsilon);
	    }
	    rdpPoints.add(allPoints.get(nextIndex));
	    rdpIndices.add(nextIndex);
	    if (endIndex != nextIndex) {
	    	indexedRDP(nextIndex, endIndex, allPoints, rdpPoints, rdpIndices, epsilon);
	    }
	  }
	}

	/**
	 * @param points     a list of points to evaluate
	 * @param a          start index in points
	 * @param b          end index in points
	 * @param epsilon    expected distance between reduced points
	 * @return
	 */
	static int findFurthest(ArrayList<PVector> points, int a, int b, float epsilon) {
	  float recordDistance = -1;
	  PVector start = points.get(a);
	  PVector end = points.get(b);
	  int furthestIndex = -1;
	  for (int i = a+1; i < b; i++) {
	    PVector currentPoint = points.get(i);
	    float d = lineDist(currentPoint, start, end);
	    if (d > recordDistance) {
	      recordDistance = d;
	      furthestIndex = i;
	    }
	  }
	  if (recordDistance > epsilon) {
	    return furthestIndex;
	  } else {
	    return -1;
	  }
	}

	static float lineDist(PVector c, PVector a, PVector b) {
	  PVector norm = scalarProjection(c, a, b);
	  return PVector.dist(c, norm);
	}

	static PVector scalarProjection(PVector p, PVector a, PVector b) {
	  PVector ap = PVector.sub(p, a);
	  PVector ab = PVector.sub(b, a);
	  ab.normalize(); // Normalize the line
	  ab.mult(ap.dot(ab));
	  PVector normalPoint = PVector.add(a, ab);
	  return normalPoint;
	}

	/* ------------- END CODE FROM CODING TRAIN ------------- */
	

	/* ------------- SOME CODE PORTED FROM http://www.particleincell.com/2012/bezier-splines/ ------------- */
	/* 
	 * There's a handy mathematical explanation of the creation of a Bezier spline out at 
	 * http://www.particleincell.com/2012/bezier-splines/ 
	 * with some details at https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm
	 *
	 */

	/**
	 * Creates a Bezier curve from a supplied set of points. 
	 * 
	 * @param framePoints	An array of 2D PVector objects representing the points a Bezier curve will traverse
	 * @return				A Bezier curve path constructed of PABezShape objects
	 */
	public static PABezShape calculateCurve(ArrayList<PVector> framePoints) {
	  int n = framePoints.size();
	  float[] xCoords = new float[n];
	  float[] yCoords = new float[n];
	  int i = 0;
	  for (PVector vec : framePoints) {
	      xCoords[i] = vec.x;
	      yCoords[i] = vec.y;
	      i++;
	  }
	  float[] xp1 = new float[n-1];
	  float[] xp2 = new float[n-1];
	  computeControlPoints(xCoords, xp1, xp2);
	  float[] yp1 = new float[n-1];
	  float[] yp2 = new float[n-1];
	  computeControlPoints(yCoords, yp1, yp2);
	  PABezShape bez = new PABezShape(framePoints.get(0).x, framePoints.get(0).y, false);
	  for (int k = 0; k < n - 1; k++) {
	    bez.append(xp1[k], yp1[k], xp2[k], yp2[k], framePoints.get(k+1).x, framePoints.get(k+1).y);
	  }
	  return bez;
	}

	static void computeControlPoints(float[] K, float[] p1, float[] p2) {
	  int n = K.length - 1;
	  if (n <= 0) return;

	  /* rhs vector */
	  float[] a = new float[n];
	  float[] b = new float[n];
	  float[] c = new float[n];
	  float[] r = new float[n];
	  
	  /* leftmost segment */
	  a[0] = 0;
	  b[0] = 2;
	  c[0] = 1;
	  r[0] = K[0] + 2 * K[1];
	  
	  /* internal segments */
	  for (int i = 1; i < n - 1; i++) {
	    a[i] = 1;
	    b[i] = 4;
	    c[i] = 1;
	    r[i] = 4 * K[i] + 2 * K[i+1];
	  }
	      
	  /* rightmost segment */
	  a[n-1] = 2;
	  b[n-1] = 7;
	  c[n-1] = 0;
	  r[n-1] = 8 * K[n-1] + K[n];
	  
	  /* solves Ax = b with the Thomas algorithm, details at https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm */
	  for (int i = 1; i < n; i++) {
	    float m = a[i] / b[i-1];
	    b[i] = b[i] - m * c[i - 1];
	    r[i] = r[i] - m * r[i-1];
	  }
	 
	  p1[n-1] = r[n-1] / b[n-1];
	  for (int i = n - 2; i >= 0; --i) {
	    p1[i] = (r[i] - c[i] * p1[i+1]) / b[i];
	  }
	    
	  /* we have p1, now compute p2 */
	  for (int i = 0;i  <n-1; i++) {
	    p2[i] = 2 * K[i+1] - p1[i+1];
	  }
	  
	  p2[n-1] = 0.5f * (K[n]+p1[n-1]);
	}

	/* ------------- END CODE FROM http://www.particleincell.com/2012/bezier-splines/ ------------- */
	
	/* ------------- code for weighted Bezier path ------------- */


	/**
	 * Scales the position of the curve control points on a Bezier curve by a factor
	 * determined by the length of the line between the two anchor points and a bias
	 * value, such as PACurveUtility.LAMBDA.
	 * It's most useful when a short control line segment follows a long segment. 
	 * 
	 * @param bezPoints
	 * @param bias
	 */
	public static PABezShape calculateWeightedCurve(PABezShape bezPoints, float bias) {
	  PABezShape weightedBezPoints = bezPoints.clone();
	  ListIterator<PAVertex2DINF> it = weightedBezPoints.curveIterator();
	  float x1 = weightedBezPoints.startVertex().x();
	  float y1 = weightedBezPoints.startVertex().y();
	  PABezVertex bz;
	  while (it.hasNext()) {
	    PAVertex2DINF bez = it.next();
	    if (bez.segmentType() == PABezShape.CURVE_SEGMENT) {
	      bz = (PABezVertex) bez;
	      // lines from anchors to control point:
	      // (x1, y1), (bz.cx1(), bz.cy1())
	      // (bz.x(), bz.y()), (bz.cx2(), bz.cy2())
	      // distance between anchor points
	      float d = PApplet.dist(x1, y1, bz.x(), bz.y());
	      PVector cxy1 = weightedControlVec(x1, y1, bz.cx1(), bz.cy1(), bias, d);
	      bz.setCx1(cxy1.x);
	      bz.setCy1(cxy1.y);
	      PVector cxy2 = weightedControlVec(bz.x(), bz.y(), bz.cx2(), bz.cy2(), bias, d);
	      bz.setCx2(cxy2.x);
	      bz.setCy2(cxy2.y);
	      // store the first anchor point for the next iteration
	      x1 = bz.x();
	      y1 = bz.y();
	    }
	    else if (bez.segmentType() == PABezShape.LINE_SEGMENT) {
	      x1 = bez.x();
	      y1 = bez.y();
	    }
	    else {
	      // error! should never arrive here
	    }
	  }
	  return weightedBezPoints;
	}
	
	public static PABezShape calculateWeightedCurve(ArrayList<PVector> framePoints, float bias) {
		PABezShape curve = PACurveUtility.calculateCurve(framePoints);
		return PACurveUtility.calculateWeightedCurve(curve, bias);
	}
	
	/**
	 * Scales the position of the curve control points on a Bezier curve by a factor
	 * determined by the length of the line between the two anchor points and a bias
	 * value, such as PACurveUtility.LAMBDA (the default, in this method).
	 * It's most useful when a short control line segment follows a long segment. 
	 * 
	 * @param bezPoints		a Bezier path encapsulated in a PABezShape instance
	 */
	public static void calculateWeightedCurve(PABezShape bezPoints) {
		calculateWeightedCurve(bezPoints, PABezShape.LAMBDA);
	}


	public static PVector weightedControlVec(float ax, float ay, float cx, float cy, float w, float d) {
	  // divide the weighted distance between anchor points by the distance from anchor point to control point
	  float t = w * d * 1/(PApplet.dist(ax, ay, cx, cy));
	  // plug into parametric line equation
	  float x = ax + (cx - ax) * t;
	  float y = ay + (cy - ay) * t;
	  return new PVector(x, y);
	}

	// ------------- CODE FOR BRUSH SHAPE ------------- //

	/**
	 * Simulates a brush stroke as a PABezShape object by creating two Bezier curves offset on either side
	 * of a supplied list of 2D PVector objects and joining them into a closed shape with pointed ends.
	 * 
	 * @param points			an ArrayList of 2D PVector objects
	 * @param brushWidth		width of the brush in pixels
	 * @param isDrawWeighted	create a weighted version of the PABezShape shape
	 * @param bias				adjusts distances between control points and anchor points
	 * @return
	 */
	public static PABezShape quickBrushShape(ArrayList<PVector> points, float brushWidth, boolean isDrawWeighted, float bias) {
	  ArrayList<PVector> pointsLeft = new ArrayList<PVector>();
	  ArrayList<PVector> pointsRight = new ArrayList<PVector>();
	  if (!(points.size() > 0)) return null;
	  // handle the first point
	  PVector v1 = points.get(0);
	  pointsLeft.add(v1.copy());
	  pointsRight.add(v1.copy());
	  for (int i = 1; i < points.size() - 1; i++) {
	    PVector v2 = points.get(i);
	    PVector v3 = points.get(i+1);
	    // get the normals to the lines (v1, v2) and (v2, v3) at the point v2
	    PVector norm1 = normalAtPoint(v1, v2, 1, 1);
	    PVector norm2 = normalAtPoint(v2, v3, 0, 1);
	    // add the normals together and take the average
	    norm1.add(norm2).mult(0.5f);
	    // normalize (probably not necessary, eh?)
	    norm1.sub(v2).normalize();
	    // add points on either side of v2 at distance brushWidth/2
	    pointsLeft.add(scaledNormalAtPoint(v2, norm1, -brushWidth/2));
	    pointsRight.add(scaledNormalAtPoint(v2, norm1, brushWidth/2));
	    v1 = v2;    // if v2 is the last point, v1 will store it when we exit the loop
	  }
	  // handle the last point
	  v1 = points.get(points.size() -1);
	  pointsLeft.add(v1.copy());
	  pointsRight.add(v1.copy());
	  // reverse one of the arrays
	  reverseArray(pointsLeft, 0, pointsLeft.size() - 1);
	  // generate two Bezier splines
	  PABezShape bezLeft = calculateCurve(pointsLeft);
	  PABezShape bezRight = calculateCurve(pointsRight);
	  if (isDrawWeighted ) {
	    bezLeft = calculateWeightedCurve(bezLeft, bias);
	    bezRight = calculateWeightedCurve(bezRight, bias);
	  }
	  // append points in bezLeft to bezRight
	  ListIterator<PAVertex2DINF> it = bezLeft.curveIterator();
	  while (it.hasNext()) {
	    bezRight.append(it.next().clone());
	  }
	  bezRight.setIsClosed(true);
	  // return the brushstroke shape in bezRight
	  return bezRight;
	}
	
	public static PABezShape quickBrushShape(ArrayList<PVector> points, float brushWidth) {
		return quickBrushShape(points, brushWidth, false, 0);
	}
	
	public static PABezShape quickBrushShape(ArrayList<PVector> points, float brushWidth, float bias) {
		return quickBrushShape(points, brushWidth, true, bias);
	}
	
	
	/**
	 * Calculates a normal to a line at a point on the line at parametric distance u, normalized if d = 1.
	 * 
	 * @param a1	2D PVector, first point
	 * @param a2	2D PVector, second point
	 * @param u		parametric distance along the line from a1 to a2
	 * @param d		distance of the normal from the line a1, a2
	 * @return		Normal to line through a1 and a2
	 */
	public static PVector normalAtPoint(PVector a1, PVector a2, float u, float d) {
	  float ax1 = a1.x;
	  float ay1 = a1.y;
	  float ax2 = a2.x;
	  float ay2 = a2.y;
	  // determine the proportions of change on x and y axes for the line segment 
	  float f = (ax2 - ax1);
	  float g = (ay2 - ay1);
	  // get the point on the line segment at u
	  float pux = ax1 + f * u;
	  float puy = ay1 + g * u;
	  // prepare to calculate normalized parametric equations
	  float root = PApplet.sqrt(f*f + g*g);
	  float inv = 1/root;
	  // plug distance d into normalized parametric equations for normal to line segment
	  float x = pux - g * inv * d;
	  float y = puy + f * inv * d;
	  return new PVector(x, y);
	}

	/**
	 * @param anchor	2D PVector, the point from which the normal extends
	 * @param normal	2D PVector, normal to anchor
	 * @param d			distance for scaling the normal, distance from anchor to returned PVector
	 * @return			2D PVector, the normal scaled by d
	 */
	public static PVector scaledNormalAtPoint(PVector anchor, PVector normal, float d) {
	  return new PVector(anchor.x + normal.x * d, anchor.y + normal.y * d);
	}

	/**
	 * Reverses the order of a portion of an ArrayList of PVector objects, 
	 * between indices l and r, inclusive.
	 * 
	 * @param arr	an ArrayList of PVector objects
	 * @param l		index at which to start reversing order of elements
	 * @param r		index at which to stop reversing order of elements
	 */
	public static void reverseArray(ArrayList<PVector> arr, int l, int r) {
	  PVector temp;
	  while (l < r) {
	    temp = arr.get(l);
	    arr.set(l, arr.get(r));
	    arr.set(r, temp);
	    l++;
	    r--;
	  }
	}	
	
	
	
	/************************************************
	 *                                              *
	 * ----------- GESTURE SCHEDULING ------------  *
	 *                                              *
	 ************************************************/

	
	/**
	 * Schedules times along a gestural curve, typically stored as a PACurveMaker instance. A calling sequence might be:
	 * <code>
	 *   PABezShape curve = curveMaker.getCurve();
	 *   float rdpTimes = intsToFloats(curveMaker.getReducedTimes());
	 *   GestureSchedule curveSchedule = PACurveUtility.buildScheduleFromBezShape(curve, cfg.bezierCurveSteps, rdpTimes, true);
	 * </code>
	 * 
	 * @param shape
	 * @param steps
	 * @param anchorTimesMs
	 * @param arcLengthTime
	 * @return
	 */
	public static GestureSchedule buildScheduleFromBezShape(PABezShape shape, 
			int steps, float[] anchorTimesMs, boolean arcLengthTime) {
		// there is one more anchor than the number of segments
		int segCount = shape.size();
		// System.out.println("-- segCount = "+ segCount);
		// System.out.println("-- anchorTimesMs.length = "+ anchorTimesMs.length);
		int anchorCount = segCount + 1;
		if (anchorTimesMs == null || anchorTimesMs.length != anchorCount) {
			throw new IllegalArgumentException(
				    "anchorTimesMs length (" + anchorTimesMs.length +
				    ") does not match curve anchors (" + anchorCount +
				    "). This usually means epsilon changed and a cached curve is stale."
				);
		}
		
		// Count points (same counting rule as getPointList)
		int ct = 1;
		ListIterator<PAVertex2DINF> it = shape.curveIterator();
		while (it.hasNext()) {
			PAVertex2DINF vt = it.next();
			if (vt.segmentType() == PABezShape.CURVE_SEGMENT) ct += Math.max(1, steps);
			else if (vt.segmentType() == PABezShape.LINE_SEGMENT) ct += 1;
		}

		ArrayList<PVector> pts = new ArrayList<>(ct);
		float[] times = new float[ct];
		int idx = 0;
		float currentX = shape.x();
		float currentY = shape.y();
		pts.add(new PVector(currentX, currentY));
		times[idx++] = anchorTimesMs[0];
		it = shape.curveIterator();
		int anchorK = 0;
		int safeSteps = Math.max(1, steps);

		while (it.hasNext()) {
			PAVertex2DINF vt = it.next();
			int segType = vt.segmentType();

			float t0 = anchorTimesMs[anchorK];
			float t1 = anchorTimesMs[anchorK + 1];

			if (segType == PABezShape.CURVE_SEGMENT) {
				float[] knots = vt.coords();
				float cx1 = knots[0], cy1 = knots[1];
				float cx2 = knots[2], cy2 = knots[3];
				float ax  = knots[4], ay  = knots[5];
				
				if (!arcLengthTime) {
					for (int j = 1; j <= safeSteps; j++) {
						float u = j / (float) safeSteps;
						float x = shape.bezierPoint(currentX, cx1, cx2, ax, u);
						float y = shape.bezierPoint(currentY, cy1, cy2, ay, u);
						pts.add(new PVector(x, y));
						times[idx++] = lerp(t0, t1, u);
					}
				} 
				
				else { // arcLengthTime == true
					// sample points
					float[] xs = new float[safeSteps + 1];
					float[] ys = new float[safeSteps + 1];
					xs[0] = currentX; ys[0] = currentY;
					for (int j = 1; j <= safeSteps; j++) {
						float u = j / (float) safeSteps;
						xs[j] = shape.bezierPoint(currentX, cx1, cx2, ax, u);
						ys[j] = shape.bezierPoint(currentY, cy1, cy2, ay, u);
					}
					// cumulative distance
					float[] cum = new float[safeSteps + 1];
					cum[0] = 0;
					float total = 0;
					for (int j = 1; j <= safeSteps; j++) {
						float dx = xs[j] - xs[j - 1];
						float dy = ys[j] - ys[j - 1];
						total += (float) Math.sqrt(dx * dx + dy * dy);
						cum[j] = total;
					}
					if (total < 1e-6f) total = 1e-6f;
					for (int j = 1; j <= safeSteps; j++) {
						float frac = cum[j] / total;
						pts.add(new PVector(xs[j], ys[j]));
						times[idx++] = lerp(t0, t1, frac);
					}
				}
				currentX = ax;
				currentY = ay;
			} 
			
			else if (segType == PABezShape.LINE_SEGMENT) {
				float nx = vt.x();
				float ny = vt.y();
				pts.add(new PVector(nx, ny));
				times[idx++] = t1;
				currentX = nx;
				currentY = ny;
			}
			anchorK++;
		}

		// Defensive trim if counts drift
		if (idx != ct) {
			while (pts.size() > idx) pts.remove(pts.size() - 1);
			float[] trimmed = new float[idx];
			System.arraycopy(times, 0, trimmed, 0, idx);
			times = trimmed;
		}

		return new GestureSchedule(pts, times);
	}

	
	/************************************************
	 *                                              *
	 * ---------------- UTILITY ------------------  *
	 *                                              *
	 ************************************************/
	
	private static float lerp(float a, float b, float u) {
		return a + u * (b - a);
	}
	
    /**
     * @param src    an array of int
     * @return array src converted to float values
     */
    public static float[] intsToFloats(int[] src) {
        if (src == null) return null;
        float[] out = new float[src.length];
        for (int i = 0; i < src.length; i++) out[i] = src[i];
        return out;
    }
    
    /**
     * @param src          an array of int
     * @param targetLen    desired length of array of float to consruct
     * @return             an array of targetLen float values derived from src 
     */
    public static float[] intsToFloats(int[] src, int targetLen) {
        float[] out = new float[Math.max(0, targetLen)];
        if (src == null || src.length == 0 || out.length == 0) return out;
        int n = Math.min(src.length, out.length);
        for (int i = 0; i < n; i++) out[i] = src[i];
        // if src shorter, extend flat (or you can extrapolate)
        for (int i = n; i < out.length; i++) out[i] = out[n - 1];
        return out;
    }
    
	/**
	 * @param points    a List of PVector
	 * @return length of the path between the points
	 */
	public static float pathLength(List<PVector> points) {
		float pathLength = 0f;
		for (int i = 1; i < points.size(); i++) {
			PVector a = points.get(i - 1);
			PVector b = points.get(i);
			float dx = b.x - a.x, dy = b.y - a.y;
			pathLength += (float)Math.sqrt(dx*dx + dy*dy);
		}
		return pathLength;
	}

	
	/************************************************
	 *                                              *
	 * ---------- GEOMETRIC TRANSFORMS -----------  *
	 *                                              *
	 ************************************************/

	/**
	 * Calculates the boundary rectangle of a list of points and returns it as an array of floats.
	 * @return	[xMin, yMin, xMax, yMax]
	 */
	public static float[] bounds(ArrayList<PVector> points) {
	    if (points == null || points.isEmpty()) return null;
		PVector vec = points.get(0);
		float xMin = vec.x;
		float yMin = vec.y;
		float xMax = xMin;
		float yMax = yMin;
		for (int i = 1; i < points.size(); i++) {
			vec = points.get(i);
			float x = vec.x;
			float y = vec.y;
			if (x < xMin) xMin = x;
			if (y < yMin) yMin = y;
			if (x > xMax) xMax = x;
			if (y > yMax) yMax = y;
		}
		return new float[] {xMin, yMin, xMax, yMax};
	} 
	
	public static PVector getBoundsCenter(ArrayList<PVector> points) {
		float[] b = bounds(points);
		return new PVector((b[0] + b[2]) * 0.5f, (b[1] + b[3]) * 0.5f);	
	}

	/**
	 * Translates a point by xOffset and yOffset, returns a new point.
	 * 
	 * @param x         x coordinate of point
	 * @param y         y coordinate of point
	 * @param xOffset   distance to translate on x-xis
	 * @param yOffset   distance to translate on y-axis
	 * @return          a new translated point
	 */
	public static PVector translateCoord(float x, float y, float xOffset, float yOffset) {
		return new PVector(x + xOffset, y + yOffset);
	}

	/**
	 * Scales a point by xScale and yScale around a point (xctr, yctr), returns a new point.
	 * 
	 * @param x        x coordinate of point
	 * @param y        y coordinate of point
	 * @param xScale   scaling on x-axis
	 * @param yScale   scaling on y-axis
	 * @param xctr     x coordinate of center of transformation
	 * @param yctr     y coordinate of center of transformation
	 * @return         a new scaled point as a PVector
	 */
	public static PVector scaleCoordAroundPoint(float x, float y, float xScale, float yScale, float xctr, float yctr) {
		float xout = xctr + (x - xctr) * xScale;
		float yout = yctr + (y - yctr) * yScale;
		return new PVector(xout, yout);
	}
	
	/**
	 * Rotates a point theta radians around a point (xctr, yctr), returns a new point.
	 * rotation is counterclockwise for positive theta in Cartesian system, 
	 * clockwise in screen display coordinate system
	 * 
	 * @param x       x coordinate of point
	 * @param y       y coordinate of point
	 * @param xctr    x coordinate of center of rotation
	 * @param yctr    y coordinate of center of rotation
	 * @param theta   angle to rotate, in radians
	 * @return        a new rotated point
	 */
	public static PVector rotateCoordAroundPoint(float x, float y, float xctr, float yctr, float theta) {
		double sintheta = Math.sin(theta);
		double costheta = Math.cos(theta);
		PVector pt = translateCoord(x, y, -xctr, -yctr);
		pt.set((float)(pt.x * costheta - pt.y * sintheta), (float)(pt.x * sintheta + pt.y * costheta));
		return translateCoord(pt.x, pt.y, xctr, yctr);
	}
		
	public static PVector flipHorizontal(float x, float y, float xctr) {
	    return new PVector(xctr - (x - xctr), y);
	}

	public static PVector flipVertical(float x, float y, float yctr) {
	    return new PVector(x, yctr - (y - yctr));
	}


	// ------------- Some basic geometric transforms on an ArrayList<PVector> ------------- //

	/**
	 * Translates points in x and y directions, in place.
	 * 
	 * @param points    list of points to translate
	 * @param xTrans    translation in x direction
	 * @param yTrans    translation in y direction
	 */
	public void translatePoints(ArrayList<PVector> points, float xTrans, float yTrans) {
		ListIterator<PVector> it = points.listIterator();
		while (it.hasNext()) {
			PVector pt = it.next();
			pt.set(pt.x + xTrans, pt.y + yTrans);
		}
	}
	
	/**
	 * Scales points around a given point, in place. 
	 * 
	 * @param points    list of points to scale
	 * @param xScale    scaling on x-axis
	 * @param yScale    scaling on y-axis
	 * @param x0        x-coordinate of the center point
	 * @param y0        y-coordinate of the center point
	 * TODO for xScale or yScale very near 0, insure correct scaling.
	 */
	public void scalePoints(ArrayList<PVector> points, float xScale, float yScale, float x0, float y0) {
		ListIterator<PVector> it = points.listIterator();
		while (it.hasNext()) {
			PVector pt = it.next();
			pt.set(x0 + (pt.x - x0) * xScale, y0 + (pt.y - y0) * yScale);
		}
	}
		
	/**
	 * Rotates this shape around a supplied center point, in place.
	 * 
	 * @param points    list of points to rotate
	 * @param xctr      x-coordinate of the center point
	 * @param yctr      y-coordinate of the center point
	 * @param theta     degrees to rotate (in radians)
	 * TODO for theta very near PI, 0, or TWO_PI, insure correct rotation.  
	 */
	public void rotatePoints(ArrayList<PVector> points, float xctr, float yctr, float theta) {
		// PVector pt = rotateCoordAroundPoint(this.x(), this.y(), xctr, yctr, theta);
		ListIterator<PVector> it = points.listIterator();
		while (it.hasNext()) {
			PVector pt = it.next();
			float x = (float)(xctr + (pt.x - xctr) * Math.cos(theta) - (pt.y - yctr) * Math.sin(theta));
			float y = (float)(yctr + (pt.x - xctr) * Math.sin(theta) + (pt.y - yctr) * Math.cos(theta));
			pt.set(x, y);
		}
	}
	
	/**
	 * Translates points in x and y directions, returning a new list.
	 * 
	 * @param points   the points to be translated
	 * @param xTrans   translation in x direction
	 * @param yTrans   translation in y direction
	 * @return the translated points
	 */
	public static ArrayList<PVector> translatePointsCopy(ArrayList<PVector> points, float xTrans, float yTrans) {
		ArrayList<PVector> translatedPoints = new ArrayList<PVector>(points.size());
		ListIterator<PVector> it = points.listIterator();
		while (it.hasNext()) {
			PVector pt = it.next();
			translatedPoints.add(new PVector(pt.x + xTrans, pt.y + yTrans));
		}
		return translatedPoints;
	}
	
	/**
	 * Scales points around a given point, returning a new list. 
	 * 
	 * @param points   the points to be scaled
	 * @param xScale   scaling on x-axis
	 * @param yScale   scaling on y-axis
	 * @param x0       center of scaling on x-axis
	 * @param y0       center of scaling on y-axis
	 * @return the scaled points
	 * TODO for xScale or yScale very near 0, insure correct scaling.
	 */
	public static ArrayList<PVector> scalePointsCopy(ArrayList<PVector> points, float xScale, float yScale, float x0, float y0) {
		ArrayList<PVector> scaledPoints = new ArrayList<PVector>(points.size());
		ListIterator<PVector> it = points.listIterator();
		while (it.hasNext()) {
			PVector pt = it.next();
			scaledPoints.add(new PVector(x0 + (pt.x - x0) * xScale, y0 + (pt.y - y0) * yScale));
		}
		return scaledPoints;
	}
		
	/**
	 * Rotates this shape around a supplied center point, returning a new list.
	 * 
	 * @param points   the points to be rotated
	 * @param xctr     center of rotation on x-axis
	 * @param yctr     center of rotation on y-axis
	 * @param theta    degrees to rotate (in radians)
	 * TODO for theta very near PI, 0, or TWO_PI, insure correct rotation.  
	 */
	public static ArrayList<PVector> rotatePointsCopy(ArrayList<PVector> points, float xctr, float yctr, float theta) {
		// PVector pt = rotateCoordAroundPoint(this.x(), this.y(), xctr, yctr, theta);
		ArrayList<PVector> rotatedPoints = new ArrayList<PVector>(points.size());
		ListIterator<PVector> it = points.listIterator();
		while (it.hasNext()) {
			PVector pt = it.next();
			float x = (float)(xctr + (pt.x - xctr) * Math.cos(theta) - (pt.y - yctr) * Math.sin(theta));
			float y = (float)(yctr + (pt.x - xctr) * Math.sin(theta) + (pt.y - yctr) * Math.cos(theta));
			rotatedPoints.add(new PVector(x, y));
		}
		return rotatedPoints;
	}

	// ------------- Composite transform ------------- //
	
	/**
	 * Applies flip/scale, then rotation, then translation to a point.
	 * Flip/scale and rotation are performed around (xctr, yctr).
	 *
	 * @param x               source x
	 * @param y               source y
	 * @param xctr            pivot x
	 * @param yctr            pivot y
	 * @param scaleX          scale in x
	 * @param scaleY          scale in y
	 * @param flipHorizontal  mirror left/right around pivot
	 * @param flipVertical    mirror up/down around pivot
	 * @param theta           rotation in radians
	 * @param xTrans          translation in x
	 * @param yTrans          translation in y
	 * @return transformed point
	 */
	public static PVector transformCoord(
	        float x, float y,
	        float xctr, float yctr,
	        float scaleX, float scaleY,
	        boolean flipHorizontal,
	        boolean flipVertical,
	        float theta,
	        float xTrans, float yTrans) {

	    float sx = flipHorizontal ? -scaleX : scaleX;
	    float sy = flipVertical ? -scaleY : scaleY;

	    float xs = xctr + (x - xctr) * sx;
	    float ys = yctr + (y - yctr) * sy;

	    double sinTheta = Math.sin(theta);
	    double cosTheta = Math.cos(theta);

	    float dx = xs - xctr;
	    float dy = ys - yctr;

	    float xr = (float)(xctr + dx * cosTheta - dy * sinTheta);
	    float yr = (float)(yctr + dx * sinTheta + dy * cosTheta);

	    return new PVector(xr + xTrans, yr + yTrans);
	}
	
	/**
	 * Applies flip/scale, then rotation, then translation to a point.
	 * Flip/scale and rotation are performed around (xctr, yctr).
	 * Returns a new <code>ArrayList<PVector></code>.
	 * 
	 * @param points
	 * @param xctr
	 * @param yctr
	 * @param scaleX
	 * @param scaleY
	 * @param flipHorizontal
	 * @param flipVertical
	 * @param theta
	 * @param xTrans
	 * @param yTrans
	 * @return
	 */
	public static ArrayList<PVector> transformPointsCopy(
	        ArrayList<PVector> points,
	        float xctr, float yctr,
	        float scaleX, float scaleY,
	        boolean flipHorizontal,
	        boolean flipVertical,
	        float theta,
	        float xTrans, float yTrans) {

	    ArrayList<PVector> out = new ArrayList<>(points.size());
	    for (PVector pt : points) {
	        out.add(transformCoord(
	                pt.x, pt.y,
	                xctr, yctr,
	                scaleX, scaleY,
	                flipHorizontal, flipVertical,
	                theta,
	                xTrans, yTrans));
	    }
	    return out;
	}
	
	/**
	 * Applies flip/scale, then rotation, then translation to a point.
	 * Flip/scale and rotation are performed around (xctr, yctr).
	 * Performs the transform in place.
	 * 
	 * @param points
	 * @param xctr
	 * @param yctr
	 * @param scaleX
	 * @param scaleY
	 * @param flipHorizontal
	 * @param flipVertical
	 * @param theta
	 * @param xTrans
	 * @param yTrans
	 */
	public static void transformPoints(
	        ArrayList<PVector> points,
	        float xctr, float yctr,
	        float scaleX, float scaleY,
	        boolean flipHorizontal,
	        boolean flipVertical,
	        float theta,
	        float xTrans, float yTrans) {

	    float sx = flipHorizontal ? -scaleX : scaleX;
	    float sy = flipVertical ? -scaleY : scaleY;

	    double sinTheta = Math.sin(theta);
	    double cosTheta = Math.cos(theta);

	    ListIterator<PVector> it = points.listIterator();
	    while (it.hasNext()) {
	        PVector pt = it.next();

	        float xs = xctr + (pt.x - xctr) * sx;
	        float ys = yctr + (pt.y - yctr) * sy;

	        float dx = xs - xctr;
	        float dy = ys - yctr;

	        float xr = (float)(xctr + dx * cosTheta - dy * sinTheta);
	        float yr = (float)(yctr + dx * sinTheta + dy * cosTheta);

	        pt.set(xr + xTrans, yr + yTrans);
	    }
	}
	
	/**
	 * Applies a GestureTransformState to a PACurveMaker using the curve's live dragPoints.
	 *
	 * Uses state.restPoints if available; otherwise uses a copy of curve.copyAllPoints()
	 * taken at call time.
	 *
	 * Transform order:
	 *   1) flip/scale about pivot
	 *   2) rotate about pivot
	 *   3) translate
	 *   
	 * @param curve    PACurveMaker instance
	 * @param state    GestureTransformState instance with transform parameters
	 */
	public static void applyTransformToCurve(PACurveMaker curve, GestureTransformState state) {
	    if (curve == null || state == null) return;

	    ArrayList<PVector> src = state.hasRestPoints() ? state.restPoints : curve.copyAllPoints();
	    if (src == null || src.isEmpty()) return;

	    PVector ctr = (state.pivot != null) ? state.pivot : getBoundsCenter(src);
	    if (ctr == null) return;

	    final float xctr = ctr.x;
	    final float yctr = ctr.y;
	    final float sx = state.flipHorizontal ? -state.scaleX : state.scaleX;
	    final float sy = state.flipVertical ? -state.scaleY : state.scaleY;
	    final float theta = state.rotation;
	    final float tx = state.translateX;
	    final float ty = state.translateY;

	    final double sinTheta = Math.sin(theta);
	    final double cosTheta = Math.cos(theta);

	    curve.mutateAllPoints(pts -> {
	        int n = Math.min(pts.size(), src.size());
	        for (int i = 0; i < n; i++) {
	            PVector s = src.get(i);
	            PVector d = pts.get(i);

	            float xs = xctr + (s.x - xctr) * sx;
	            float ys = yctr + (s.y - yctr) * sy;

	            float dx = xs - xctr;
	            float dy = ys - yctr;

	            float xr = (float)(xctr + dx * cosTheta - dy * sinTheta);
	            float yr = (float)(yctr + dx * sinTheta + dy * cosTheta);

	            d.set(xr + tx, yr + ty);
	        }
	    });
	}
	
	/**
	 * Restores curve points from state.restPoints, if present.
	 */
	public static void restoreCurveRestPoints(PACurveMaker curve, GestureTransformState state) {
	    if (curve == null || state == null || !state.hasRestPoints()) return;

	    final ArrayList<PVector> src = state.restPoints;
	    curve.mutateAllPoints(pts -> {
	        int n = Math.min(pts.size(), src.size());
	        for (int i = 0; i < n; i++) {
	            pts.get(i).set(src.get(i));
	        }
	    });
	}
	
	
	/************************************************
	 *                                              *
	 * ------------- DRAWING METHODS -------------  *
	 *                                              *
	 ************************************************/
	
	
	/**
	 * Draws a line in a PApplet context using the PVector data in points and supplied color and weight.
	 * 
	 * @param parent		a PApplet where drawing takes place on screen
	 * @param points		ArrayList of 2D PVector objects, typically a 
	 * 						point set without successive identical points
	 * @param lineColor		color for the line that is drawn
	 * @param lineWeight	weight for the line that is drawn
	 */
	public static void lineDraw(PApplet parent, ArrayList<PVector> points, int lineColor, float lineWeight) {
		if (points.size() > 1) {
			parent.stroke(lineColor);
			parent.strokeWeight(lineWeight);
			parent.noFill();
			parent.beginShape();
			for (PVector vec : points) {
				parent.vertex(vec.x, vec.y);
			}
			parent.endShape();
		}
	}

	/**
	 * Draws a line in a PGraphics using the 2D PVector data and supplied color and weight.
	 * 
	 * @param pg			a PGraphics where drawing takes place off screen
	 * @param points		ArrayList of 2D PVector objects, typically a 
	 * 						point set without successive identical points
	 * @param lineColor		color for the line that is drawn
	 * @param lineWeight	weight for the line that is drawn
	 */
	public static void lineDraw(PGraphics pg, ArrayList<PVector> points, int lineColor, float lineWeight) {
		if (points.size() > 1) {
			pg.stroke(lineColor);
			pg.strokeWeight(lineWeight);
			pg.noFill();
			pg.beginShape();
			for (PVector vec : points) {
				pg.vertex(vec.x, vec.y);
			}
			pg.endShape();
		}
	}
	
	/**
	 * Draws a Bezier path in a PApplet using 2D curve data and supplied stroke color and weight. 
	 * The path is drawn with no fill.
	 * 
	 * @param parent
	 * @param curve
	 * @param curveColor
	 * @param curveWeight
	 */
	public static void curveDraw(PApplet parent, PABezShape curve, int curveColor, float curveWeight) {
		if (null != curve && curve.size() > 0) {
			parent.pushStyle();
			parent.stroke(curveColor);
			parent.strokeWeight(curveWeight);
			parent.noFill();
			curve.drawQuick(parent);
			parent.popStyle();
		}
	}

	/**
	 * Draws a Bezier path in a PApplet using 2D curve data and the local stroke color and weight 
	 * of a PABezShape. If the PABezShape is filled, the fill will be drawn.
	 * 
	 * @param parent
	 * @param curve
	 */
	public static void curveDraw(PApplet parent, PABezShape curve) {
		if (null != curve && curve.size() > 0) {
            curve.draw(parent);
        }
	}

	/**
	 * Draws a Bezier path in a PGraphics using 2D curve data and supplied stroke color and weight.
	 * The path is drawn with no fill.
	 * 
	 * @param pg
	 * @param curve
	 * @param curveColor
	 * @param curveWeight
	 */
	public static void curveDraw(PGraphics pg, PABezShape curve, int curveColor, float curveWeight) {
		if (null != curve && curve.size() > 0) {
			pg.pushStyle();
			pg.stroke(curveColor);
			pg.strokeWeight(curveWeight);
			pg.noFill();
			curve.drawQuick(pg);
			pg.popStyle();
		}
	}

	/**
	 * Draws a Bezier path in a PGraphics using 2D curve data and local stroke color and weight 
	 * of a PABezShape. If the PABezShape is filled, the fill will be drawn.
	 * 
	 * @param pg
	 * @param curve
	 */
	public static void curveDraw(PGraphics pg, PABezShape curve) {
		if (null != curve && curve.size() > 0) {
            curve.draw(pg);
        }
	}

	/**
	 * Draws a PABezShape in a PApplet using supplied fill, stroke, and weight. If weight == 0,
	 * the shape is drawn with no stroke.
	 * 
	 * @param parent
	 * @param shape
	 * @param shapeFill
	 * @param shapeStroke
	 * @param shapeWeight
	 */
	public static void shapeDraw(PApplet parent, PABezShape shape, int shapeFill, int shapeStroke, float shapeWeight) {
	    if (null != shape && shape.size() > 0) {
	        parent.pushStyle();
	        parent.stroke(shapeStroke);
	        parent.strokeWeight(shapeWeight);
	        if (shapeWeight == 0) parent.noStroke();
	        else parent.fill(shapeStroke);
	        shape.drawQuick(parent);
	        parent.popStyle();
	    }
	}

	/**
	 * Draws a PABezShape in a PApplet using local fill, stroke, and weight of the shape.
	 * 
	 * @param parent
	 * @param shape
	 */
	public static void shapeDraw(PApplet parent, PABezShape shape) {
		if (null != shape && shape.size() > 0) {
			shape.draw(parent);
		}
	}
	
	/**
	 * Draws a PABezShape in a PGraphics using supplied fill, stroke, and weight. If weight == 0,
	 * the shape is drawn with no stroke.
	 * 
	 * @param pg
	 * @param shape
	 * @param shapeFill
	 * @param shapeStroke
	 * @param shapeWeight
	 */
	public static void shapeDraw(PGraphics pg, PABezShape shape, int shapeFill, int shapeStroke, float shapeWeight) {
	    if (null != shape && shape.size() > 0) {
	        pg.pushStyle();
	        pg.stroke(shapeStroke);
	        pg.strokeWeight(shapeWeight);
	        if (shapeWeight == 0) pg.noFill();
	        else pg.fill(shapeStroke);
	        shape.drawQuick(pg);
	        pg.popStyle();
	    }
	}

	/**
	 * Draws a PABezShape in a PGraphics using local fill, stroke, and weight of the shape.
	 * 
	 * @param pg
	 * @param shape
	 */
	public static void shapeDraw(PGraphics pg, PABezShape shape) {
		if (null != shape && shape.size() > 0) {
			shape.draw(pg);
		}
	}

	/**
	 * Draws circular points in a PApplet context using the PVector data and supplied color and diameter.
	 * 
	 * @param parent		a PApplet where drawing takes place on screen
	 * @param points		ArrayList of 2D PVector objects, typically a 
	 * 						point set without successive identical points
	 * @param pointColor	color for the circle that is drawn
	 * @param diameter	    diameter of circle that is drawn
	 */
	public static void pointsDraw(PApplet parent, ArrayList<PVector> points, int pointColor, float diameter) {
		if (points.size() > 1) {
			parent.pushStyle();
			parent.noStroke();
			parent.fill(pointColor);
			for (PVector vec : points) {
				parent.circle(vec.x, vec.y, diameter);
			}
			parent.popStyle();
		}
	}

	/**
	 * Draws circular points in a PGraphics context using the PVector data and supplied color and diameter.
	 * 
	 * @param pg			a PGraphics where drawing takes place off screen
	 * @param points		ArrayList of 2D PVector objects, typically a 
	 * 						point set without successive identical points
	 * @param pointColor	color for the circle that is drawn
	 * @param diameter	    diameter of circle that is drawn
	 */
	public static void pointsDraw(PGraphics pg, ArrayList<PVector> points, int pointColor, float diameter) {
		if (points.size() > 1) {
			pg.pushStyle();
			pg.noStroke();
			pg.fill(pointColor);
			for (PVector vec : points) {
				pg.circle(vec.x, vec.y, diameter);
			}
			pg.popStyle();
		}
	}

	
}
