-- spring for an array of linear values

local LinearSpring = {} do
	LinearSpring.__index = LinearSpring

	function LinearSpring.new(dampingRatio, frequency, pos, typedat, rawTarget)
		local linearPos = typedat.toIntermediate(pos)
		return setmetatable(
			{
				d = dampingRatio,
				f = frequency,
				g = linearPos,
				p = linearPos,
				v = table.create(#linearPos, 0),
				typedat = typedat,
				rawTarget = rawTarget,
			},
			LinearSpring
		)
	end

	function LinearSpring:setGoal(goal)
		self.rawTarget = goal
		self.g = self.typedat.toIntermediate(goal)
	end

	function LinearSpring:canSleep()
		if magnitudeSq(self.v) > SLEEP_VELOCITY_SQ_LIMIT then
			return false
		end

		if distanceSq(self.p, self.g) > SLEEP_OFFSET_SQ_LIMIT then
			return false
		end

		return true
	end

	function LinearSpring:step(dt)
		-- Advance the spring simulation by dt seconds.
		-- Take the damped harmonic oscillator ODE:
		--    f^2*(X[t] - g) + 2*d*f*X'[t] + X''[t] = 0
		-- Where X[t] is position at time t, g is target position,
		-- f is undamped angular frequency, and d is damping ratio.
		-- Apply constant initial conditions:
		--    X[0] = p0
		--    X'[0] = v0
		-- Solve the IVP to get analytic expressions for X[t] and X'[t].
		-- The solution takes one of three forms for 0<=d<1, d=1, and d>1

		local d = self.d
		local f = self.f*2*pi -- Hz -> Rad/s
		local g = self.g
		local p = self.p
		local v = self.v

		if d == 1 then -- critically damped
			local q = exp(-f*dt)
			local w = dt*q

			local c0 = q + w*f
			local c2 = q - w*f
			local c3 = w*f*f

			for idx = 1, #p do
				local o = p[idx] - g[idx]
				p[idx] = o*c0 + v[idx]*w + g[idx]
				v[idx] = v[idx]*c2 - o*c3
			end

		elseif d < 1 then -- underdamped
			local q = exp(-d*f*dt)
			local c = sqrt(1 - d*d)

			local i = cos(dt*f*c)
			local j = sin(dt*f*c)

			-- Damping ratios approaching 1 can cause division by very small numbers.
			-- To mitigate that, group terms around z=j/c and find an approximation for z.
			-- Start with the definition of z:
			--    z = sin(dt*f*c)/c
			-- Substitute a=dt*f:
			--    z = sin(a*c)/c
			-- Take the Maclaurin expansion of z with respect to c:
			--    z = a - (a^3*c^2)/6 + (a^5*c^4)/120 + O(c^6)
			--    z ≈ a - (a^3*c^2)/6 + (a^5*c^4)/120
			-- Rewrite in Horner form:
			--    z ≈ a + ((a*a)*(c*c)*(c*c)/20 - c*c)*(a*a*a)/6

			local z
			if c > EPS then
				z = j/c
			else
				local a = dt*f
				z = a + ((a*a)*(c*c)*(c*c)/20 - c*c)*(a*a*a)/6
			end

			-- Frequencies approaching 0 present a similar problem.
			-- We want an approximation for y as f approaches 0, where:
			--    y = sin(dt*f*c)/(f*c)
			-- Substitute b=dt*c:
			--    y = sin(b*c)/b
			-- Now reapply the process from z.

			local y
			if f*c > EPS then
				y = j/(f*c)
			else
				local b = f*c
				y = dt + ((dt*dt)*(b*b)*(b*b)/20 - b*b)*(dt*dt*dt)/6
			end

			for idx = 1, #p do
				local o = p[idx] - g[idx]
				p[idx] = (o*(i + z*d) + v[idx]*y)*q + g[idx]
				v[idx] = (v[idx]*(i - z*d) - o*(z*f))*q
			end

		else -- overdamped
			local c = sqrt(d*d - 1)

			local r1 = -f*(d - c)
			local r2 = -f*(d + c)

			local ec1 = exp(r1*dt)
			local ec2 = exp(r2*dt)

			for idx = 1, #p do
				local o = p[idx] - g[idx]
				local co2 = (v[idx] - o*r1)/(2*f*c)
				local co1 = ec1*(o - co2)

				p[idx] = co1 + co2*ec2 + g[idx]
				v[idx] = co1*r1 + co2*ec2*r2
			end
		end

		return self.typedat.fromIntermediate(self.p)
	end
end

local function inverseGammaCorrectD65(c)
	return c < 0.0404482362771076 and c/12.92 or 0.87941546140213*(c + 0.055)^2.4
end

local function gammaCorrectD65(c)
	return c < 3.1306684425e-3 and 12.92*c or 1.055*c^(1/2.4) - 0.055
end

local function rgbToLuv(value)
	-- convert RGB to a variant of cieluv space
	local r, g, b = value.R, value.G, value.B

	-- D65 sRGB inverse gamma correction
	r = inverseGammaCorrectD65(r)
	g = inverseGammaCorrectD65(g)
	b = inverseGammaCorrectD65(b)

	-- sRGB -> xyz
	local x = 0.9257063972951867*r - 0.8333736323779866*g - 0.09209820666085898*b
	local y = 0.2125862307855956*r + 0.71517030370341085*g + 0.0722004986433362*b
	local z = 3.6590806972265883*r + 11.4426895800574232*g + 4.1149915024264843*b

	-- xyz -> scaled cieluv
	local l = y > 0.008856451679035631 and 116*y^(1/3) - 16 or 903.296296296296*y

	local u, v
	if z > 1e-14 then
		u = l*x/z
		v = l*(9*y/z - 0.46832)
	else
		u = -0.19783*l
		v = -0.46832*l
	end

	return {l, u, v}
end

local function luvToRgb(value)
	-- convert back from modified cieluv to rgb space

	local l = value[1]
	if l < 0.0197955 then
		return Color3.new(0, 0, 0)
	end
	local u = value[2]/l + 0.19783
	local v = value[3]/l + 0.46832

	-- cieluv -> xyz
	local y = (l + 16)/116
	y = y > 0.206896551724137931 and y*y*y or 0.12841854934601665*y - 0.01771290335807126
	local x = y*u/v
	local z = y*((3 - 0.75*u)/v - 5)

	-- xyz -> D65 sRGB
	local r =  7.2914074*x - 1.5372080*y - 0.4986286*z
	local g = -2.1800940*x + 1.8757561*y + 0.0415175*z
	local b =  0.1253477*x - 0.2040211*y + 1.0569959*z

	-- clamp minimum sRGB component
	if r < 0 and r < g and r < b then
		r, g, b = 0, g - r, b - r
	elseif g < 0 and g < b then
		r, g, b = r - g, 0, b - g
	elseif b < 0 then
		r, g, b = r - b, g - b, 0
	end

	-- gamma correction from D65
	-- clamp to avoid undesirable overflow wrapping behavior on certain properties (e.g. BasePart.Color)
	return Color3.new(
		min(gammaCorrectD65(r), 1),
		min(gammaCorrectD65(g), 1),
		min(gammaCorrectD65(b), 1)
	)
end
