function aiGetClosestNode(x,y,z,path)
	local cDist = nil
	local cNode = nil
	local cNodeID = nil
	
	for i,node in ipairs(path) do
		local dist = getDistanceBetweenPoints3D(x,y,z,node.x,node.y,node.z)
		if cDist == nil then
			cDist = dist
			cNodeID = i
			cNode = node
		end
		
		if dist < cDist then
			cDist = dist
			cNodeID = i
			cNode = node
		end
	end
	
	return cNodeID,cNode
end

function aiGetNextNode(path,nodeID,minDist)
	minDist = minDist or 0
	local prevX = path[nodeID].x
	local prevY = path[nodeID].y
	local prevZ = path[nodeID].z
	local dist = 0
	nodeID = nodeID + 1
	local length = #path
	while nodeID <= length do
		local x = path[nodeID].x
		local y = path[nodeID].y
		local z = path[nodeID].z
		dist = dist + getDistanceBetweenPoints3D(x,y,z,prevX,prevY,prevZ)
		if dist >= minDist then
			return nodeID,path[nodeID]
		end
		prevX = x
		prevY = y
		prevZ = z
		nodeID = nodeID + 1
	end
	
	nodeID = length
	return nodeID,path[nodeID]
end

function handleGpsAi(driver)
	local gx = getElementData(driver,"gps-ai.x")
	local gy = getElementData(driver,"gps-ai.y")
	local gz = getElementData(driver,"gps-ai.z")
	local veh = getPedOccupiedVehicle(driver)
	local x,y,z = getElementPosition(veh)
	local rx,ry,rz = getElementRotation(veh)
	local speed = getElementSpeed(veh)
	
	if not getElementData(driver,"gps-ai.path") then
		local path = calculatePathByCoords(x,y,z,gx,gy,gz)
		if not path then return end
		setElementData(driver,"gps-ai.path",path,false)
	end
	
	local path = getElementData(driver,"gps-ai.path")
	local closestNodeID,closestNode = aiGetClosestNode(x,y,z,path)
	local checkNodeOneID,checkNodeOne = aiGetNextNode(path,closestNodeID,20 + ((speed * 0.15) * (speed * 0.01)))
	local checkNodeTwoID,checkNodeTwo = aiGetNextNode(path,closestNodeID,20 + ((speed * 0.8) * (speed * 0.008)))
	
	--[[ hit = processLineOfSight(x,y,z,checkNodeOne.x,checkNodeOne.y,checkNodeOne.z + 0.5,true,false,false,true,false,false,false,false,nil,false,false)
	if hit then
		checkNodeOneID,checkNodeOne = aiGetNextNode(path,closestNodeID)
	end ]]--
	
	local reqzOne = findRotation(x,y,checkNodeOne.x,checkNodeOne.y)
	local reqzTwo = findRotation(x,y,checkNodeTwo.x,checkNodeTwo.y)
	
	reqzOneLocal = rz - reqzOne
	if reqzOneLocal > 180 then reqzOneLocal = reqzOneLocal - 360 end
	if reqzOneLocal < -180 then reqzOneLocal = reqzOneLocal + 360 end
	
	reqzTwoLocal = rz - reqzTwo
	if reqzTwoLocal > 180 then reqzTwoLocal = reqzTwoLocal - 360 end
	if reqzTwoLocal < -180 then reqzTwoLocal = reqzTwoLocal + 360 end
	
	local steeringAngleOne = reqzOneLocal
	if steeringAngleOne < 0 then
		steeringAngleOne = 0 - steeringAngleOne
	end
	steeringAmountOne = steeringAngleOne / 180
	
	local steeringAngleTwo = reqzTwoLocal
	if steeringAngleTwo < 0 then
		steeringAngleTwo = 0 - steeringAngleTwo
	end
	steeringAmountTwo = steeringAngleTwo / 180
	
	local brake = false
	--[[ if steeringAngleOne > steeringAngleTwo then
		regzTwo = regzOne
		regzTwoLocal = regzOneLocal
		steeringAngleTwo = steeringAngleOne
		steeringAmountTwo = steeringAmountOne
		brake = true
	end ]]--
	
	local maxSpeed = getElementData(driver,"gps-ai.maxSpeed")
	if steeringAngleTwo > 10 then
		local owo = steeringAmountTwo * 2.5
		if owo > 1 then owo = 1 end
		local nMaxSpeed = 15 + (130 * (1 - owo))
		if nMaxSpeed < maxSpeed then maxSpeed = nMaxSpeed end
	end
	
	if brake then maxSpeed = maxSpeed * 0.5 end
	
	if speed < maxSpeed then
		setPedControlState(driver,"accelerate",true)
		setPedAnalogControlState(driver,"brake_reverse",0)
	else
		setPedControlState(driver,"accelerate",false)
	end
	
	if speed > maxSpeed + 5 then
		setPedAnalogControlState(driver,"brake_reverse",0.8)
	end
	
	local currentSteer = getPedAnalogControlState(driver,"vehicle_right") - getPedAnalogControlState(driver,"vehicle_left")
	local steerSpeed = 0.2
	
	if reqzOneLocal < 0 then
		currentSteer = currentSteer - steerSpeed
	end
	
	if reqzOneLocal > 0 then
		currentSteer = currentSteer + steerSpeed
	end
	steerLimit = steeringAngleOne / 10
	
	if steerLimit < 0.4 then steerLimit = 0.4 end
	if steerLimit > 1 then steerLimit = 1 end
	if currentSteer > steerLimit then currentSteer = steerLimit end
	if currentSteer < 0 - steerLimit then currentSteer = 0 - steerLimit end
	
	if currentSteer > 0 then
		setPedAnalogControlState(driver,"vehicle_left",0)
		setPedAnalogControlState(driver,"vehicle_right",currentSteer)
	end
	
	if currentSteer < 0 then
		setPedAnalogControlState(driver,"vehicle_right",0)
		setPedAnalogControlState(driver,"vehicle_left",0 - currentSteer)
	end
	
	-- DEBUG --
	local resx,resy = guiGetScreenSize()
	dxDrawLine3D(x,y,z,checkNodeOne.x,checkNodeOne.y,checkNodeOne.z + 1,tocolor(255,0,0,128),10)
	dxDrawLine3D(x,y,z,checkNodeTwo.x,checkNodeTwo.y,checkNodeTwo.z + 1,tocolor(255,255,0,128),10)
	
	local debugText = ""
	debugText = debugText .. "Required Z 1: " ..reqzOne
	debugText = debugText .. "\nRequired Z 2: " ..reqzTwo
	debugText = debugText .. "\n"
	debugText = debugText .. "\nRequired Z 1 - LOCAL: " ..reqzOneLocal
	debugText = debugText .. "\nMax speed: " ..maxSpeed.. " (" ..steeringAmountTwo.. ")"
	local px,py,pz = getElementPosition(driver)
	local dbx,dby,dbz = getScreenFromWorldPosition(px,py,pz + 1)
	dxDrawText(debugText,dbx + 1,dby + 1,dbx + 1,dby + 1,tocolor(0,0,0),1,"default-bold","center","center",false,true,true)
	dxDrawText(debugText,dbx,dby,dbx,dby,tocolor(255,255,0),1,"default-bold","center","center",false,true,true)
end

function handleGpsAiFrame()
	for _,ped in ipairs(getElementsByType("ped")) do
		if getElementData(ped,"gps-ai.x") then
			handleGpsAi(ped)
		end
	end
end

addEventHandler("onClientRender",root,handleGpsAiFrame)

function getElementSpeed(theElement)
    return (Vector3(getElementVelocity(theElement)) * 180).length
end

function findRotation( x1, y1, x2, y2 ) 
    local t = -math.deg( math.atan2( x2 - x1, y2 - y1 ) )
    return t < 0 and t + 360 or t
end