;;; -*- Mode: Lisp; Syntax: Common-Lisp; -*- File: search/domains/route-finding
;;;; Find a Route Between Cities on a Map
;;; Defining Problems
;;;;;;;;;;;;;;;;;;;;;
(defstruct problem
  "A problem is defined by the initial state, and the type of problem it is.
  We will be defining subtypes of PROBLEM later on."
  (initial-state (required)) ; A state in the domain
  (goal nil)                 ; Optionally store the desired state here.
  (num-expanded 0)           ; Number of nodes expanded in search for solution.
  (iterative? nil)           ; Are we using an iterative algorithm?
  )

;;; Defining the route finding Problems
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defstruct (route-finding-problem (:include problem
					       (initial-state 'Arad)
					       (goal 'Bucharest)))
  "The problem of finding a route from one city to another on a map.
  A state in a route-finding problem is just the name of the current 
  city. Note that a more complicated version of this problem would 
  augment the state with considerations of time, gas used, wear on 
  car, tolls to pay, etc."
    						(map *romania-map*))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun successors (problem   city-name)
  "Return a list of (action . new-state) pairs.
  In this case, the action and the new state are both the name of the city."
  (let ((result nil))
    (setq   interm (city-neighbors (find-city city-name problem)))
      (do () ((equal (car interm) NIL)  (mapcar 'first result) ) 
      (push (cons (first (first interm)) (first (first interm))) result)
              (setq interm  (rest interm))
      
      
      )
  )
)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun  edge-cost (problem current-city   next-city)
  "The edge-cost is the road distance to the next city."
  (road-distance (find-city current-city  problem)   next-city)
)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun  h-cost (problem  city-name)
  "The heuristic cost is the straight-line distance to the goal."
  (straight-distance (find-city city-name problem)
		     (find-city (problem-goal problem) problem)))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;  Inserta los sucesores en la lista de caminos
(defun inserta_sucesores (sucesores camino)
 (cond ((null sucesores) nil)
       (T (cons (cons (car sucesores) camino) (inserta_sucesores (cdr sucesores) camino)))
 )
)


;-------------------------------- loop?
;recibe una lista de la forma:
;    (ORADEA ZERIND SIBIU ARAD)
;y retorna T si el primer estado (estado recientemente incertado) ya existe
;y NIL en caso contrario
 
(defun loop? (one-expanded-state)                                 
 (let ((st (car one-expanded-state)))
      (dolist (element (cdr one-expanded-state))
         (when (member st (list element)) (return t)))))   




;---------------------------- eliminate-loops
;recibe una lista de la forma:
;    ((FAGARAS SIBIU ARAD ZERIND) (RIMNICU SIBIU ARAD ZERIND))              
;que representa todas las posibles expansiones del ultimo estado estudiado
;devuelve la misma lista habiendo eliminado aquellas sublistas que llevan a un ciclo

(defun eliminate-loops (expanded-states &aux expanded-states-without-loops (h nil))
        (dolist (cs expanded-states expanded-states-without-loops)
           (when (not (loop? cs)) (setq expanded-states-without-loops
                                      (cons cs expanded-states-without-loops)))))



;;;; The City and Map data structures
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defstruct (city (:type list))
  "A city's loc (location) is an (x y) pair.  The neighbors slot holds
  a list of (city-name . distance-along-road) pairs.  Be careful to 
  distinguish between a city name and a city structure."
  name loc neighbors)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun road-distance (city1 city-name2)
  "The distance along the road between two cities.  The first is a city 
  structure, the second just the name of the intended destination."
  (if (eq (city-name city1) city-name2)
      0
    (cdr (assoc city-name2 (city-neighbors city1)))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun straight-distance (city1 city2)
  "Distance between two cities on a straight line (as the crow flies)."
  ;; We round this to the nearest integer, just to make things easier to read
  (round (xy-distance (city-loc city1) (city-loc city2))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun find-city (name map)
  "Look up the city on the map, and return its information."
  (if (problem-p map) (setf map (route-finding-problem-map map)))
  (assoc name map))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun xy-distance (loc1 loc2)
(sqrt (apply '+   (mapcar '* (mapcar '- loc1 loc2) (mapcar '- loc1 loc2) )))   )

;;;; The Romanian Map

