 
 
 
-(defn preprocess-map 
-  "Returns a new, hopefully fast/compact c-space free-fn"
-  [width height free-fn radius]
-  (let [width (int width)
-	height (int height)
-	radius (int radius)
-	offsets (for [dx (range (- radius) (inc radius)), dy (range (- radius) (inc radius))
-		      :when (<= (+ (* dx dx) (* dy dy)) (* radius radius))]
-		  [dx dy])
-	fill-map (make-array Boolean/TYPE (* height width))]
-    (doseq [y (range height), x (range width)]
-      (let [x (int x) y (int y)]
-	(when (some (fn [[dx dy]]
-		      (let [dx (int dx)
-			    dy (int dy)
-			    nx (+ x dx)
-			    ny (+ y dy)]
-			(or (< nx 0) (>= nx width) (< ny 0) (>= ny height) (not (free-fn [nx ny])))))
-		offsets)
-	  (aset-boolean fill-map (+ x (* y width)) true)))
-;      (println x y (aget fill-map (idx-fn x y)))
-      )
-    (fn [[x y]]
-      (let [x (int x) y (int y)]
-	(and (aget fill-map (+ x (* y width)))
-	     (some (fn [[dx dy]]
-		     (let [dx (int dx) dy (int dy)
-			   nx (+ x dx), ny (+ y dy)]
-		       (if (and (>= nx 0) (< nx width) (>= ny 0) (< ny height) (not (aget fill-map (+ nx (* ny width)))))
-			 true nil)))
-		 [[-1 0] [1 0] [0 -1] [0 1]]))))))
-
 
 (let [f (util/path-local "nav_2d.pddl")]
   (defn make-nav-2d-strips-domain []
     (strips/read-strips-planning-domain f)))
 
  
-(defn make-nav-2d-strips-env [border-fn [width height] [init-x init-y] [goal-x goal-y]]
+(defn make-nav-2d-strips-env 
+  "map-fn takes [x y] pair, returns :free, :border, or :occupied"
+  [map-fn [width height] [init-x init-y] [goal-x goal-y]]
+  (util/assert-is (= :free (map-fn [init-x init-y])))
+  (util/assert-is (= :free (map-fn [goal-x goal-y])))
   (strips/make-strips-planning-instance 
    "nav-2d"
    (make-nav-2d-strips-domain)

	  (for [y (range height)]
	    (apply str
	      (for [x (range width)]
		  (cond (= [x y] pos)             "O"
			(= [x y] [goal-x goal-y]) "G"
			(border-fn [x y])         "X"
			(nil? (border-fn [x y]))  "@"
			:else                     ".")))))))))
    
(comment 



























(comment 
(defn preprocess-map 
  "Returns a new, hopefully fast/compact c-space free-fn"
  [width height free-fn radius]
  (let [width (int width)
	height (int height)
	radius (int radius)
	offsets (for [dx (range (- radius) (inc radius)), dy (range (- radius) (inc radius))
		      :when (<= (+ (* dx dx) (* dy dy)) (* radius radius))]
		  [dx dy])
	fill-map (make-array Boolean/TYPE (* height width))]
    (doseq [y (range height), x (range width)]
      (let [x (int x) y (int y)]
	(when (some (fn [[dx dy]]
		      (let [dx (int dx)
			    dy (int dy)
			    nx (+ x dx)
			    ny (+ y dy)]
			(or (< nx 0) (>= nx width) (< ny 0) (>= ny height) (not (free-fn [nx ny])))))
		offsets)
	  (aset-boolean fill-map (+ x (* y width)) true)))
;      (println x y (aget fill-map (idx-fn x y)))
      )
    (fn [[x y]]
      (let [x (int x) y (int y)]
	(and (aget fill-map (+ x (* y width)))
	     (some (fn [[dx dy]]
		     (let [dx (int dx) dy (int dy)
			   nx (+ x dx), ny (+ y dy)]
		       (if (and (>= nx 0) (< nx width) (>= ny 0) (< ny height) (not (aget fill-map (+ nx (* ny width)))))
			 true nil)))
		 [[-1 0] [1 0] [0 -1] [0 1]])))))) )

