local f=require(script.Parent.assign)local e=math.pi local t=math.abs local I=math.exp local S=math.sin local b=math.cos local a=math.sqrt local w=.001 local p=.01 local function Q(f,Q,P)local m=f.__dampingRatio local D=(f.__frequency*2)*e local u=f.__goalPosition local W=Q.value local l=Q.velocity or 0 local s=W-u local R=I((-P*m)*D)local J,y if m==1 then J=(l*P+s*(D*P+1))*R+u y=(l-(D*P)*(s*D+l))*R elseif m<1 then local f=a(1-m*m)local e=b((D*f)*P)local t=S((D*f)*P)local I if f>.0001 then I=t/f else local e=P*D I=e+(((((e*e)*(f*f))*(f*f))/20-f*f)*((e*e)*e))/6 end local w if D*f>.0001 then w=t/(D*f)else local e=D*f w=P+(((((P*P)*(e*e))*(e*e))/20-e*e)*((P*P)*P))/6 end J=(s*(e+m*I)+l*w)*R+u y=(l*(e-I*m)-s*(I*D))*R else local f=a(m*m-1)local e=-D*(m-f)local t=-D*(m+f)local S=(l-e*s)/((2*D)*f)local b=s-S local w=b*I(e*P)local p=S*I(t*P)J=(w+p)+u y=e*w+t*p end local V=t(J-f.__goalPosition)local r=t(y)local G=r<w and V<p if G then J=f.__goalPosition y=0 end return{value=J,velocity=y;complete=G}end local function P(e,t)assert(typeof(e)=="\110\117\109\098\101\114")local I={dampingRatio=1,frequency=1}if t~=nil then assert(typeof(t)=="\116\097\098\108\101")f(I,t)end local S=I.dampingRatio local b=I.frequency assert(typeof(S)=="\110\117\109\098\101\114")assert(typeof(b)=="\110\117\109\098\101\114")assert(S*b>=0,"\069\120\112\101\099\116\101\100 \100\097\109\112\105\110\103\082\097\116\105\111 \042 \102\114\101\113\117\101\110\099\121 \062\061 \048")local a={__dampingRatio=S;__frequency=b;__goalPosition=e;step=Q}return a end return P
--[[
	An analytical spring solution as a function of damping ratio and frequency.

	Adapted from
	https://gist.github.com/Fraktality/1033625223e13c01aa7144abe4aaf54d
]]

local assign = require(script.Parent.assign)

local pi = math.pi
local abs = math.abs
local exp = math.exp
local sin = math.sin
local cos = math.cos
local sqrt = math.sqrt

local RESTING_VELOCITY_LIMIT = 1e-3
local RESTING_POSITION_LIMIT = 1e-2

local function step(self, state, 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 desired position, f is 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 on one of three forms for d=1, d<1, and d>1

	local d = self.__dampingRatio
	local f = self.__frequency * 2 * pi -- Rad/s
	local g = self.__goalPosition

	local p0 = state.value
	local v0 = state.velocity or 0

	local offset = p0 - g
	local decay = exp(-dt*d*f)

	local p1, v1

	if d == 1 then -- Critically damped
		p1 = (v0*dt + offset*(f*dt + 1))*decay + g
		v1 = (v0 - f*dt*(offset*f + v0))*decay

	elseif d < 1 then -- Underdamped
		local c = sqrt(1 - d*d)

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

		-- Problem: Damping ratios close to 1 can cause numerical instability.
		-- Solution: Rearrange to group terms involving j/c, then find an approximation z for j/c.
		--    z = sin(dt*f*c)/c
		-- Substitute a for dt*f
		--    z = sin(a*c)/c
		-- Take the 5th-order series expansion of z at c = 0
		--    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 to mitigate precision issues
		--    z ≈ a + ((a*a)*(c*c)*(c*c)/20 - c*c)*(a*a*a)/6

		local z
		if c > 1e-4 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

		-- Repeat the process with a->dt and c->b=f*c for the f->0 case
		local y
		if f*c > 1e-4 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

		p1 = (offset*(i + d*z) + v0*y)*decay + g
		v1 = (v0*(i - z*d) - offset*(z*f))*decay

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

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

		local co2 = (v0 - r1*offset)/(2*f*c)
		local co1 = offset - co2

		local e1 = co1*exp(r1*dt)
		local e2 = co2*exp(r2*dt)

		p1 = e1 + e2 + g
		v1 = r1*e1 + r2*e2
	end

	local positionOffset = abs(p1 - self.__goalPosition)
	local velocityOffset = abs(v1)

	local complete = velocityOffset < RESTING_VELOCITY_LIMIT and positionOffset < RESTING_POSITION_LIMIT

	if complete then
		p1 = self.__goalPosition
		v1 = 0
	end

	return {
		value = p1,
		velocity = v1,
		complete = complete,
	}
end

local function spring(goalPosition, inputOptions)
	assert(typeof(goalPosition) == "number")

	local options = {
		dampingRatio = 1,
		frequency = 1,
	}

	if inputOptions ~= nil then
		assert(typeof(inputOptions) == "table")
		assign(options, inputOptions)
	end

	local dampingRatio = options.dampingRatio
	local frequency = options.frequency

	assert(typeof(dampingRatio) == "number")
	assert(typeof(frequency) == "number")

	assert(dampingRatio * frequency >= 0, "Expected dampingRatio * frequency >= 0")

	local self = {
		__dampingRatio = dampingRatio,
		__frequency = frequency, -- Hz
		__goalPosition = goalPosition,
		step = step,
	}

	return self
end

return spring
