(let
	((predecessor (make-hash-table :size 20))
	(dist-from-start (make-hash-table :size 20))
	(already-visited (make-hash-table :size 20))
	(expansion-list nil)
	(first-round t)
	(output nil)
	(possible-moves nil)
	(starting nil)
	(goal nil)
	(stood-at nil)
	(standing-at nil))

(labels 

((set-predecessor (x y)
								 (setf (gethash x predecessor) y))
(get-predecessor (x)
								 (gethash x predecessor))

(set-already (x)
								 (setf (gethash x already-visited) t))
(get-already (x)
								 (gethash x already-visited))

(set-dist-from-start (x y)
								 (setf (gethash x dist-from-start) y))
(get-dist-from-start (x)
								 (gethash x dist-from-start))

(insert (node lst)
				(cond ((null lst)(list node))
     	        ((< (get-dist-from-start node) (get-dist-from-start (first lst)))
               (cons node lst) )
				      (t (cons (first lst)(insert node (rest lst)))) ) )

(starting () 
		(setf startp (first(get-list 'startpos output))) 
		(setf goal (first(get-list 'goalpos output))) 
		(setf standing-at startp)
		(setf first-round nil)
		(set-dist-from-start startp 0)
		(set-already startp))

(get-list (expr lst) 
					(cond 
						((null lst)(nil))
						((equal (first(first lst)) expr) (rest(first lst)))
						(t (get-list expr (rest lst)))))

(test-move (move cost) 
	(if
		(or (null (get-dist-from-start move)) 
	 		(< (+(get-dist-from-start standing-at) cost) (get-dist-from-start move)))
				(progn
					(set-dist-from-start move 
						(+ (get-dist-from-start standing-at) cost))
	 				(setf expansion-list (insert move expansion-list))
					(set-predecessor move standing-at))))

(test-possible-moves () 
	(setf possible-moves (get-list 'posmoves output))
	(loop for x in possible-moves
		do (test-move (first x)(first(rest x)))))

(calculate-next-expansion () 
	(print expansion-list)
	(loop until (null (get-already (first expansion-list)))
				do 
					(setf expansion-list (rest expansion-list)))
	(setf stood-at standing-at)
	(setf standing-at (first expansion-list))	
	(setf expansion-list (rest expansion-list))
	(set-already standing-at)
	 standing-at)

(get-final-path (field)
	(if (null field) nil
	(append (get-final-path (get-predecessor field))(list field))))

)
#'(lambda (out)
(print out)
;posmoves not in a list
(if (equal (first out) 'posmoves) (setf out (list out)))

(if (equal (first out) 'stop)
	(progn 
		(print out)
		(print (get-final-path standing-at))
		(print (get-dist-from-start standing-at))
		(quit)))

(setf output out)
(if (equal first-round t) (starting))


(if (equal (first output) 'error)
	(setf standing-at stood-at)
	(test-possible-moves))

(calculate-next-expansion)

;(print "Your input here: ")
;(setf input (read))
)))

