-- 	["leftLeg"] = {
-- 		["CurrentCycle"] = math.pi,
-- 		["LimbChain"] =leftLegChain,
-- 		["CCDIKController"] =leftLegChain,
-- 		["HipAttachment"]= leftHipAttachment,
-- 		["FootAttachment"] = leftStepAttachment,
-- 	}
-- }

local Signal = require(script.Parent.Signal)

function ProceduralAnimatorClass.new(RootPart,Legs,RootMotor,raycastParams)

	local self = setmetatable({}, ProceduralAnimatorClass)

	--Constants
	self.RootPart = RootPart
	self.RaycastParams = raycastParams --manual input it

	if RootMotor then
		self.RootMotor = RootMotor
		self.RootMotorC1Store = RootMotor.C1
		self.WaistCycle = 0 
	end

	self.Legs = Legs

	--Default settings for legs
	self.DefaultStride = 2 -- Changes how far the legs move
	self.CycleSpeed = 15 -- How fast the leg-movement cycle is. Change this to suit your needs!
	self.DefaultStrideOffset = 0
	-- Radius of the circle at CFrame front of the player
	self.DefaultStrideCF = CFrame.new(0, 0, -self.DefaultStride / 2) -- Turn that stride number into a CFrame we can use


	--Variables that will change
	self.MovementDirectionXZ = Vector3.new(1, 0, 1) -- This will be changed
	self.rootvelm = 0

	--Sound
	self.FootStep = Signal.new();
	self.MaxSpeed = 20
	self.EngineSound = nil;
	self.FootStepSound = nil;
	self.RandomNumGenerator = Random.new()
	--debug the signal, works
	--self.FootStep:Connect(function()
	--	print("Step")
	--end)

	self.WalkBounce = 0.4 -- factor by which it bounces
	self.SwayX = -1*5 -- factor in Z direction front or behind, currently set to tilt forward
	return self
end

function ProceduralAnimatorClass:MoveLegs(stepCycle,dt)
	--if moving
	if self.rootVelocityMagnitude > 0.1 then
		for _, Leg in pairs(self.Legs) do
			local strideCF = Leg.StrideCF or self.DefaultStrideCF
			local strideOffset = Leg.StrideOffset or self.DefaultStrideOffset
			local raycastParams = self.RaycastParams
			Leg.CurrentCycle = (Leg.CurrentCycle+stepCycle)%360
			local cycle = Leg.CurrentCycle
			local IKTolerance = Leg.IKTolerance or 0

			local hip			=Leg.HipAttachment.WorldPosition
			--Position of where the lower leg should be, spread out
			local ground		=Leg.FootAttachment.WorldPosition
			local desiredPos	=(CF(ground, ground+self.MovementDirectionXZ)*ANGLES(-cycle, 0, 0)*strideCF).p
			local offset		=(desiredPos-hip)--vector from hip to the circle
			local raycastResult = workspace:Raycast(hip,offset.unit*(offset.magnitude+strideOffset),raycastParams)
			local footPos = raycastResult and raycastResult.Position or (hip + offset.unit*(offset.magnitude+strideOffset))

			--debug foot pos position
			--local part = Instance.new("Part")
			--part.CanCollide = false
			--part.CanTouch = false
			--part.BrickColor = BrickColor.Red()
			--part.Anchored = true
			--part.CanQuery = false
			--part.Size = Vector3.new(0.1,0.1,0.1)
			--part.Position = footPos
			--part.Parent = workspace
			--game.Debris:AddItem(part,0.1)

			--Do IK towards foot pos
			--Leg.CCDIKController:CCDIKIterateOnce(footPos,IKTolerance)
			--Iterating once won't fully track the footPos, needs to iterate until
			Leg.CCDIKController:CCDIKIterateUntil(footPos,IKTolerance)

			if not Leg.TouchGround and raycastResult then
				--print("Stomp")
				self.FootStep:Fire(raycastResult)
			end
			--
			if raycastResult then -- hit ground so raycast result
				Leg.TouchGround = true
			else
				Leg.TouchGround = false
			end
		end
	else--stand still
		for _, Leg in pairs(self.Legs) do
			local strideCF = Leg.StrideCF or self.DefaultStrideCF
			local strideOffset = Leg.StrideOffset or self.DefaultStrideOffset
			local raycastParams = self.RaycastParams
			local IKTolerance = Leg.IKTolerance or 0

			local hip			=Leg.HipAttachment.WorldPosition
			--Position of where the lower leg should be, spread out
			local desiredPos		=Leg.FootAttachment.WorldPosition+DOWN
			local offset		=(desiredPos-hip)--vector from hip to the circle
			local raycastResult = workspace:Raycast(hip,offset.unit*(offset.magnitude+strideOffset),raycastParams)
			local footPos = raycastResult and raycastResult.Position or (hip + offset.unit*(offset.magnitude+strideOffset))

			--Do IK towards foot pos
			Leg.CCDIKController:CCDIKIterateOnce(footPos,IKTolerance)
			--Leg.LimbChain:IterateOnce(footPos,0.1)
			--Leg.LimbChain:UpdateMotors()
			if not Leg.TouchGround and raycastResult then
				--print("Stomp")
				self.FootStep:Fire(raycastResult)
			end

			if raycastResult then -- hit ground so raycast result
				Leg.TouchGround = true
			else
				Leg.TouchGround = false
			end

		end
	end

