-- Private Functions


local function arcLengthIntegrand(self, spline, t)
	local a, b, c, d = unpack(self.Coefficients)
	local dv = b[spline] + 2*c[spline]*t + 3*d[spline]*t*t
	return dv.Magnitude
end

local function integrate(self, spline, t)
	local h = t / SIMPSON_N
	local xi0, xi1, xi2 = arcLengthIntegrand(self, spline, t), 0, 0
	
	for i = 0, SIMPSON_N - 1 do
		local x = i*h
		if (i % 2 == 0) then
			xi2 = xi2 + arcLengthIntegrand(self, spline, x)
		else
			xi1 = xi1 + arcLengthIntegrand(self, spline, x)
		end
	end
	
	return h * (xi0 + 2*xi2 + 4*xi1) / 3
end

local function clampedCubicSpline(p, fpo, fpn)
	local n = #p
	local a, b, c, d = {unpack(p)}, {}, {}, {}
	
	local aa = {}
	aa[1] = 3*(a[2] - a[1]) - 3*fpo
	aa[n] = 3*fpn - 3*(a[n] - a[n-1])
	
	for i = 2, n - 1 do
		aa[i] = 3 * (a[i+1] - 2*a[i] + a[i-1])
	end
	
	local l, mu, z = {2}, {0.5}, {aa[1]/2}
	
	for i = 2, n - 1 do
		l[i] = 4 - mu[i-1]
		mu[i] = 1 / l[i]
		z[i] = (aa[i] - z[i-1]) / l[i]
	end
	
	l[n] = 2 - mu[n-1]
	z[n] = (aa[n] - z[n-1]) / l[n]
	c[n] = z[n]
	
	for j = n - 1, 1, -1 do
		c[j] = z[j] - mu[j]*c[j+1]
		d[j] = (c[j+1] - c[j]) / 3
		b[j] = p[j+1] - p[j] - c[j] - d[j]
	end
	
	return a, b, c, d
end

local function naturalCubicSpline(p)
	local n = #p
	local a, b, c, d = {unpack(p)}, {}, {}, {}
	
	local aa = {}
	
	for i = 2, n - 1 do
		aa[i] = 3 * (a[i+1] - 2*a[i] + a[i-1])
	end
	
	local start = Vector3.new(0, 10, 0)
	local l, mu, z = {1}, {0}, {ZERO}
	l[n], z[n], c[n] = 1, ZERO, ZERO

	for i = 2, n - 1 do
		l[i] = 4 - mu[i-1]
		mu[i] = 1 / l[i]
		z[i] = (aa[i] - z[i-1]) / l[i]
	end
	
	for j = n - 1, 1, -1 do
		c[j] = z[j] - mu[j]*c[j+1]
		d[j] = (c[j+1] - c[j]) / 3
		b[j] = p[j+1] - p[j] - c[j] - d[j]
	end
	
	return a, b, c, d
end
