-- functions

function stopAllAnimations()
	local oldAnim = currentAnim

	-- return to idle if finishing an emote
	if (emoteNames[oldAnim] ~= nil and emoteNames[oldAnim] == false) then
		oldAnim = "idle"
	end

	if currentlyPlayingEmote then
		oldAnim = "idle"
		currentlyPlayingEmote = false
	end

	currentAnim = ""
	currentAnimInstance = nil
	if (currentAnimKeyframeHandler ~= nil) then
		currentAnimKeyframeHandler:disconnect()
	end

	if (currentAnimTrack ~= nil) then
		currentAnimTrack:Stop()
		currentAnimTrack:Destroy()
		currentAnimTrack = nil
	end

	for _,v in pairs(locomotionMap) do
		if v.track then
			v.track:Stop()
			v.track:Destroy()
			v.track = nil
		end
	end

	return oldAnim
end

function getHeightScale()
	if Humanoid then
		if not Humanoid.AutomaticScalingEnabled then
			return 1
		end

		local scale = Humanoid.HipHeight / HumanoidHipHeight
		if AnimationSpeedDampeningObject == nil then
			AnimationSpeedDampeningObject = script:FindFirstChild("ScaleDampeningPercent")
		end
		if AnimationSpeedDampeningObject ~= nil then
			scale = 1 + (Humanoid.HipHeight - HumanoidHipHeight) * AnimationSpeedDampeningObject.Value / HumanoidHipHeight
		end
		return scale
	end
	return 1
end


local function signedAngle(a, b)
	return -math.atan2(a.x * b.y - a.y * b.x, a.x * b.x + a.y * b.y)
end

local angleWeight = 2.0
local function get2DWeight(px, p1, p2, sx, s1, s2)
	local avgLength = 0.5 * (s1 + s2)

	local p_1 = {x = (sx - s1)/avgLength, y = (angleWeight * signedAngle(p1, px))}
	local p12 = {x = (s2 - s1)/avgLength, y = (angleWeight * signedAngle(p1, p2))}	
	local denom = smallButNotZero + (p12.x*p12.x + p12.y*p12.y)
	local numer = p_1.x * p12.x + p_1.y * p12.y
	local r = math.clamp(1.0 - numer/denom, 0.0, 1.0)
	return r
end

local function blend2D(targetVelo, targetSpeed)
	local h = {}
	local sum = 0.0
	for n,v1 in pairs(locomotionMap) do
		if targetVelo.x * v1.lv.x < 0.0 or targetVelo.y * v1.lv.y < 0 then
			-- Require same quadrant as target
			h[n] = 0.0
			continue
		end
		h[n] = math.huge
		for j,v2 in pairs(locomotionMap) do
			if targetVelo.x * v2.lv.x < 0.0 or targetVelo.y * v2.lv.y < 0 then
				-- Require same quadrant as target
				continue
			end
			h[n] = math.min(h[n], get2DWeight(targetVelo, v1.lv, v2.lv, targetSpeed, v1.speed, v2.speed))
		end
		sum += h[n]
	end

	--truncates below 10% contribution
	local sum2 = 0.0
	local weightedVeloX = 0
	local weightedVeloY = 0
	for n,v in pairs(locomotionMap) do

		if (h[n] / sum > 0.1) then
			sum2 += h[n]
			weightedVeloX += h[n] * v.lv.x
			weightedVeloY += h[n] * v.lv.y
		else
			h[n] = 0.0
		end
	end
	local animSpeed
	local weightedSpeedSquared = weightedVeloX * weightedVeloX + weightedVeloY * weightedVeloY
	if weightedSpeedSquared > smallButNotZero then
		animSpeed = math.sqrt(targetSpeed * targetSpeed / weightedSpeedSquared)
	else
		animSpeed = 0
	end

	animSpeed = animSpeed / getHeightScale()
	local groupTimePosition = 0
	for n,v in pairs(locomotionMap) do
		if v.track.IsPlaying then
			groupTimePosition = v.track.TimePosition
			break
		end
	end
	for n,v in pairs(locomotionMap) do
		-- if not loco
		if h[n] > 0.0 then
			if not v.track.IsPlaying then 
				v.track:Play(runBlendtime)
				v.track.TimePosition = groupTimePosition
			end

			local weight = math.max(smallButNotZero, h[n] / sum2)
			v.track:AdjustWeight(weight, runBlendtime)
			v.track:AdjustSpeed(animSpeed)
		else
			v.track:Stop(runBlendtime)
		end
	end

end

local function getWalkDirection()
	local walkToPoint = Humanoid.WalkToPoint
	local walkToPart = Humanoid.WalkToPart
	if Humanoid.MoveDirection ~= Vector3.zero then
		return Humanoid.MoveDirection
	elseif walkToPart or walkToPoint ~= Vector3.zero then
		local destination
		if walkToPart then
			destination = walkToPart.CFrame:PointToWorldSpace(walkToPoint)
		else
			destination = walkToPoint
		end
		local moveVector = Vector3.zero
		if Humanoid.RootPart then
			moveVector = destination - Humanoid.RootPart.CFrame.Position
			moveVector = Vector3.new(moveVector.x, 0.0, moveVector.z)
			local mag = moveVector.Magnitude
			if mag > 0.01 then
				moveVector /= mag
			end
		end
		return moveVector
	else
		return Humanoid.MoveDirection
	end
