content: package thebetweenlands.util;
  
  import net.minecraft.util.math.Vec3d;
  
  public class ReparameterizedSpline implements ISpline {
  	private final ISpline spline;
  
  	private double length;
  
  	private CatmullRomSpline reparameterizedInterpol;
  
  	private double[] segmentLengths;
  
  	private int segmentSubdivs;
  	private double epsilon = 1e-3;
  	private int maxNewtonIter = 16;
  
  	public ReparameterizedSpline(ISpline spline) {
  		this.spline = spline;
  	}
  
  	public ReparameterizedSpline init(int subdivs, int segmentSubdivs, double epsilon, int maxNewtonIter) {
  		this.init(subdivs, segmentSubdivs);
  		this.epsilon = epsilon;
  		this.maxNewtonIter = maxNewtonIter;
  		return this;
  	}
  
  	/**
  	 * Initializes the reparameterized spline
  	 * @param subdivs The number of total subdivisions to sample velocity
  	 * @param segmentSubdivs The number of segment subdivisions to approximate length
  	 */
  	public ReparameterizedSpline init(int subdivs, int segmentSubdivs) {
  		this.segmentSubdivs = segmentSubdivs;
  
  		this.segmentLengths = new double[this.spline.getNumSegments() + 1];
  		this.segmentLengths[0] = 0.0D;
  		double segmentDelta = 1.0D / this.spline.getNumSegments();
  		for(int i = 1; i <= this.spline.getNumSegments(); i++) {
  			this.segmentLengths[i] = this.segmentLengths[i - 1] + this.getArcLength(segmentDelta * (i - 1), segmentDelta * i, subdivs);
  		}
  		this.length = this.segmentLengths[this.spline.getNumSegments()];
  
  		Vec3d[] arcLengths = new Vec3d[subdivs + 2];
  		double delta = this.length / (subdivs - 1);
  		for(int i = 0; i < subdivs; i++) {
  			arcLengths[i+1] = new Vec3d(this.getCurveParameter(delta * i), 0.0D, 0.0D);
  		}
  		arcLengths[0] = arcLengths[subdivs + 1] = new Vec3d(0, 0, 0.000001F);
  		//TODO Use simple lerp?
  		this.reparameterizedInterpol = new CatmullRomSpline(arcLengths);
  		return this;
  	}
  
  	@Override
  	public Vec3d interpolate(float t) {
  		return this.spline.interpolate(this.getInterpolatedCurveParameter(t * this.length));
  	}
  
  	@Override
  	public Vec3d derivative(float t) {
  		return this.spline.derivative(this.getInterpolatedCurveParameter(t * this.length));
  	}
  
  	@Override
  	public Vec3d[] getNodes() {
  		return this.spline.getNodes();
  	}
  
  	@Override
  	public int getNumSegments() {
  		return this.spline.getNumSegments();
  	}
  
  	private float getInterpolatedCurveParameter(double s) {
  		float parameter;
  		if(s <= 0.0D) {
  			parameter = 0.0F;
  		} else if(s >= this.length) {
  			parameter = 1.0F;
  		} else {
  			parameter = (float)this.reparameterizedInterpol.interpolate((float)(s / this.length)).x;
  		}
  		return parameter;
  	}
  
  	/**
  	 * Approximates the arc length of the spline by subdividing it
  	 * @param t
  	 * @return
  	 */
  	private double getArcLength(double start, double end, int subdivs) {
  		double sum = 0.0D;
  		double delta = (end - start) / subdivs;
  		Vec3d prev = null;
  		for(int i = 0; i <= subdivs; i++) {
  			Vec3d interp = this.spline.interpolate((float)(start + (i * delta)));
  			if(prev != null) {
  				sum += prev.subtract(interp).length();
  			}
  			prev = interp;
  		}
  		return sum;
  	}
  
  	private double speed(double t) {
  		return this.spline.derivative((float) t).length();
  	}
  
  	/**
  	 * Approximates the interpolation parameter to get to the specified length of the spline
  	 * https://www.geometrictools.com/Documentation/MovingAlongCurveSpecifiedSpeed.pdf
  	 * @param s
  	 * @return
  	 */
  	private double getCurveParameter(double s) {
  		if(s <= 0.0D) {
  			return 0.0D;
  		} else if(s >= this.length) {
  			return 1.0D;
  		}
  
  		//The segment we're currently in
  		int segment;
  		for(segment = 1; segment < this.spline.getNumSegments(); segment++) {
  			if(s <= this.segmentLengths[segment]) {
  				break;
  			}
  		}
  
  		double segmentDelta = 1.0D / this.spline.getNumSegments();
  
  		double t = (s - this.segmentLengths[segment - 1]) / this.length;
  
  		double segmentT = segmentDelta * (segment - 1);
  
  		double lower = 0.0D;
  		double upper = segmentDelta;
  
  		for(int i = 0; i < this.maxNewtonIter; i++) {
  			double error = this.segmentLengths[segment - 1] + this.getArcLength(segmentT, segmentT + t, this.segmentSubdivs) - s;
  
  			if(Math.abs(error) < this.epsilon) {
  				return segmentT + t;
  			}
  
  			//Use Newton's method to find a better approximation
  			double tCandidate = t - error / this.speed(segmentT + t);
  
  			if(error > 0) {
  				upper = t;
  
  				if(tCandidate <= lower) {
  					t = 0.5D * (upper + lower);
  				} else {
  					t = tCandidate;
  				}
  			} else {
  				lower = t;
  
  				if(tCandidate >= upper) {
  					t = 0.5D * (upper + lower);
  				} else {
  					t = tCandidate;
  				}
  			}
  		}
  
  		return segmentT + t;
  	}
  	
  	@Override
  	public boolean hasArcLength() {
  		return true;
  	}
  
  	@Override
  	public double getArcLength() {
  		return this.length;
  	}
  }
  