local state = {}
local pathindex = {}
local pathpointindex = {}
local pathdirection = {}
local movespeed = {}
local rotationOffset = {}

function follow_waypoints_init(e)
	state[e] = "find a waypoint"	
	movespeed[e] = 60
	rotationOffset[e] = 180
end 

function follow_waypoints_main(e)

	if state[e] == "find a waypoint" then 
		pathindex[e],pathpointindex[e],pathdirection[e] = GetNearestWaypoint(e)
		if pathindex[e] > -1 then 
			state[e] = "follow waypoints"
		else 
			state[e] = "no path"
		end
	elseif state[e] == "follow waypoints" then 
		pathindex[e],pathpointindex[e],pathdirection[e] = FollowWaypointPath(e,pathindex[e],pathpointindex[e],pathdirection[e],movespeed[e])
	elseif state[e] == "no path" then 
		PromptLocal(e,"No path found near me!")
	end 
	PromptLocal(e,"path index = "..pathindex[e].." , point index = "..pathpointindex[e].." , direction = "..pathdirection[e])
end 

function follow_waypoints_exit(e)

end 

function GetNearestWaypoint(e)
	PathIndex = -1
	PathPointIndex = -1
	PathDirection = -1
	pClosest = 99999
	for pa = 1, AIGetTotalPaths(), 1 do
		for po = 1 , AIGetPathCountPoints(pa), 1 do
			pDX = g_Entity[e]['x'] - AIPathGetPointX(pa,po)
			pDZ = g_Entity[e]['z'] - AIPathGetPointZ(pa,po)
			pDist = math.sqrt(math.abs(pDX*pDX)+math.abs(pDZ*pDZ))
			if pDist < pClosest and pDist < 200 then
				pClosest = pDist
				PathIndex = pa
				PathPointIndex = po
				if po > AIGetPathCountPoints(pa) then 
					PathDirection = -1
				else 
					PathDirection = 1 
				end 
			end
		end -- po
	end -- pa
	return PathIndex, PathPointIndex, PathDirection
end

function FollowWaypointPath(e,pathid,pointid,pathdirection,speed)
	dpx = AIPathGetPointX(pathid,pointid)
	dpz = AIPathGetPointZ(pathid,pointid)
	CollisionOff(e)
	RotateToPoint(e,dpx,dpz,rotationOffset)
	MoveForward(e,speed)
	CollisionOn(e)
	tDistX = g_Entity[e]['x'] - dpx
	tDistZ = g_Entity[e]['z'] - dpz
	DistFromPath = math.sqrt(math.abs(tDistX*tDistX)+math.abs(tDistZ*tDistZ))	
	if DistFromPath < 50 then
		maxpathpoints = AIGetPathCountPoints(pathid)
		if pathdirection == 1 then 
			pointid = pointid + pathdirection
			if pointid > maxpathpoints then
				pathdirection = -1
				pointid = maxpathpoints-1
			end 
		else 
			pointid = pointid + pathdirection
			if pointid < 1 then 
				pathdirection = 1
				pointid = 2
			end 
		end 
	end
	return pathid, pointid, pathdirection
end

function RotateToPoint(e,x,z,offset)
	if g_Entity[e] ~= nil and x > 0 and z > 0 then
		local destx = x - g_Entity[e]['x']
		local destz = z - g_Entity[e]['z']
		local angle = math.atan2(destx,destz)
		angle = angle * (180.0 / math.pi)
		if angle < 0 then
			angle = 360 + angle
		elseif angle > 360 then
			angle = angle - 360
		end
		SetRotation(e,0,angle+offset,0)
		return angle
	end
end