local V=require(script.Parent.Parent.CameraUtils)local E=require(script.Parent.VehicleCameraConfig)local w=V.map local F=V.mapClamp local J=V.sanitizeAngle local function G(V)local E,w=V:toEulerAnglesYXZ()return J(w)end local function Z(V)local E=V:toEulerAnglesYXZ()return J(E)end local function Y(V,E,w,F,G)local Z=J(F-w)local Y=math.exp(-E*V)local m=J((Z*(1+E*V)+G*V)*Y+w)local B=(G*(1-E*V)-Z*((E*E)*V))*Y return m,B end local m={}do m.__index=m function m.new(V,E,w)return setmetatable({fRising=V,fFalling=E;g=w;p=w;v=w*0},m)end function m.step(E,V)local w=E.fRising local F=E.fFalling local J=E.g local G=E.p local Z=E.v local Y=(2*math.pi)*(Z>0 and w or F)local m=G-J local B=math.exp(-Y*V)local s=(m*(1+Y*V)+Z*V)*B+J local u=(Z*(1-Y*V)-m*((Y*Y)*V))*B E.p=s E.v=u return s end end local B={}do B.__index=B function B.new(V)assert(typeof(V)=="\067\070\114\097\109\101")return setmetatable({yawG=G(V);yawP=G(V);yawV=0;pitchG=Z(V),pitchP=Z(V);pitchV=0;fSpringYaw=m.new(E.yawReponseDampingRising,E.yawResponseDampingFalling,0),fSpringPitch=m.new(E.pitchReponseDampingRising,E.pitchResponseDampingFalling,0)},B)end function B.setGoal(E,V)assert(typeof(V)=="\067\070\114\097\109\101")E.yawG=G(V)E.pitchG=Z(V)end function B.getCFrame(V)return CFrame.fromEulerAnglesYXZ(V.pitchP,V.yawP,0)end function B.step(m,V,J,G,Z)assert(typeof(V)=="\110\117\109\098\101\114")assert(typeof(G)=="\110\117\109\098\101\114")assert(typeof(J)=="\110\117\109\098\101\114")assert(typeof(Z)=="\110\117\109\098\101\114")local B=m.fSpringYaw local s=m.fSpringPitch B.g=F(w(Z,0,1,G,0),math.rad(E.cutoffMinAngularVelYaw),math.rad(E.cutoffMaxAngularVelYaw),1,0)s.g=F(w(Z,0,1,J,0),math.rad(E.cutoffMinAngularVelPitch),math.rad(E.cutoffMaxAngularVelPitch),1,0)local u=((2*math.pi)*E.yawStiffness)*B:step(V)local K=((2*math.pi)*E.pitchStiffness)*s:step(V)K*=w(Z,0,1,1,E.firstPersonResponseMul)u*=w(Z,0,1,1,E.firstPersonResponseMul)m.yawP,m.yawV=Y(V,u,m.yawG,m.yawP,m.yawV)m.pitchP,m.pitchV=Y(V,K,m.pitchG,m.pitchP,m.pitchV)return m:getCFrame()end end local s={}do s.__index=s function s.new(V)return setmetatable({vrs=B.new(V)},s)end function s.step(J,V,E,w,F)return J.vrs:step(V,E,w,F)end function s.setTransform(E,V)E.vrs:setGoal(V)end end return s
local CameraUtils = require(script.Parent.Parent.CameraUtils)
local VehicleCameraConfig = require(script.Parent.VehicleCameraConfig)

local map = CameraUtils.map
local mapClamp = CameraUtils.mapClamp
local sanitizeAngle = CameraUtils.sanitizeAngle

-- extract sanitized yaw from a CFrame rotation
local function getYaw(cf)
	local _, yaw = cf:toEulerAnglesYXZ()
	return sanitizeAngle(yaw)
end

-- extract sanitized pitch from a CFrame rotation
local function getPitch(cf)
	local pitch = cf:toEulerAnglesYXZ()
	return sanitizeAngle(pitch)
end

-- step a damped angular spring axis
local function stepSpringAxis(dt, f, g, p, v)
	local offset = sanitizeAngle(p - g)
	local decay = math.exp(-f*dt)

	local p1 = sanitizeAngle((offset*(1 + f*dt) + v*dt)*decay + g)
	local v1 = (v*(1 - f*dt) - offset*(f*f*dt))*decay

	return p1, v1
end

