package Interpolation
import NoWurst
import Vectors
import Wurstunit

// Credits to BlinkyBoy
/** Look at linear */
public function linear(vec2 start, vec2 stop, real p) returns vec2
	return vec2(linear(start.x, stop.x, p), linear(start.y, stop.y, p))

/**
	Linear Interpolation
	Sample Usage:
	AddSpecialEffect(TEST_SFX,linear(P1X,P2X,p),linear(P1Y,P2Y,p))
*/
public function linear(real start, real stop, real p) returns real
	return start + (stop - start) * p

public function vec3.bezier3(vec3 mid, vec3 stop, real p) returns vec3
	return vec3(bezier3(this.x, mid.x, stop.x, p), bezier3(this.y, mid.y, stop.y, p), bezier3(this.z, mid.z, stop.z, p))

public function vec2.bezier3(vec2 mid, vec2 stop, real p) returns vec2
	return vec2(bezier3(this.x, mid.x, stop.x, p), bezier3(this.y, mid.y, stop.y, p))

public function vec3.bezier4(vec3 mid1, vec3 mid2, vec3 stop, real p) returns vec3
	return vec3(bezier4(this.x, mid1.x, mid2.x, stop.x, p), bezier4(this.y, mid1.y, mid2.y, stop.y, p), bezier4(this.z, mid1.z, mid2.z, stop.z, p))

public function vec2.bezier4(vec2 mid1, vec2 mid2, vec2 stop, real p) returns vec2
	return vec2(bezier4(this.x, mid1.x, mid2.x, stop.x, p), bezier4(this.y, mid1.y, mid2.y, stop.y, p))

public function vec3.lerp(vec3 target, real alpha) returns vec3
	return vec3(this.x.lerp(target.x, alpha), this.y.lerp(target.y, alpha), this.z.lerp(target.z, alpha))

public function vec2.lerp(vec2 target, real alpha) returns vec2
	return vec2(this.x.lerp(target.x, alpha), this.y.lerp(target.y, alpha))

/**
	3-Point-Bezier Interpolation
	Sample Usage:
	AddSpecialEffect(TEST_SFX, bezier3(P1X, P3X, P2X, p), bezier3(P1Y, P3Y, P2Y, p))
*/
public function bezier3(real start, real mid, real stop, real p) returns real
	return start + 2. * (mid - start) * p + (stop - 2. * mid + start) * p * p

/**
	This function returns the derivate of the 3-Point-Bezier curve on certain point.
	(d(bezier3())/dt)
*/
public function derivBezier3(real start, real mid, real stop, real p) returns real
	return 2. * (mid - start) + 2. * (stop - 2. * mid + start) * p

public function vec3.derivBezier3(vec3 mid, vec3 stop, real p) returns vec3
	return vec3(derivBezier3(this.x, mid.x, stop.x, p), derivBezier3(this.y, mid.y, stop.y, p), derivBezier3(this.z, mid.z, stop.z, p))

public function vec2.derivBezier3(vec2 mid, vec2 stop, real p) returns vec2
	return vec2(derivBezier3(this.x, mid.x, stop.x, p), derivBezier3(this.y, mid.y, stop.y, p))

/**
	4-Point-Bezier Interpolation
	Sample Usage:
	AddSpecialEffect(TEST_SFX,bezier4(P1X,P3X,P4X,P2X,t),bezier4(P1Y,P3Y,P4Y,P2Y,t))
*/
public function bezier4(real start, real mid1, real mid2, real stop, real p) returns real
	let m = p * p
	return start + 3. * p * (mid1 - start) + 3. * m * (mid2 - 2. * mid1 + start) + m * p * (3. * (mid1 - mid2) + stop - start)

/**
	This function returns the derivate of the 4-Point-Bezier curve on certain point.
	(d(bezier4())/dt)
*/
public function derivBezier4(real start, real mid1, real mid2, real stop, real p) returns real
	return 3. * (mid1 - start) + 6. * (mid2 - 2. * mid1 + start) * p + 3. * (3. * (mid1 - mid2) + stop - start) * p * p

public function vec3.derivBezier4(vec3 mid1, vec3 mid2, vec3 stop, real p) returns vec3
	return vec3(derivBezier4(this.x, mid1.x, mid2.x, stop.x, p), derivBezier4(this.y, mid1.y, mid2.y, stop.y, p), derivBezier4(this.z, mid1.z, mid2.z, stop.z, p))

public function vec2.derivBezier4(vec2 mid1, vec2 mid2, vec2 stop, real p) returns vec2
	return vec2(derivBezier4(this.x, mid1.x, mid2.x, stop.x, p), derivBezier4(this.y, mid1.y, mid2.y, stop.y, p))

// You can use Bezier 3 to graphic perfect parabolas
// bezier4(<Start Point>, <OutTan_of_StartPoint>, <InTan_of_EndPoint>, <EndPoint>)

