--[[ The ClickToMove Controller Class ]]--
local KeyboardController = require(script.Parent:WaitForChild("Keyboard"))
local ClickToMove = setmetatable({}, KeyboardController)
ClickToMove.__index = ClickToMove

function ClickToMove.new(CONTROL_ACTION_PRIORITY)
	local self = setmetatable(KeyboardController.new(CONTROL_ACTION_PRIORITY), ClickToMove)
	
	self.fingerTouches = {}
	self.numUnsunkTouches = 0
	-- PC simulation
	self.mouse1Down = tick()
	self.mouse1DownPos = Vector2.new()
	self.mouse2DownTime = tick()
	self.mouse2DownPos = Vector2.new()
	self.mouse2UpTime = tick()



	self.tapConn = nil
	self.inputBeganConn = nil
	self.inputChangedConn = nil
	self.inputEndedConn = nil
	self.humanoidDiedConn = nil
	self.characterChildAddedConn = nil
	self.onCharacterAddedConn = nil
	self.characterChildRemovedConn = nil
	self.renderSteppedConn = nil
	self.humanoidSeatedConn = nil
	
	self.running = false
	
	self.wasdEnabled = false
	
	return self
end

function ClickToMove:DisconnectEvents()
	DisconnectEvent(self.tapConn)
	DisconnectEvent(self.inputBeganConn)
	DisconnectEvent(self.inputChangedConn)
	DisconnectEvent(self.inputEndedConn)
	DisconnectEvent(self.humanoidDiedConn)
	DisconnectEvent(self.characterChildAddedConn)
	DisconnectEvent(self.onCharacterAddedConn)
	DisconnectEvent(self.renderSteppedConn)
	DisconnectEvent(self.characterChildRemovedConn)
	
	-- TODO: Resolve with ControlScript handling of seating for vehicles
	DisconnectEvent(self.humanoidSeatedConn)
	
	RunService:UnbindFromRenderStep("ClickToMoveRenderUpdate")
end

function ClickToMove:OnTouchBegan(input, processed)
	if self.fingerTouches[input] == nil and not processed then
		self.numUnsunkTouches = self.numUnsunkTouches + 1
	end
	self.fingerTouches[input] = processed
end

function ClickToMove:OnTouchChanged(input, processed)
	if self.fingerTouches[input] == nil then
		self.fingerTouches[input] = processed
		if not processed then
			self.numUnsunkTouches = self.numUnsunkTouches + 1
		end
	end
end

function ClickToMove:OnTouchEnded(input, processed)
	if self.fingerTouches[input] ~= nil and self.fingerTouches[input] == false then
		self.numUnsunkTouches = self.numUnsunkTouches - 1
	end
	self.fingerTouches[input] = nil
end


