temp A* pathfinding, with wiki as example

#used http://en.wikipedia.org/wiki/A*#Example as example

import pygame
import os.path
import math
BLACK = (0,0,0)
WHITE = (255,255,255)

UP = (0, -1)
DOWN = (0, 1)
LEFT = (-1, 0)
RIGHT = (1, 0)

pygame.init()

screen = pygame.display.set_mode((250,250))
screen.set_colorkey((WHITE))
pygame.display.set_caption('test')
screen.fill(WHITE)
clock = pygame.time.Clock()
basicFont = pygame.font.SysFont(None, 48)
pygame.display.flip()


#image = pygame.image.load(r'C:\Python26\projects\squares\art\man\1.png')
#image.set_colorkey((WHITE))


started = False

pygame.display.flip()
running = 1


class algo():
    def __init__(self):
        self.closedset = []
        self.openset = [] #put the first click here
        self.came_from = {} #where the algo has been, with all the node per pass looking to be eval

        self.g_score = {}
        self.h_score = {}
        self.f_score = {}

        self.path = []

    def dist_between(self, first, last):

        xs = last[0] - first[0]
        ys = last[1] - first[1]

        xs **= 2
        ys **= 2

        z = xs + ys

        return int(math.sqrt(z))

    def get_g(self, point, pointbefore):
        self.g_score[point] = self.g_score[pointbefore] + self.dist_between(pointbefore,point)
        return self.g_score[point]
    def get_h(self, point):
        self.h_score[point] = self.dist_between(point,self.END)
        return self.h_score[point]
    def get_f(self,point,pointbefore):

        self.f_score[point] = self.get_g(point,pointbefore) + self.get_h(point)
        return self.f_score[point]
    def first(self):

        self.g_score[self.START] = 0 #cost from start along best current path
        self.h_score[self.START] = self.dist_between(self.START, self.END) #distance straight line from A to B
        self.f_score[self.START] = self.g_score[self.START] + self.h_score[self.START] #cost of following current path from A to B
        #self.openset.append(self.START)
        #self.openset.sort()
        pass


    def start(self, START):
        self.START = START
        self.openset.append(self.START)

    def end(self, END):
        self.END = END
    def calc(self):
        while len(self.openset)is not 0:
            self.fscores = []

            for node in self.openset:
                self.fscores.append((self.f_score[node], node))
            self.fscores.sort()
            #print self.fscores
            #self.openset.sort()
            x = self.fscores[0][1]
            print 'lowest f_score in openset: ', x,'with f of ' ,self.fscores[0][0],', not ', self.fscores[-1][1],'with f of ' ,self.fscores[-1][0]
            print 'also, lowest f in open is {0}, while the highest is {1}'.format(min(self.fscores[0]),max(self.fscores[0]))
            print '*'* 10
            if x == self.END:
                self.path = self.reconstruct_path(self.came_from, self.came_from[self.END])
                for i in self.path:
                    screen.set_at(i,BLACK)
                pygame.display.flip()
                print 'OVER!!!',
                break
            self.openset.remove(x)
            self.closedset.append(x)
            nearby = self.neighborNodes(x)
            for y in nearby:
                if y in self.closedset:
                    #print 'got rid of ', y
                    continue
                self.tentative_g_score = self.g_score[x]+ self.dist_between(x,y)
                #print 'dist', self.dist_between(x,y)
                #self.tentative_is_better = False
                if y not in self.openset:
                    self.openset.append(y)
                    self.tentative_is_better = True
                elif self.tentative_g_score < self.g_score[y]:
                    self.tentative_is_better= True
                else:
                    self.tentative_is_better = False
                    #print 'tent is better'
                if self.tentative_is_better:
                    #screen.set_at(x, (255,255,0))
                    self.came_from[y] = x
                    self.g_score[y] = self.tentative_g_score
                    self.h_score[y] = self.dist_between(y, self.END)
                    self.f_score[y] = self.g_score[y] + self.h_score[y]
                    print 'g {0}, h {1}, f {2}, from point {3}'.format(self.g_score[y],self.h_score[y],self.f_score[y], y)
        print 'returned path'
        #print self.came_from
        return self.came_from
    def neighborNodes(self,cur):

        #cur = list(cur[1])

        left = tuple(sum(x) for x in zip(cur, LEFT))
        up = tuple(map(sum,zip(cur,UP)))
        right = tuple(map(sum,zip(cur,RIGHT)))
        down = tuple(map(sum,zip(cur,DOWN)))
        nodes = (left,up,right,down)
        unused = []
        for node in nodes:
            if node not in self.came_from:
                unused.append(node)
        return unused
    def reconstruct_path(self, came_from, current_node):
        #if self.came_from[current_node] is not None:
        if current_node in came_from.keys():
            p = self.reconstruct_path(came_from, came_from[current_node])
            return p + [current_node]

        else : return [current_node]

a= algo()
while running:

    screen.blit(image,(0,0))

    pygame.display.flip()

    for event in pygame.event.get():
        if event.type== pygame.QUIT:
            running = 0

        if event.type== pygame.MOUSEBUTTONUP:
            if not started:
                started = 1

                a.start(event.pos)
                print 'start point: ', event.pos
            elif started:
                started = 0
                a.end(event.pos)
                a.first()
                print 'end point: ', event.pos
        if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_c:
                print 'calc that shit'
                done = a.calc()

                print done

            elif event.key == pygame.K_SPACE:
                print 'skipping, shortcutting'
                a.start((10,10))
                a.end((20,20))
                a.first()
                done = a.calc()
                #print done
                #for point in done.keys():
                    #screen.set_at((point),(BLACK))
                pygame.display.flip()
                print 'done drawing'


    clock.tick(30)