end

local function updateVelocity(currentTime)

	local tempDir

	if locomotionMap == strafingLocomotionMap then

		local moveDirection = getWalkDirection()

		if not Humanoid.RootPart then
			return
		end

		local cframe = Humanoid.RootPart.CFrame
		if math.abs(cframe.UpVector.Y) < smallButNotZero or pose ~= "Running" or humanoidSpeed < 0.001 then
			-- We are horizontal!  Do something  (turn off locomotion)
			for n,v in pairs(locomotionMap) do
				if v.track then
					v.track:AdjustWeight(smallButNotZero, runBlendtime)
				end
			end
			return
		end
		local lookat = cframe.LookVector
		local direction = Vector3.new(lookat.X, 0.0, lookat.Z)
		direction = direction / direction.Magnitude --sensible upVector means this is non-zero.
		local ly = moveDirection:Dot(direction)
		if ly <= 0.0 and ly > -0.05 then
			ly = smallButNotZero -- break quadrant ties in favor of forward-friendly strafes
		end
		local lx = direction.X*moveDirection.Z - direction.Z*moveDirection.X
		local tempDir = Vector2.new(lx, ly) -- root space moveDirection
		local delta = Vector2.new(tempDir.x-cachedLocalDirection.x, tempDir.y-cachedLocalDirection.y)
		-- Time check serves the purpose of the old keyframeReached sync check, as it syncs anim timePosition
		if delta:Dot(delta) > 0.001 or math.abs(humanoidSpeed - cachedRunningSpeed) > 0.01 or currentTime - lastBlendTime > 1 then
			cachedLocalDirection = tempDir
			cachedRunningSpeed = humanoidSpeed
			lastBlendTime = currentTime
			blend2D(cachedLocalDirection, cachedRunningSpeed)
		end 
	else
		if math.abs(humanoidSpeed - cachedRunningSpeed) > 0.01 or currentTime - lastBlendTime > 1 then
			cachedRunningSpeed = humanoidSpeed
			lastBlendTime = currentTime
			blend2D(Vector2.yAxis, cachedRunningSpeed)
		end
	end
end

function setAnimationSpeed(speed)
	if currentAnim ~= "walk" then
		if speed ~= currentAnimSpeed then
			currentAnimSpeed = speed
			currentAnimTrack:AdjustSpeed(currentAnimSpeed)
		end
	end
end

function keyFrameReachedFunc(frameName)
	if (frameName == "End") then
		local repeatAnim = currentAnim
		-- return to idle if finishing an emote
		if (emoteNames[repeatAnim] ~= nil and emoteNames[repeatAnim] == false) then
			repeatAnim = "idle"
		end

		if currentlyPlayingEmote then
			if currentAnimTrack.Looped then
				-- Allow the emote to loop
				return
			end

			repeatAnim = "idle"
			currentlyPlayingEmote = false
		end

		local animSpeed = currentAnimSpeed
		playAnimation(repeatAnim, 0.15, Humanoid)
		setAnimationSpeed(animSpeed)
	end
end

function rollAnimation(animName)
	local roll = math.random(1, animTable[animName].totalWeight)
	local origRoll = roll
	local idx = 1
	while (roll > animTable[animName][idx].weight) do
		roll = roll - animTable[animName][idx].weight
		idx = idx + 1
	end
	return idx
end

local function switchToAnim(anim, animName, transitionTime, humanoid)
	-- switch animation		
	if (anim ~= currentAnimInstance) then

		if (currentAnimTrack ~= nil) then
			currentAnimTrack:Stop(transitionTime)
			currentAnimTrack:Destroy()
		end
		if (currentAnimKeyframeHandler ~= nil) then
			currentAnimKeyframeHandler:disconnect()
		end


		currentAnimSpeed = 1.0

		currentAnim = animName
		currentAnimInstance = anim	-- nil in the case of locomotion

		if animName == "walk" then
			setupWalkAnimations()
		else
			destroyWalkAnimations()
			-- load it to the humanoid; get AnimationTrack
			currentAnimTrack = humanoid:LoadAnimation(anim)
			currentAnimTrack.Priority = Enum.AnimationPriority.Core

			currentAnimTrack:Play(transitionTime)	

			-- set up keyframe name triggers
			currentAnimKeyframeHandler = currentAnimTrack.KeyframeReached:connect(keyFrameReachedFunc)
		end
	end
end

function playAnimation(animName, transitionTime, humanoid)
	local idx = rollAnimation(animName)
	local anim = animTable[animName][idx].anim

	switchToAnim(anim, animName, transitionTime, humanoid)
	currentlyPlayingEmote = false
end

function playEmote(emoteAnim, transitionTime, humanoid)
	switchToAnim(emoteAnim, emoteAnim.Name, transitionTime, humanoid)
	currentlyPlayingEmote = true
end