dofile("data/scripts/lib/utilities.lua")

local infinity = 1/0

function IsPointObstructed(point, curr)

    if (RaytraceSurfaces( curr.x, curr.y, point.x, point.y )) then
        return true
    end

    return false
end

function DistanceBetween ( nodeA, nodeB )

	return get_distance( nodeA.x, nodeA.y, nodeB.x, nodeB.y )
end

function HeuristicCostEstimate ( nodeA, nodeB )

	return get_distance( nodeA.x, nodeA.y, nodeB.x, nodeB.y )
end

function IsValidNode ( node, neighbor, goal, node_density )
	--print(table.dump(neighbor))
	--print(table.dump(node))
	local MAX_DIST = 10
		
	if get_distance( node.x, node.y, neighbor.x, neighbor.y ) > MAX_DIST then
		return false
	end

	return not IsPointObstructed({x = neighbor.x, y = neighbor.y}, {x = node.x, y = node.y} )
end

function LowestFScore ( set, f_score )

	local lowest, bestNode = infinity, nil
	for _, node in ipairs ( set ) do
		local score = f_score [ node ]
		if score < lowest then
			lowest, bestNode = score, node
		end
	end
	return bestNode
end

function NeighborNodes ( theNode, nodes, goal, solver, grid_density )

	local neighbors = {}
	for _, node in ipairs ( nodes ) do
		if theNode ~= node and solver( theNode, node, goal, grid_density ) then
			table.insert ( neighbors, node )
		end
	end
	return neighbors
end

function NotIn ( set, theNode )

	for _, node in ipairs ( set ) do
		if node == theNode then return false end
	end
	return true
end