function ClickToMove:OnCharacterAdded(character)
	self:DisconnectEvents()

	self.inputBeganConn = UserInputService.InputBegan:Connect(function(input, processed)
		if input.UserInputType == Enum.UserInputType.Touch then
			self:OnTouchBegan(input, processed)

			-- Give back controls when they tap both sticks
			local wasInBottomLeft = IsInBottomLeft(input.Position)
			local wasInBottomRight = IsInBottomRight(input.Position)
			if wasInBottomRight or wasInBottomLeft then
				for otherInput, _ in pairs(self.fingerTouches) do
					if otherInput ~= input then
						local otherInputInLeft = IsInBottomLeft(otherInput.Position)
						local otherInputInRight = IsInBottomRight(otherInput.Position)
						if otherInput.UserInputState ~= Enum.UserInputState.End and ((wasInBottomLeft and otherInputInRight) or (wasInBottomRight and otherInputInLeft)) then
							if BindableEvent_OnFailStateChanged then
								BindableEvent_OnFailStateChanged:Fire(true)
							end
							return
						end
					end
				end
			end
		end

		 -- Cancel path when you use the keyboard controls.
		if processed == false and input.UserInputType == Enum.UserInputType.Keyboard and movementKeys[input.KeyCode] then
			CleanupPath()
		end
		if input.UserInputType == Enum.UserInputType.MouseButton1 then
			self.mouse1DownTime = tick()
			self.mouse1DownPos = input.Position
		end
		if input.UserInputType == Enum.UserInputType.MouseButton2 then
			self.mouse2DownTime = tick()
			self.mouse2DownPos = input.Position
		end
	end)

	self.inputChangedConn = UserInputService.InputChanged:Connect(function(input, processed)
		if input.UserInputType == Enum.UserInputType.Touch then
			self:OnTouchChanged(input, processed)
		end
	end)

	self.inputEndedConn = UserInputService.InputEnded:Connect(function(input, processed)
		if input.UserInputType == Enum.UserInputType.Touch then
			self:OnTouchEnded(input, processed)
		end

		if input.UserInputType == Enum.UserInputType.MouseButton2 then
			self.mouse2UpTime = tick()
			local currPos = input.Position
			if self.mouse2UpTime - self.mouse2DownTime < 0.25 and (currPos - self.mouse2DownPos).magnitude < 5 and self.moveVector.Magnitude<=0 then
				local positions = {currPos}
				OnTap(positions)
			end
		end
	end)

	self.tapConn = UserInputService.TouchTap:Connect(function(touchPositions, processed)
		if not processed then
			OnTap(touchPositions)
		end
	end)
	
	local function computeThrottle(dist)
		if dist > .2 then
			return 0.5+(dist^2)/2
		else
			return 0
		end
	end		

	local lastSteer = 0

	--kP = how much the steering corrects for the current error in driving angle
	--kD = how much the steering corrects for how quickly the error in driving angle is changing
	local kP = 1
	local kD = 0.5
	local function getThrottleAndSteer(object, point)
		local throttle, steer = 0, 0
		local oCF = object.CFrame
		
		local relativePosition = oCF:pointToObjectSpace(point)
		local relativeZDirection = -relativePosition.z
		local relativeDistance = relativePosition.magnitude
		
		-- throttle quadratically increases from 0-1 as distance from the selected point goes from 0-50, after 50, throttle is 1.
		-- this allows shorter distance travel to have more fine-tuned control.
		throttle = computeThrottle(math.min(1,relativeDistance/50))*math.sign(relativeZDirection)
		
		local steerAngle = -math.atan2(-relativePosition.x, -relativePosition.z)
		steer = steerAngle/(math.pi/4)

		local steerDelta = steer - lastSteer
		lastSteer = steer
		local pdSteer = kP * steer + kD * steer
		return throttle, pdSteer
	end
	
	local function Update()
		if CurrentSeatPart then
			if DrivingTo then
				local throttle, steer = getThrottleAndSteer(CurrentSeatPart, DrivingTo)
				CurrentSeatPart.ThrottleFloat = throttle
				CurrentSeatPart.SteerFloat = steer
			else
				CurrentSeatPart.ThrottleFloat = 0
				CurrentSeatPart.SteerFloat = 0
			end
		end
		
		local cameraPos = workspace.CurrentCamera.CFrame.p
		for i = 1, #activePopups do
			local popup = activePopups[i]
			popup.CFrame = CFrame.new(popup.CFrame.p, cameraPos)
		end
	end
	
	RunService:BindToRenderStep("ClickToMoveRenderUpdate",Enum.RenderPriority.Camera.Value - 1,Update)

	local function OnCharacterChildAdded(child)
		if UserInputService.TouchEnabled then
			if child:IsA('Tool') then
				child.ManualActivationOnly = true
			end
		end
		if child:IsA('Humanoid') then
			DisconnectEvent(self.humanoidDiedConn)
			self.humanoidDiedConn = child.Died:Connect(function()
				if ExistingIndicator then
					DebrisService:AddItem(ExistingIndicator.Model, 1)
				end
			end)
		end
	end

	self.characterChildAddedConn = character.ChildAdded:Connect(function(child)
		OnCharacterChildAdded(child)
	end)
	self.characterChildRemovedConn = character.ChildRemoved:Connect(function(child)
		if UserInputService.TouchEnabled then
			if child:IsA('Tool') then
				child.ManualActivationOnly = false
			end
		end
	end)
	for _, child in pairs(character:GetChildren()) do
		OnCharacterChildAdded(child)
	end
end

function ClickToMove:Start()
	self:Enable(true)
end

function ClickToMove:Stop()
	self:Enable(false)
end

function ClickToMove:Enable(enable, enableWASD)
	if enable then
		if not self.running then
			if Player.Character then -- retro-listen
				self:OnCharacterAdded(Player.Character)
			end
			self.onCharacterAddedConn = Player.CharacterAdded:Connect(function(char)
				self:OnCharacterAdded(char)
			end)
			self.running = true
		end
	else
		if self.running then
			self:DisconnectEvents()
			CleanupPath()
			-- Restore tool activation on shutdown
			if UserInputService.TouchEnabled then
				local character = Player.Character
				if character then
					for _, child in pairs(character:GetChildren()) do
						if child:IsA('Tool') then
							child.ManualActivationOnly = false
						end
					end
				end
			end
			DrivingTo = nil
			self.running = false
		end
	end
	
	-- Extension for initializing Keyboard input as this class now derives from Keyboard
	if UserInputService.KeyboardEnabled and enable ~= self.enabled then
	
		self.forwardValue  = 0
		self.backwardValue = 0
		self.leftValue = 0
		self.rightValue = 0
		
		self.moveVector = ZERO_VECTOR3
		
		if enable then
			self:BindContextActions()
			self:ConnectFocusEventListeners()
		else
			self:UnbindContextActions()
			self:DisconnectFocusEventListeners()
		end	
	end
	
	self.wasdEnabled = enable and enableWASD or false
	self.enabled = enable
end