(defparameter *romania-map*
  '(
    (Arad       ( 91 492) ((Zerind . 75) (Sibiu . 140) (Timisoara . 118)))
    (Bucharest	(400 327) ((Fagaras . 211) (Pitesti . 101) (Giurgiu . 90)
			   (Urziceni . 85)))
    (Craiova	(253 288) ((Dobreta . 120) (Rimnicu . 146) (Pitesti . 138)))
    (Dobreta	(165 299) ((Mehadia . 75) (Craiova . 120)))
    (Eforie	(562 293) ((Hirsova . 86)))
    (Fagaras	(305 449) ((Sibiu . 99) (Bucharest . 211)))
    (Giurgiu	(375 270) ((Bucharest . 90)))
    (Hirsova	(534 350) ((Urziceni . 98) (Eforie . 86)))
    (Iasi	(473 506) ((Neamt . 87) (Vaslui . 92)))
    (Lugoj	(165 379) ((Timisoara . 111) (Mehadia . 70)))
    (Mehadia	(168 339) ((Lugoj . 70) (Dobreta . 75)))
    (Neamt	(406 537) ((Iasi . 87)))
    (Oradea	(131 571) ((Zerind . 71) (Sibiu . 151)))
    (Pitesti	(320 368) ((Rimnicu . 97) (Craiova . 138) (Bucharest . 101)))
    (Rimnicu	(233 410) ((Sibiu . 80) (Pitesti . 97) (Craiova . 146)))
    (Sibiu	(207 457) ((Arad . 140) (Oradea . 151) (Fagaras . 99)
			   (Rimnicu . 80)))
    (Timisoara	( 94 410) ((Arad . 118) (Lugoj . 111)))
    (Urziceni	(456 350) ((Bucharest . 85) (Hirsova . 98) (Vaslui . 142)))
    (Vaslui	(509 444) ((Iasi . 92) (Urziceni . 142)))
    (Zerind	(108 531) ((Arad . 75) (Oradea . 71)))
    )
  "A representation of the map in Figure 4.2 [p 95].
  But note that the straight-line distances to Bucharest are NOT the same.")

; gets a list (ARAD CITY1 CITY2 .. ENDCITY)
; returns accumulated cost from ARAD - ENDCITY
(defun accum-cost (p l)
	(cond ((eql (length l) 1) 0)
	      (T (+ (edge-cost p (first l) (nth 1 l)) (accum-cost p (rest l))))
	)
)

(defun f-funct(p l)
  (+ (accum-cost p l ) (h-cost p (first l)))
)


(defun astar(p)

	(setq open-list (list (list (route-finding-problem-initial-state p))))
	(setq goal (route-finding-problem-goal p) )

	; como se puede poner valores en una estructura????
	; me gustaba poner el counter en el route-finding-problem
	; pero no quiero usar (make-route....) cada vez por razones de velocidad
	(setq counter 0)

	(do () ; while begin
	  ( 
	    (or (eql (first (first open-list)) goal ) (eql open-list NIL ) )
	    NIL 
	  )  

	(setq counter (+ 1 counter) )

	(setq actfirst (first open-list))
	(setq open-list (rest open-list))

	(setq temp1  (successors p (first actfirst)) )
	(setq temp2 (inserta_sucesores temp1 actfirst))
	(setq temp3 (eliminate-loops temp2))
	(setq temp3
	      (sort temp3 #' (lambda(x y) (< (f-funct p x) (f-funct p y))))
	)
	(setq open-list 
	 (merge 'list open-list temp3 
		#' (lambda(x y) (< (f-funct p x) (f-funct p y)))
	 )
	)
	) ; while end
	(list counter (accum-cost p (first open-list)) (first open-list))
)  

(defun profund_prio (p)
   (setq list-of-paths NIL)
   (setq camino (list (problem-initial-state p)))
   (setq city (problem-goal p))
   (setq n 0)
   (loop
      (progn
         (if (string-equal (first camino) city)
	   (return (list n (accum-cost p camino) camino))
	 )
         (setq llista (successors p (first camino)) )
         (setq novel-paths (inserta_sucesores llista camino))
         (setq novel-path-without-loops (eliminate-loops novel-paths))
         (setq list-of-paths (append novel-path-without-loops list-of-paths))
         (setq camino (first list-of-paths))
         (setq list-of-paths (rest list-of-paths))
         (setq n (+ n 1))
      ) 
   )
)

(defun amplitud_prio (p)
   (setq list-of-paths NIL)
   (setq camino (list (problem-initial-state p)))
   (setq city (problem-goal p))
   (setq n 0)
   (loop
      (progn
         (if (string-equal (first camino) city)
	   (return (list n (accum-cost p camino) camino))
	 )
         (setq llista (successors p (first camino)) )
         (setq novel-paths (inserta_sucesores llista camino))
         (setq novel-path-without-loops (eliminate-loops novel-paths))
         ;(setq list-of-paths (append novel-path-without-loops list-of-paths))
         (setq list-of-paths (append list-of-paths novel-path-without-loops))
         (setq camino (first list-of-paths))
         (setq list-of-paths (rest list-of-paths))
         (setq n (+ n 1))
      ) 
   )
)

(setq repuesta 
      "
      
      Ventajas de ASTAR: rapido, porque considera como las ciudades
      estan puestos en el campo. Si la function de heuristica esta bien 
      (nunca se sobrevalora), encontra solution optimal.
      Desventaja. No se puede aplicar a les problemas, donde
      falta una function heuristica adecuada. (sin heuristica ASTAR es 
      el algorithmo de Dijkstra.
      Ventajas de Amplitud_prio: entra bien en todos les problemas. 
      No conocimiento profundo del problema necesario, porque no
      function heuristica tiene que ser programado.
      Desventaja: en general lento como la RENFE, porque en el caso
      malo todo el espacio de la buesqueda tiene que ser buscado. 
      (tiempo exponential)"
)


(setq p 
      (make-route-finding-problem 
	:initial-state 'Dobreta
	:goal 'Fagaras
	:map *romania-map*))

(print "aestrela: ")
(princ (astar p))
(print "profund_prio: ")
(princ (profund_prio p))
(print "amplitud_prio: ")
(princ (amplitud_prio p))

(print repuesta)