package me.bauer.BauerCam.Interpolation;

import java.util.List;

import me.bauer.BauerCam.Path.Position;

public final class CubicInterpolator extends Interpolator {

	public static final IInterpolatorFactory factory = new IInterpolatorFactory() {
		@Override
		public Interpolator getInterpolator(final List<Position> points) {
			return new CubicInterpolator(points);
		}
	};

	public CubicInterpolator(final List<Position> points) {
		super(points);
	}

	@Override
	protected Position interpolate(final int actualLength, final int section1, final int section2, final double step) {
		int section0 = section1 - 1;
		int section3 = section2 + 1;

		// Bounding the outer nodes inside the array if necessary
		// I could extrapolate these points, but this is a quick and acceptable
		// solution
		if (section0 < 0) {
			section0 = 0;
		}
		if (section3 > actualLength) {
			section3 = actualLength;
		}

		final Position y0 = this.points[section0];
		final Position y1 = this.points[section1];
		final Position y2 = this.points[section2];
		final Position y3 = this.points[section3];

		return new Position(interpolate(y0.x, y1.x, y2.x, y3.x, step), interpolate(y0.y, y1.y, y2.y, y3.y, step),
				interpolate(y0.z, y1.z, y2.z, y3.z, step),
				(float) interpolate(y0.pitch, y1.pitch, y2.pitch, y3.pitch, step),
				(float) interpolate(y0.yaw, y1.yaw, y2.yaw, y3.yaw, step),
				(float) interpolate(y0.roll, y1.roll, y2.roll, y3.roll, step));
	}

	/**
	 * Interpolates between y1 and y2. This is a cubic interpolation
	 *
	 * @param y0
	 * @param y1
	 * @param y2
	 * @param y3
	 * @param x
	 *            from 0 to 1
	 * @return
	 */
	private static double interpolate(final double y0, final double y1, final double y2, final double y3,
			final double x) {
		final double a = y3 - y2 - y0 + y1;
		final double b = y0 - y1 - a;
		final double c = y2 - y0;

		return ((a * x + b) * x + c) * x + y1;
	}

}

@@ -1109,24 +1109,37 @@
 (interpolate
+Sophisticated
 (y0.x, y1.x,
@@ -1157,17 +1157,21 @@
 , step),
- 
+%0A%09%09%09%09
 interpol
@@ -1169,24 +1169,37 @@
 %09interpolate
+Sophisticated
 (y0.y, y1.y,
@@ -1229,24 +1229,37 @@
 %09interpolate
+Sophisticated
 (y0.z, y1.z,
@@ -1278,32 +1278,24 @@
  step),%0A%09%09%09%09
-(float) 
 interpolate(
@@ -1289,24 +1289,30 @@
 %09interpolate
+Simple
 (y0.pitch, y
@@ -1339,16 +1339,24 @@
 3.pitch,
+ (float)
  step),%0A
@@ -1355,32 +1355,24 @@
  step),%0A%09%09%09%09
-(float) 
 interpolate(
@@ -1366,24 +1366,30 @@
 %09interpolate
+Simple
 (y0.yaw, y1.
@@ -1408,16 +1408,24 @@
  y3.yaw,
+ (float)
  step),%0A
@@ -1428,24 +1428,16 @@
 p),%0A%09%09%09%09
-(float) 
 interpol
@@ -1439,16 +1439,22 @@
 erpolate
+Simple
 (y0.roll
@@ -1481,16 +1481,24 @@
 y3.roll,
+ (float)
  step));
@@ -1572,16 +1572,51 @@
 polation
+ which%0A%09 * behaves like Catmull-Rom
 %0A%09 *%0A%09 *
@@ -1755,24 +1755,37 @@
  interpolate
+Sophisticated
 (final doubl
@@ -1885,25 +1885,50 @@
 a = 
-y3 - y2
+-0.5 * y0 + 1.5 * y1
  -
+ 1.5 *
  y
-0 + y1
+2 + 0.5 * y3
 ;%0A%09%09
@@ -1953,30 +1953,525 @@
 0 - 
-y1 - a;%0A%09%09final double
+2.5 * y1 + 2 * y2 - 0.5 * y3;%0A%09%09final double c = -0.5 * y0 + 0.5 * y2;%0A%0A%09%09return ((a * x + b) * x + c) * x + y1;%0A%09%7D%0A%0A%09/**%0A%09 * Interpolates between y1 and y2. This is a simple cubic interpolation%0A%09 *%0A%09 * @param y0%0A%09 * @param y1%0A%09 * @param y2%0A%09 * @param y3%0A%09 * @param x%0A%09 *            from 0 to 1%0A%09 * @return%0A%09 */%0A%09private static float interpolateSimple(final float y0, final float y1, final float y2, final float y3,%0A%09%09%09final float x) %7B%0A%09%09final float a = y3 - y2 - y0 + y1;%0A%09%09final float b = y0 - y1 - a;%0A%09%09final float
  c =

Add a new, more sophisticated cubic interpolation