#!/usr/bin/python
# D. Vrajitoru, C463/B551 Spring 2008

# Implementation of a class for an undirected, weighted graph, and of
# the algorithm A* applied to it.

import string

fval = []

# A class storing an undirected, weighted graph with distances between
# nodes.
class Graph:
    # Constructor
    def __init__(self, nr_nodes=0):
        # some data attributes; no need to declare them before
        self.nedges = 0
        self.nodes = []
        self.edges = []
        self.cost = []
        self.distances = []
        self.tot_cost = []
        self.pred = []
        self.nnodes = nr_nodes
        for i in range(self.nnodes):
            self.nodes.append(i)
            self.edges.append([])
            self.cost.append([])
            self.tot_cost.append(0)
            self.pred.append(0)
            fval.append(0)
            self.distances.append([])
            #distances is a nxn table initializes to 0
            for j in range(self.nnodes):
                self.distances[i].append(0)
                self.cost[i].append(0)
            

    # A method for explicitly building a small graph
    def readfile(self, filename):
        fin = open(filename, "r") #open the file for reading
        line = string.split(fin.readline())
        self.nnodes = int(line[0])
        self.__init__(self.nnodes)
        self.nedges = int(line[1])
        #skip an empty line
        line = string.split(fin.readline())
        # read all the edges
        for e in range(self.nedges):
            line = string.split(fin.readline())
            first = int(line[0])
            second = int(line[1])
            weight = float(line[2])
            self.edges[first].append(second)
            self.edges[second].append(first)
            self.cost[first][second] = weight
            self.cost[second][first] = weight
        #skip an empty line
        line = string.split(fin.readline())
        #read all the distances
        for n in range(self.nnodes):
            line = string.split(fin.readline())
            for m in range(self.nnodes - n -1):
                p = n+m+1
                weight = float(line[m])
                self.distances[n][p] = weight
                self.distances[p][n] = weight
        fin.close()

    # Printing out the information in the graph
    def output(self):
        print "Number of nodes:", self.nnodes
        print "Number of edges:", self.nedges
        print "Edges:\n", self.edges
        print "Cost:\n", self.cost
        print "Distances:\n", self.distances
        print "Total cost:", self.tot_cost
        print "Predecessors:", self.pred

    # Printing the path from destination to the origin
    def print_path(self, origin, dest):
        x = dest
        path = ""
        while x != origin:
            path = "%s %d" %(path, x)
            x = self.pred[x]
        print "The path is:", path, origin

    # Initializes all the data attributes used by A* in case we want
    # to run it several times.
    def init_A_star(self):
        for i in range(self.nnodes):
            try:
                self.tot_cost[i] = 0
            except:
                self.tot_cost.append(0)
            try:
                self.pred[i] = 0
            except:
                self.pred.append(0)
            try:
                fval[i] = 0
            except:
                fval.append(0)
        

# A shallow copy of a list
def copy_list(list):
    newlist = []
    for x in list:
        newlist.append(x)
    return newlist

# Defining it as a function to make it more clear
def reach_goal(state, goal):
    return (state == goal)

# Deletes the first element from the list and returns its value.
def pop_state(list):
    if not list:
        return False
    value = list[0]
    del list[0]
    return value

# Generates all the children of a state and updates the predecessors
# and total cost for each of them. Note that the total cost and
# predecessors should be updated only if we've never seen the child
# before or if we see it on a new path and the new total cost is less
# than the one before because in this case we found a better path to
# it.
def expand_state(graph, state, close_list):
    children = copy_list(graph.edges[state])
    for c in children:
        if (not c in close_list and
            graph.tot_cost[c] == 0 or
            graph.tot_cost[state] + graph.cost[state][c] < graph.tot_cost[c]):
            graph.pred[c] = state
            graph.tot_cost[c] = graph.tot_cost[state] + graph.cost[state][c]
    return children

# Generates all the children of a state and updates the predecessors
# and total cost for each of them, except for those that are already
# in the close list. This is used by rbfs and so it ignores previous
# values of the total_cost and predecessor computed on a different
# path.
def expand_state_rbfs(graph, state, close_list):
    children = []
    for c in graph.edges[state]:
        if not c in close_list:
            graph.pred[c] = state
            graph.tot_cost[c] = graph.tot_cost[state] + graph.cost[state][c]
            children.append(c)
    return children

# Evaluates the f value for all the states in the list based on their
# distance to the given destination.
def eval_f(graph, list, dest):
    for state in list:
        try:
            fval[state] = graph.tot_cost[state] + graph.distances[state][dest]
        except:
            print "err", state, fval

# Compares the f values for the two nodes for the purpose of sorting a
# list of nodes by this criterium.
def f_value(x, y):
    if fval[x] < fval[y]:
        return -1
    elif fval[x] == fval[y]:
        return 0
    else:
        return 1

# An implementation of the A* algorithm using the Graph class
# implemented above.
def A_star(graph, initial, dest):
    graph.init_A_star()
    open_list = [initial]
    close_list = []
    goal = -1 # because False == 0 which could be a valid node
    print "Starting A*\n"
    while open_list and goal == -1:
        print open_list #, fval
        state = pop_state(open_list)
        if state in close_list:
            continue
        else:
            close_list.append(state)
        if (reach_goal(state, dest)):
            goal = state
        else:
            children = expand_state(graph, state, close_list)
            open_list += children
            eval_f(graph, open_list, dest)
            open_list.sort(f_value)
    return goal

def rbfs(graph, state, best_alt, dest, close_list):
    print "At:", state,"Best alt:", best_alt, "Path:",
    print close_list, "F-values:", fval
    if state in close_list:
        return -1
    close_list.append(state)
    if state == -1 or reach_goal(state, dest): 
        return state
    else:
        children = expand_state_rbfs(graph, state, close_list)
        if not children:
            del close_list[len(close_list)-1]
            return -1
        eval_f(graph, children, dest)
        goal = -1
        while goal == -1:
            children.sort(f_value)
            child = children[0]
            if fval[child] > fval[state]:
                fval[state] = fval[child]
            if fval[child] > best_alt:
                del close_list[len(close_list)-1]
                return -1
            alt = best_alt
            if len(children) > 1 and fval[children[1]] < best_alt:
                alt = fval[children[1]]
            goal = rbfs(graph, child, alt, dest, close_list)
        return goal

#Example of creating a graph and calling the A* on it.
g = Graph()
g.readfile("roman.txt")
#g.output()
A_star(g, 4, 12)
g.print_path(4,12)
g.init_A_star()
close_list = []
rbfs(g, 4, 2000, 12, close_list)
g.print_path(4,12)