-- 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
