a.star <- function(graph, startNode, endNodes, hCost)
{
	if (is.null(rownames(graph)))
		vNames <- 1:nrow(graph)
	else
		vNames <- rownames(graph)

	startNode <- which(vNames %in% startNode)
	endNodes <- which(vNames %in% endNodes)


	open <- rep(FALSE, len = nrow(graph))
	closed <- rep(FALSE, len = nrow(graph))
	
	gScore <- rep(Inf, len = nrow(graph))
	gScore[startNode] <- 0

	fScore <- rep(Inf, len = nrow(graph))
	fScore[startNode] <- hCost[startNode]

	from <- rep(-1, len = nrow(graph))

	open[startNode] <- TRUE
	print(paste("Odpiram vozlisce", vNames[startNode]))
	
	while (any(open))
	{
		sel <- which.min(fScore[open])
		curNode <- which(open)[sel]

		open[curNode] <- FALSE
		closed[curNode] <- TRUE
		print(paste("Zapiram vozlisce", vNames[curNode]))

		if (curNode %in% endNodes)
		{
			print(paste("Resitev A* v vozliscu", vNames[curNode]))

			path <- vNames[curNode]
			while (TRUE)
			{
				curNode <- from[curNode]
				if (curNode != -1)
					path <- paste(path, "<--", vNames[curNode])
				else
					return(path)
			}
		}

		for (nextNode in 1:ncol(graph))
		{
			if (!is.na(graph[curNode, nextNode]) && !closed[nextNode])
			{
				if (!open[nextNode])
					print(paste("Odpiram vozlisce", vNames[nextNode])) 

				open[nextNode] <- TRUE
				dist <- gScore[curNode] + graph[curNode, nextNode]
				
				if (dist < gScore[nextNode])
				{
					from[nextNode] <- curNode
					gScore[nextNode] <- dist
					fScore[nextNode] <- gScore[nextNode] + hCost[nextNode]
					print(paste("Posodabljam vozlisce", vNames[nextNode]))
				}
			}
		}
	}
}


ida.star <- function(graph, startNode, endNodes, hCost)
{
	# lokalna funkcija 
	search <- function(gScore, bound)
	{
		# iz lokalne funkcije "search" so vidne vse lokalne spremenljivke funkcije "ida.star"
		# z operatorjem "<-" zapisujemo v lokalne spremenljivke, ki so vidne samo znotraj funkcije "search"
		# z operatorjem "<<-" zapisujemo v lokalne spremenljivke funkcije "ida.star"
   		

		curNode <- path[length(path)]

   		fScore <- gScore + hCost[curNode]
   		
		if (fScore > bound)
		{
			print(paste("Vozlisce ", vNames[curNode], " je zunaj trenutne meje (razdalja ", fScore, ")", sep=""))
			flush.console()

			return (fScore)
		}

		print(paste("Vozlisce", vNames[curNode], "je znotraj trenutne meje"))
		flush.console()

   		if (curNode %in% endNodes)
		{
			found <<- TRUE												
			return (fScore)
   		}

		min <- Inf
		sel <- which(!is.na(graph[curNode,]))
			
		for (nextNode in sel)
		{
   			if (!(nextNode %in% path))
			{
       			path <<- append(path, nextNode)
				res <- search(gScore + graph[curNode, nextNode], bound)
       			
				if (found)
					return(res)
       			
				if (res < min)
					min <- res
       			
				path <<- path[-length(path)]
     			}
   		}

   		min
 	}


	if (is.null(rownames(graph)))
		vNames <- 1:nrow(graph)
	else
		vNames <- rownames(graph)

	startNode <- which(vNames %in% startNode)
	endNodes <- which(vNames %in% endNodes)

	found <- FALSE
	path <- startNode
	bound <- hCost[startNode]
	
	while (TRUE)
	{
		print(paste("Meja iskanja je nastavljena na", bound))
		flush.console()

		res <- search(0, bound)

		if (found)
		{
			print(paste("Resitev IDA* je v vozliscu:", vNames[path[length(path)]]))
			print(paste("Najdena resitev je na razdalji:", res))
			print(paste("Najdena pot:", paste(vNames[path],collapse=" --> ")))
			break
		}

		if (is.infinite(res))
		{
			print("Iz zacetnega vozlisca ni mozno priti do nobenega ciljnega vozlisca!")
			break
		}

		# postavi novo mejo iskanja
		bound <- res
	}
}



#
# Naloga iz vaj
#

graph <- matrix(c(NA, 7, 9,NA,NA, 3,NA,NA,
		     	 4,NA,NA, 2, 8,NA,NA,NA,
		     	NA, 3,NA,NA, 6,NA, 5,NA,
			NA,NA,NA,NA, 3,NA,NA, 6,
			NA,NA,NA,NA,NA,NA, 9,10,
			NA,NA, 8,NA, 6,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,10,
			NA,NA,NA,NA,NA,NA,NA,NA), byrow=TRUE, ncol=8, nrow=8)

colnames(graph) <- c("a","b","c","d","e","f","g","h")
rownames(graph) <- c("a","b","c","d","e","f","g","h")
graph

hCost <- c(8,2,4,3,9,12,0,0)

a.star(graph, "a", c("g","h"), hCost)
ida.star(graph, "a", c("g","h"), hCost)





#
# Samostojno delo (naloga iz vaj)
#

graph <- matrix(c(NA, 3, 3, 2,NA,NA,NA,NA,NA,NA,NA,NA,NA,
		     	NA,NA,NA,NA, 2, 4,NA,NA,NA,NA,NA,NA,NA,
		     	NA,NA,NA,NA,NA, 7, 2, 8,NA,NA,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,NA, 5, 2,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,NA,NA,NA, 2, 2,NA,
			NA,NA,NA,NA,NA, 6,NA,NA,NA,NA,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA, 3,
			NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,
			NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA,NA), byrow=TRUE, ncol=13, nrow=13)

colnames(graph) <- c("s","a","b","c","d","e","f","g","h","i","j","k","l")
rownames(graph) <- c("s","a","b","c","d","e","f","g","h","i","j","k","l")
graph

hCost <- c(6,5,8,4,10,2,8,0,1,12,12,0,0)

a.star(graph, "s", c("g","k","l"), hCost)
ida.star(graph, "s", c("g","k","l"), hCost)