/** Hermite Interpolation */
public function hermite(real start, real stop, real tangent1, real tangent2, real s) returns real
	let s2 = s * s
	var h1 = 8.0 * s * s2 - 9.0 * s2
	let h2 = h1 * -1
	let h3 = s2 * (s - 2.0) + s
	let h4 = s2 * (s - 1.0)
	h1++
	return h1 * start + h2 * stop + h3 * tangent1 + h4 * tangent2

// TCB functions for finding the tangents among 3 points
// Most modellers may know TCB from 3dsmax, in which they are used to make rotations
// look very smooth.
// p = tension, c = continuity, b = bias
// tension is how sharply the curve makes turns.
// continuity specifies the rate of change between speed and direction.
// bias specifies the direction of the curve.
// sidenote: specify p1 as 0, when working with 2 points.

/** Returns the intan of p2 */
public function inTan(real p1, real p2, real p3, real p, real c, real b) returns real
	return (1.0 - p) * (1.0 - c) * (1.0 + b) * (p2 - p1) / 2. + (1.0 - p) * (1.0 + c) * (1.0 - b) * (p3 - p2) / 2.

/** Returns the outtan of p2 */
public function outTan(real p1, real p2, real p3, real p, real c, real b) returns real
	return (1.0 - p) * (1.0 + c) * (1.0 + b) * (p2 - p1) / 2. + (1.0 - p) * (1.0 - c) * (1.0 - b) * (p3 - p2) / 2.

@Test
function linearVecTest()
	let v = linear(vec2(3, 4), vec2(6, 2), 0.5)
	v.x.assertEquals(4.5)
	v.y.assertEquals(3)

@Test function testVec2Bezier3()
	let start = vec2(-1, 0)
	let mid = vec2(0, 2)
	let stop = vec2(1, 0)
	let p = 0.75
	let expected = vec2(0.5, 0.75)
	start.bezier3(mid, stop, p).assertEquals(expected)
	start.bezier3(mid, stop, 0).assertEquals(start)
	start.bezier3(mid, stop, 1).assertEquals(stop)

@Test function testVec2DerivBezier3()
	let start = vec2(-1, 0)
	let mid = vec2(0, 2)
	let stop = vec2(1, 0)
	let p = 0.75
	let expected = vec2(2, -2)
	start.derivBezier3(mid, stop, p).assertEquals(expected)

@Test function testVec2Bezier4()
	let start = vec2(-3, 0)
	let mid1 = vec2(-2, 2)
	let mid2 = vec2(0, 0)
	let stop = vec2(1, 1)
	let p = 0.75
	let expected = vec2(0.09375, 0.703125)
	start.bezier4(mid1, mid2, stop, p).assertEquals(expected)
	start.bezier4(mid1, mid2, stop, 0).assertEquals(start)
	start.bezier4(mid1, mid2, stop, 1).assertEquals(stop)

@Test function testVec2DerivBezier4()
	let start = vec2(-3, 0)
	let mid1 = vec2(-2, 2)
	let mid2 = vec2(0, 0)
	let stop = vec2(1, 1)
	let p = 0.75
	let expected = vec2(4.125, -0.1875)
	start.derivBezier4(mid1, mid2, stop, p).assertEquals(expected)

@Test function testVec3Bezier3()
	let start = vec3(-1, 0, -2)
	let mid = vec3(0, 2, 0.5)
	let stop = vec3(1, 0, 3)
	let p = 0.75
	let expected = vec3(0.5, 0.75, 1.75)
	start.bezier3(mid, stop, p).assertEquals(expected)
	start.bezier3(mid, stop, 0).assertEquals(start)
	start.bezier3(mid, stop, 1).assertEquals(stop)

@Test function testVec3DerivBezier3()
	let start = vec3(-1, 0, -2)
	let mid = vec3(0, 2, 0.5)
	let stop = vec3(1, 0, 3)
	let p = 0.75
	let expected = vec3(2, -2, 5)
	start.derivBezier3(mid, stop, p).assertEquals(expected)

@Test function testVec3Bezier4()
	let start = vec3(-3, 0, 0)
	let mid1 = vec3(-2, 2, -3)
	let mid2 = vec3(0, 0, 0)
	let stop = vec3(1, 1, 2)
	let p = 0.75
	let expected = vec3(0.09375, 0.703125, 0.421875)
	start.bezier4(mid1, mid2, stop, p).assertEquals(expected)
	start.bezier4(mid1, mid2, stop, 0).assertEquals(start)
	start.bezier4(mid1, mid2, stop, 1).assertEquals(stop)

@Test function testVec3DerivBezier4()
	let start = vec3(-3, 0, 0)
	let mid1 = vec3(-2, 2, -3)
	let mid2 = vec3(0, 0, 0)
	let stop = vec3(1, 1, 2)
	let p = 0.75
	let expected = vec3(4.125, -0.1875, 6.1875)
	start.derivBezier4(mid1, mid2, stop, p).assertEquals(expected)