function RemoveNode ( set, theNode )

	for i, node in ipairs ( set ) do
		if node == theNode then 
			set [ i ] = set [ #set ]
			set [ #set ] = nil
			break
		end
	end	
end

function UnwindPath ( flat_path, map, current_node )

	if map [ current_node ] then
		table.insert ( flat_path, 1, map [ current_node ] ) 
		return UnwindPath ( flat_path, map, map [ current_node ] )
	else
		return flat_path
	end
end

function aStar ( start, goal, nodes, grid_density, valid_node_func )

	local closedset = {}
	local openset = { start }
	local came_from = {}
	local node_function = nil

	if valid_node_func ~= nil then 
		node_function = valid_node_func 
	else
		node_function = IsValidNode
	end

	local g_score, f_score = {}, {}
	g_score [ start ] = 0
	f_score [ start ] = g_score [ start ] + HeuristicCostEstimate ( start, goal )

	while #openset > 0 do
	
		local current = LowestFScore ( openset, f_score )
		if current == goal then
			local path = UnwindPath ( {}, came_from, goal )
			table.insert ( path, goal )
			return path
		end

		RemoveNode ( openset, current )		
		table.insert ( closedset, current )
		
		local neighbors = NeighborNodes ( current, nodes, goal, node_function, grid_density )
		for _, neighbor in ipairs ( neighbors ) do 
			if NotIn ( closedset, neighbor ) then
			
				local tentative_g_score = g_score [ current ] + DistanceBetween ( current, neighbor )
				 
				if NotIn ( openset, neighbor ) or tentative_g_score < g_score [ neighbor ] then 
					came_from 	[ neighbor ] = current
					g_score 	[ neighbor ] = tentative_g_score
					f_score 	[ neighbor ] = g_score [ neighbor ] + HeuristicCostEstimate ( neighbor, goal )
					if NotIn ( openset, neighbor ) then
						table.insert ( openset, neighbor )
					end
				end
			end
		end
	end
	return nil -- no valid path
end

function CreatePath ( start, goal, nodes, grid_density, valid_node_func )

    local resPath = aStar ( start, goal, nodes, grid_density, valid_node_func )

	return resPath
end

table.reverse = function(t)
    local n = #t
    local i = 1
    while i < n do
      t[i],t[n] = t[n],t[i]
      i = i + 1
      n = n - 1
    end
end

table.indexOf = function(t, object)
    if type(t) ~= "table" then error("table expected, got " .. type(t), 2) end

    for i, v in pairs(t) do
        if object == v then
            return i
        end
    end
end

function table.removeRange(t, i1, i2)
	indexes = {}
	for k, v in pairs(t)do
		if(k >= i1 and k <= i2)then
			table.insert(indexes, k)
		end
	end
	for k, v in pairs(indexes)do
		table.remove(t, v)
	end
end


function FindClosest(x, y, t)
	closest = 9999999999
	closest_item = nil
	for k, v in pairs(t)do

		
		distance = get_distance( x, y, v.x, v.y )
		if(distance < closest)then
			closest_item = v
			closest = distance
		end
	end

	return closest_item
end

function ShallowCopy(orig)
    local orig_type = type(orig)
    local copy
    if orig_type == 'table' then
        copy = {}
        for orig_key, orig_value in pairs(orig) do
            copy[orig_key] = orig_value
        end
    else -- number, string, boolean, etc
        copy = orig
    end
    return copy
end

function SmoothPath(orig_path)
	smooth_path = {}

	path = ShallowCopy(orig_path)

	table.insert(smooth_path, path[1])

	table.remove(path, 1)
	table.reverse(path)

	while #path > 0 do
		lP = smooth_path[#smooth_path]
		nP = path[#path]

		for _, p in pairs(path)do

			dir = get_direction(lP.x, lP.y, p.x, p.y)
			dir_offset1 = (dir + math.rad(90)) % (2 * math.pi)
			dir_offset2 = (dir + math.rad(-90)) % (2 * math.pi)
			
			vec1_x, vec1_y = rad_to_vec( dir_offset1 )
			vec2_x, vec2_y = rad_to_vec( dir_offset2 )
			
			start1_x = lP.x + (vec1_x * 5)
			start1_y = lP.y + (vec1_y * 5)
			end1_x = p.x + (vec1_x * 5)
			end1_y = p.y + (vec1_y * 5)
			
			start2_x = lP.x + (vec2_x * 5)
			start2_y = lP.y + (vec2_y * 5)
			end2_x = p.x + (vec2_x * 5)
			end2_y = p.y + (vec2_y * 5)

			if (not RaytraceSurfaces( lP.x, lP.y, p.x, p.y ) and not RaytraceSurfaces( start1_x, start1_y, end1_x, end1_y ) and not RaytraceSurfaces( start2_x, start2_y, end2_x, end2_y )) then
				nP = p
				break
			end
		end

		table.insert(smooth_path, nP)
		index = table.indexOf(path, nP)
		table.removeRange(path, index, #path)
	end

	return smooth_path
end

function nearest(i, v)
    return math.floor(i/v) * v
end

function FindPath( start_x, start_y, goal_x, goal_y, smooth, custom_solver, node_grid )

	if(type(custom_solver) == "table")then
		node_grid = custom_solver
		custom_solver = nil
	end

	

	node_grid = node_grid or {}

	id = 0

	--markers = EntityGetWithTag("nodes_debug_marker")
	--for k, v in pairs(markers)do
	--	EntityKill(v)
	--end

	grid_density = 8

	start_x_rounded = nearest(start_x, grid_density)
	start_y_rounded = nearest(start_y, grid_density)

	--GamePrint("x = "..start_x_rounded..", y = "..start_y_rounded)


	time_start = GameGetRealWorldTimeSinceStarted()
	
	if(node_grid[1] == nil)then
		for x = start_x_rounded - 256, start_x_rounded + 256, grid_density do
			for y = start_y_rounded - 256, start_y + 256, grid_density do

				if (not RaytraceSurfaces( x, y, x + 1, y )) then
				

					id = id + 1



					table.insert(node_grid, {id = id, x = x, y = y})

				end
				--entity = EntityLoad("mods/carrion_creature/files/entities/node.xml", x, y)
				--EntityAddTag(entity, "nodes_debug_marker")
			end
		end
	end

	--print("Generating nodes took: "..tostring((GameGetRealWorldTimeSinceStarted() - time_start) * 1000).."ms")
	
	--time_start = GameGetRealWorldTimeSinceStarted()
	path = CreatePath( FindClosest(start_x, start_y, node_grid), FindClosest(goal_x, goal_y, node_grid), node_grid, grid_density, custom_solver )
	--print("Pathfinding took: "..tostring((GameGetRealWorldTimeSinceStarted() - time_start) * 1000).."ms")

	if(path == nil)then
		return nil
	end

	if(not smooth)then
		return path, node_grid
	end

	smooth_path = SmoothPath(path)

	return smooth_path, node_grid
		
end