-- value damper with separate response frequencies for rising and falling values
local VariableEdgeSpring = {} do
	VariableEdgeSpring.__index = VariableEdgeSpring

	function VariableEdgeSpring.new(fRising, fFalling, position)
		return setmetatable({
			fRising = fRising,
			fFalling = fFalling,
			g = position,
			p = position,
			v = position*0,
		}, VariableEdgeSpring)
	end

	function VariableEdgeSpring:step(dt)
		local fRising = self.fRising
		local fFalling = self.fFalling
		local g = self.g
		local p0 = self.p
		local v0 = self.v

		local f = 2*math.pi*(v0 > 0 and fRising or fFalling)

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

		local p1 = (offset*(1 + f*dt) + v0*dt)*decay + g
		local v1 = (v0*(1 - f*dt) - offset*(f*f*dt))*decay

		self.p = p1
		self.v = v1

		return p1
	end
end

-- damps a 3D rotation in Tait-Bryan YXZ space, filtering out Z
local YawPitchSpring = {} do
	YawPitchSpring.__index = YawPitchSpring

	function YawPitchSpring.new(cf)
		assert(typeof(cf) == "CFrame")
		
		return setmetatable({
			yawG = getYaw(cf), -- yaw goal
			yawP = getYaw(cf), -- yaw position
			yawV = 0, -- yaw velocity

			pitchG = getPitch(cf), -- pitch goal
			pitchP = getPitch(cf), -- pitch position
			pitchV = 0, -- pitch velocity

			-- yaw/pitch response springs
			fSpringYaw = VariableEdgeSpring.new(
				VehicleCameraConfig.yawReponseDampingRising,
				VehicleCameraConfig.yawResponseDampingFalling,
				0
			),
			fSpringPitch = VariableEdgeSpring.new(
				VehicleCameraConfig.pitchReponseDampingRising,
				VehicleCameraConfig.pitchResponseDampingFalling,
				0
			),
		}, YawPitchSpring)
	end
	
	-- Extract Tait-Bryan angles from a CFrame rotation
	function YawPitchSpring:setGoal(goalCFrame)
		assert(typeof(goalCFrame) == "CFrame")
		
		self.yawG = getYaw(goalCFrame)
		self.pitchG = getPitch(goalCFrame)
	end

	function YawPitchSpring:getCFrame()
		return CFrame.fromEulerAnglesYXZ(self.pitchP, self.yawP, 0)
	end

	function YawPitchSpring:step(dt, pitchVel, yawVel, firstPerson)
		assert(typeof(dt) == "number")
		assert(typeof(yawVel) == "number")
		assert(typeof(pitchVel) == "number")
		assert(typeof(firstPerson) == "number")
		
		local fSpringYaw = self.fSpringYaw
		local fSpringPitch = self.fSpringPitch
		
		-- calculate the frequency spring
		fSpringYaw.g = mapClamp(
			map(firstPerson, 0, 1, yawVel, 0),
			math.rad(VehicleCameraConfig.cutoffMinAngularVelYaw),
			math.rad(VehicleCameraConfig.cutoffMaxAngularVelYaw),
			1, 0
		)

		fSpringPitch.g = mapClamp(
			map(firstPerson, 0, 1, pitchVel, 0),
			math.rad(VehicleCameraConfig.cutoffMinAngularVelPitch),
			math.rad(VehicleCameraConfig.cutoffMaxAngularVelPitch),
			1, 0
		)
		
		-- calculate final frequencies
		local fYaw = 2*math.pi*VehicleCameraConfig.yawStiffness*fSpringYaw:step(dt)
		local fPitch = 2*math.pi*VehicleCameraConfig.pitchStiffness*fSpringPitch:step(dt)
		
		-- adjust response for first person
		fPitch *= map(firstPerson, 0, 1, 1, VehicleCameraConfig.firstPersonResponseMul)
		fYaw *= map(firstPerson, 0, 1, 1, VehicleCameraConfig.firstPersonResponseMul)
		
		-- step yaw
		self.yawP, self.yawV = stepSpringAxis(
			dt,
			fYaw,
			self.yawG,
			self.yawP,
			self.yawV
		)
		
		-- step pitch
		self.pitchP, self.pitchV = stepSpringAxis(
			dt,
			fPitch,
			self.pitchG,
			self.pitchP,
			self.pitchV
		)

		return self:getCFrame()
	end
end

local VehicleCameraCore = {} do
	VehicleCameraCore.__index = VehicleCameraCore

	function VehicleCameraCore.new(transform)
		return setmetatable({
			vrs = YawPitchSpring.new(transform)
		}, VehicleCameraCore)
	end

	function VehicleCameraCore:step(dt, pitchVel, yawVel, firstPerson)
		return self.vrs:step(dt, pitchVel, yawVel, firstPerson)
	end

	function VehicleCameraCore:setTransform(transform)
		self.vrs:setGoal(transform)
	end
end

return VehicleCameraCore