end


function ProceduralAnimatorClass:MoveTorso(stepCycle,dt10,rootVelocity)

	local lowercf = self.RootPart.CFrame
	local waistjoint = self.RootMotor
	local waist1 = self.RootMotorC1Store
	local rootvel = rootVelocity


	if self.rootVelocityMagnitude > 0.1 then

		self.WaistCycle = (self.WaistCycle+stepCycle)%360

		local relv0		=lowercf:vectorToObjectSpace(rootvel)
		local relv1		=relv0*0.2

		do -- Upper Torso
			local bounceCFrame = CFrame.new(0,self.WalkBounce*math.cos((self.WaistCycle+90+45)*2),0)

			local sway = math.rad(-relv1.X)+0.08*math.cos(self.WaistCycle+90)
			local swayY = 0.1*math.cos(self.WaistCycle)-2*math.rad(relv1.X)
			local swayX = math.rad(relv1.Z)*0.5*self.SwayX
			local goalCF = bounceCFrame*waist1*ANGLES(swayX,swayY,sway):inverse()
			-- goalCF *= CFrame.new(0,math.cos((self.WaistCycle+90+45)*2),0)-- Up and down
			--goalCF *= CFrame.new(0,self.WalkBounce*math.cos((self.WaistCycle+90+45)*2),0)-- Up and down
			--local rotationOnly = goalCF-goalCF.Position
			waistjoint.C1	=	waistjoint.C1:Lerp(goalCF,dt10)
		end

	else
		--when not moving go back to original position
		local goalCF = waistjoint.C1:Lerp(waist1, dt10)
		--local rotationOnly = goalCF-goalCF.Position
		waistjoint.C1	= goalCF
	end
end


function ProceduralAnimatorClass:Animate(dt)

	-- Begin the step-------
	local dt10 = math.min(dt*10, 1) -- Normalize dt for our needs

	local rootpart = self.RootPart
	local rootvel0 = rootpart.Velocity -- Our movement velocity

	local rootVelocity = rootvel0 * x_and_y --XY plane velocity only
	local rootVelocityMagnitude = rootVelocity.Magnitude --root velocity magnitude
	self.rootVelocityMagnitude = rootVelocityMagnitude

	if self.EngineSound then
		self.EngineSound.PlaybackSpeed = (rootVelocityMagnitude / self.MaxSpeed) + 0.6
	end

	--if moving then lerp current direction
	if rootVelocityMagnitude > 0.1 then 
		--lerp current direction towards curren velocity
		self.MovementDirectionXZ = self.MovementDirectionXZ:Lerp(rootVelocity.unit, dt10) 
	end

	local relativizeToHumanoidSpeed = rootVelocityMagnitude/16 --default walk speed is 16
	local stepCycle = relativizeToHumanoidSpeed*dt*self.CycleSpeed

	self:MoveLegs(stepCycle,dt)
	if self.RootMotor then
		self:MoveTorso(stepCycle,dt10,rootVelocity)
	end

end

function ProceduralAnimatorClass:ConnectFootStepSound(sound : Sound)
	self.FootStep:Connect(function(raycastResult)
		local soundPositionAttachment = Instance.new("Attachment")
		soundPositionAttachment.WorldPosition = raycastResult.Position
		soundPositionAttachment.Parent = workspace.Terrain

		local footStepSound = sound:Clone()
		local randomPlaybackSpeed = self.RandomNumGenerator:NextNumber(0.7,1)
		footStepSound.PlaybackSpeed = randomPlaybackSpeed

		local reverbEffect = Instance.new("ReverbSoundEffect")
		reverbEffect.Density = 0.8
		reverbEffect.DecayTime = 1
		reverbEffect.Parent = footStepSound
		footStepSound.PlayOnRemove = true

		footStepSound.Parent = soundPositionAttachment
		soundPositionAttachment:Destroy()
	end)
end

function ProceduralAnimatorClass:StartEngineSound(sound : Sound)
	local engineSound = sound:Clone()
	engineSound.Parent = self.RootPart
	engineSound.Looped = true
	engineSound:Play()
end

function ProceduralAnimatorClass:InitDragDebug()
	for _, Leg in pairs(self.Legs) do
		Leg.CCDIKController:InitDragDebug()
	end
end

function ProceduralAnimatorClass:Destroy()
	if self.FootStep then
		self.FootStep:Destroy()
	end
	self = nil
end

return ProceduralAnimatorClass
