; **********************************************************************
; *                                                                    *
; *  The procedures in this file filter propositional belief state     *
; *  formulae with STRIPS actions and observations.                    *
; *  They take a set of action descriptions                            *
; *  (preconditions and effects), a sequence of actions and            *
; *  observations, and an initial belief state in PI-CNF (prime        *
; *  implicate CNF). They output a belief state in PI-CNF.             *
; *                                                                    *
; *  Author: Eyal Amir                                                 *
; *  Date:  January 7, 2003                                            *
; *                                                                    *
; **********************************************************************

;----------------------------------------------------------------------
		     
(defun get-list-item (identifier given-list)
  (find-if #'(lambda (x) (and (listp x) (equal identifier (car x))))
	   given-list)
)

;----------------------------------------------------------------------
		     
(defun get-list-item2 (identifier1 identifier2 given-list)
  (find-if #'(lambda (x) (and (listp x) (equal identifier1 (car x))
			      (equal identifier2 (cadr x))))
	   given-list)
)

;----------------------------------------------------------------------
		     
(defun get-list-item-rest (identifier given-list)
  (remove-if #'(lambda (x) (and (listp x) (equal identifier (car x))))
	   given-list)
)


;----------------------------------------------------------------------
		     
(defun recursive-substitute (new-item old-item tree)
  (if tree ; else nil
      (if (not (listp tree)) (if (equal old-item tree) new-item tree) ; stop
	(cons (recursive-substitute new-item old-item (car tree))
	      (recursive-substitute new-item old-item (cdr tree)))
	)
    ))



;----------------------------------------------------------------------

(defun mod1 (x m)
  (+ 1 (mod (- x 1) m))
)

;----------------------------------------------------------------------

(defun list-to-string (lst)
  (if lst              ; otherwise, return nil
      (if (cdr lst)         ; if list has more than one item
	  (format nil "~A~A" (write-to-string (car lst))
		  (list-to-string (cdr lst)))
	(write-to-string (car lst))) ; if list has length 1
    )
)

;----------------------------------------------------------------------

(defun get-subsequent (start lst)
  (nth (+ 1 (position start lst)) lst)
)

;----------------------------------------------------------------------

(defun get-literals (formula)
  (setq literals
	(if formula ; else nil
	    (if (listp formula)
		(if (or (equal (car formula) 'and)
			(equal (car formula) 'or))
		    (get-literals (cdr formula))
		  (if (listp (car formula))
		      (append 
		       (get-literals (car formula))
		       (get-literals (cdr formula)))
		    (list formula)))
	      (if (not (or (equal formula 'and)
			   (equal formula 'or)
			   (equal formula 'variable)))
		  (list formula)) ; else nil
	      )
	  ))
  (remove-duplicates literals :from-end T :test #'equal)
)

;----------------------------------------------------------------------

(defun get-fluents (formula)
  (setq fluents
	(if formula ; else nil
	    (if (listp formula)
		(if (or (equal (car formula) 'and)
			(equal (car formula) 'or)
			(equal (car formula) 'not))
		    (loop for obj in (cdr formula)
			  append
			  (get-fluents obj))
		  (list formula))
	      (list formula))))
  (remove-duplicates fluents :from-end T :test #'equal))


;----------------------------------------------------------------------
		     
(defun find-recursive (fluents clause)
  (if clause
      (if (not (listp clause)) (if (find clause fluents) T nil)
	(if (or (find-recursive fluents (car clause))
		(find-recursive fluents (cdr clause))) T nil)
	)
    )
  )


;----------------------------------------------------------------------

; We implemented this as a loop because it turns up being a very long
; recursion, even for moderate numbers of fluents (~400), which in turn
; breaks some lisp implementations.
; Alas, the recursive version (below) is faster, but unstable (for now).

(defun remove-clauses (fluents beliefState)
  (setq bs0 beliefState)
  (setq newBeliefState nil)

  (loop while bs0
    do
    (if (and (not (equal (car bs0) 'and)) 
	     (not (intersection fluents (get-fluents (car bs0))
				 :test #'equal)))
					; add this clause only if no fluent
					; from the list appears in the clause.
	(setq newBeliefState (cons (car bs0) newBeliefState)))
    (setq bs0 (cdr bs0))
    )

  (if (equal (car beliefState) 'and)
      (setq newBeliefState (cons 'and newBeliefState)))

  newBeliefState
  )

;----------------------------------------------------------------------
		     
;(defun remove-clauses (fluents beliefState)
;  (if (equal beliefState nil) nil
;    (append 
;     (if (equal (car beliefState) 'and) '(and)
;       (if (intersection fluents (get-fluents (car beliefState))
;			 :test #'equal)
;	   nil
;	 (list (car beliefState)))
;     )
;     (remove-clauses fluents (cdr beliefState)))
;   )
;  )

;----------------------------------------------------------------------

; group-literals: append the list of literals to each clause in lst

(defun group-literals (lst)
  (if lst
      (cons (list (car lst) (get-literals (car lst)))
	    (group-literals (cdr lst))))
)

;----------------------------------------------------------------------

(defun get-clauses (lst)
  (if lst
      (cons (caar lst) (get-clauses (cdr lst)))
    )
)

;----------------------------------------------------------------------

(defun remove-subsumed (lst)
  (if (member 'and lst)
      (let nil
	(setq locallst (remove 'and lst :test #'equal))
	(setq myAnd 'and))
    (let nil
      (setq myAnd nil)
      (setq locallst lst)))

  (setq locallst-lit (group-literals locallst))

  (setq newlst
	(remove-duplicates
	 locallst-lit
	 :from-end T 
	 :test #'(lambda (x y)
		   (subsetp (cadr x) (cadr y) :test #'equal))))

  (setq newlst (get-clauses newlst))

  (if myAnd (cons myAnd newlst)
    newlst)
)


;----------------------------------------------------------------------
		     
(defun add-clauses (beliefState1 beliefState2)
  (setq beliefState
	(remove-subsumed (append beliefState1 beliefState2)))
  )

;=========================================================================

(defun getContent (filename)
  (setq filestr (open filename :direction :input))
  (format *standard-output* "reading file : ~S~%" filename)
  (setq content (read filestr)) ; reads a list from the file
  (close filestr)
  content
)

;=========================================================================

(defun write-to-file (lst filename comment)
  (setq outfilestr (open filename :direction :output :if-exists :supersede :if-does-not-exist :create))
  (format outfilestr comment)
  (prin1 lst outfilestr)
  (close outfilestr)
)

;=========================================================================

(defun append-to-file (filename comment)
  (setq outfilestr (open filename :direction :output :if-exists :append :if-does-not-exist :create))
  (format outfilestr comment)
  (close outfilestr)
)

;=========================================================================
		     
(defun filter-act (action domain beliefState)
  (setq action-def (get-list-item2 ':action action domain))
  (setq preconds (get-subsequent ':precondition action-def))
  (setq effects (get-subsequent ':effect action-def))

  (setq effA (get-fluents effects))
  (setq preA (get-fluents preconds))

  (setq beliefState (remove-clauses effA beliefState))
  (setq beliefState (add-clauses effects beliefState))
)


;=========================================================================

(defun filter-obs (observation beliefState)
  (add-clauses observation beliefState) ; **** change to complete prime implicates and remove subsumed clauses (can avoid this as long as observations are single literals
)

;=========================================================================

(defun all-permutations (variables objects)
  (if (equal nil variables) (list nil)
    (let ((var nil))
      (setq var (car variables))
      (setq permutations-togo (all-permutations (cdr variables) objects))
      (loop for obj in objects
	append
	(loop for perm in permutations-togo
	  collect
	  (cons (list var obj) perm)))
      )
    )
)


;=========================================================================

(defun get-permutation (combination)
  (loop for item in combination
    collect (cadr item))
)


;=========================================================================

(defun propositionalize-action (action-def objects)
  (let ((action-name nil)
	(params nil)
	(preconds nil)
	(effects nil))
    (setq action-name (cadr action-def))
    (setq params (get-subsequent ':parameters action-def))
    (setq preconds (get-subsequent ':precondition action-def))
    (setq effects (get-subsequent ':effect action-def))

    (setq combinations (all-permutations params objects))

    (loop for comb in combinations
      collect
      (let ((curr-action action-def))
	(loop for substitution in comb
	  do
	  (setq curr-action
		(recursive-substitute (cadr substitution)
				      (car substitution)
				      curr-action)))
	(substitute (cons action-name (get-permutation comb))
		    action-name
		    curr-action))
      )
    )
  )

;=========================================================================

(defun propositionalize (domain objects)
  (loop for clause in domain
    append 
    (if (and (listp clause) (equal ':action (car clause)))
	(propositionalize-action clause objects)
      (list clause))
    )
)


;==========================================================================

(defun STRIPS-filter-ret (domainName sequenceName beliefs0Name beliefsNName propositional objectsName collect-data)
  
  (setq domainFile (format nil "~A.pddl" domainName))
  (setq sequenceFile (format nil "~A.seq" sequenceName))
  (setq beliefs0File (format nil "~A.bel" beliefs0Name))

  (setq domain (getContent domainFile))
  (setq belief0 (getContent beliefs0File))
  (setq sequence (getContent sequenceFile))

  (if (not propositional)
      (let nil
	(setq objectsFile (format nil "~A.objs" objectsName))
	(setq objects (getContent objectsFile))
	(setq domain (propositionalize domain objects))
	(prin1 "Finished propositionalization of domain")
	))

  (if collect-data
      (let nil
	(setq time-data nil)
	(setq space-data nil)
	))

  
; (get-universal-time)
  (setq start-time (get-internal-real-time))
  (setq curr-step 0)

  (let nil ; main loop -- after all this preparation
    (setq beliefState belief0)
;	  (setq output-string *trace-output*)
;	  (setq *trace-output* outfilestr)
	  
    (loop while (not (equal nil sequence))
	  do
	  (setq curr-step (+ 1 curr-step))
	  (let nil
	    (setq action (car sequence))
	    (setq observation (cadr sequence))
	    (setq beliefState (filter-act action domain beliefState))
	    (setq beliefState (filter-obs observation beliefState))
	    (setq sequence (cddr sequence)))
	  (if collect-data
	      (let nil
		(setq time-per-step
		      (round (/ 
			      (- (get-internal-real-time) start-time)
			      curr-step)))
		(setq time-data (cons time-per-step time-data))
		(setq space-data (cons (list-length beliefState) space-data))))
	  ))

;  internal-time-units-per-second

  (if collect-data
      (list time-data space-data beliefState)
    beliefState)
)


;==========================================================================

(defun STRIPS-filter (domainName sequenceName beliefs0Name beliefsNName propositional objectsName)
  (setq beliefState (STRIPS-filter-ret domainName sequenceName
				       beliefs0Name beliefsNName
				       propositional objectsName nil))
  (setq outfile (format nil "~A.bel" beliefsNName))
  (write-to-file beliefState outfile "; Filtered belief state is presented below.~%")
)

;======================================================================

(defun random-sequence (domainName length propositional objectsName
				   outFileName) 
  (setq domainFile (format nil "~A.pddl" domainName))
  (setq domain (getContent domainFile))

  (if (not propositional)
      (let nil
	(setq objectsFile (format nil "~A.objs" objectsName))
	(setq objects (getContent objectsFile))
	(setq domain (propositionalize domain objects))
	(prin1 "Finished propositionalization of domain")
	))

  (setq action-num (- (length domain) 4)) ; *** Should be made more general
  (setq sequence
	(loop for i from 1 to length
	  append
	  (list (cadr (nth (+ 4 (random action-num)) domain))
		nil) ; creates an action and observation, with nil observations
	  ))

  (setq outfile (format nil "~A.seq" outFileName))
  (write-to-file sequence outfile "; Random action/observation sequence is presented below.~%")
)

;======================================================================

(defun main-prop-single ()
  (STRIPS-filter "blocks-prop-domain" "blocks-sequence-prop"
	  "blocks-begin" "blocks-end-prop" t nil)
)

;======================================================================

(defun main-single ()
  (STRIPS-filter "blocks-domain"
		 "ijcai03-exper-dir/ijcai03-exper/blocks-rand-seq3x10"
		 "blocks-begin"
		 "ijcai03-exper-dir/ijcai03-exper/blocks-end3x10"
		 nil "ijcai03-exper-dir/ijcai03-exper/blocks-inst3")
)

;======================================================================

(defun main-random (num-objects)
  (setq old-trace-output *trace-output*)
  (loop for i in '(10 20 30 40 50 60 70 80 90 100 120 140 160 180 200
		      250 300 350 400 450 500 600 700 800 900 1000)
    do
    (setq randSeqFile 
	  (format nil "ijcai03-exper-dir/ijcai03-exper/blocks-rand-seq~Ax~A" num-objects i))
    (setq blocksInstFile 
	  (format nil "ijcai03-exper-dir/ijcai03-exper/blocks-inst~A" num-objects))
    (setq blocksEndFile 
	  (format nil "ijcai03-exper-dir/ijcai03-exper/blocks-end~Ax~A" num-objects i))

    (random-sequence "blocks-domain" i nil
		     blocksInstFile
		     randSeqFile)

    (setq outfile (format nil "~A.bel" blocksEndFile))
    (setq outfilestr 
	  (open outfile
		:direction :output
		:if-exists :supersede
		:if-does-not-exist :create))
    (format outfilestr "; Filtered belief state for domain ~A with ~A object after ~A actions/observations is presented below.~%" "blocks-domain" num-objects i)
    (setq *trace-output* outfilestr)
    (setq beliefState (STRIPS-filter-ret "blocks-domain"
					 randSeqFile  
					 "blocks-begin"
					 blocksEndFile
					 nil 
					 blocksInstFile nil)) ; timed inside
    (format outfilestr "~%")
    (prin1 beliefState outfilestr)
    (close outfilestr)
    (format t "Finished processing ~A~%" randSeqFile)
    )
  (setq *trace-output* old-trace-output)
)

;======================================================================

(defun breadth-exper (seq-len)
  (setq old-trace-output *trace-output*)
  (loop for num-objects from 3 to 20
    do
    (setq randSeqFile 
	  (format nil "ijcai03-exper-dir/ijcai03-breadth/blocks-rand-seq~Ax~A" num-objects seq-len))
    (setq blocksInstFile 
	  (format nil "ijcai03-exper-dir/ijcai03-exper/blocks-inst~A" num-objects))
    (setq blocksEndFile 
	  (format nil "ijcai03-exper-dir/ijcai03-breadth/blocks-end~Ax~A" num-objects seq-len))

    (random-sequence "blocks-domain" seq-len nil
		     blocksInstFile
		     randSeqFile)

    (setq outfile (format nil "~A.bel" blocksEndFile))
    (setq outfilestr 
	  (open outfile
		:direction :output
		:if-exists :supersede
		:if-does-not-exist :create))
    (format outfilestr "; Filtered belief state for domain ~A with ~A object after ~A actions/observations is presented below.~%" "blocks-domain" num-objects seq-len)
    (setq *trace-output* outfilestr)
    (setq beliefState (STRIPS-filter-ret "blocks-domain"
					 randSeqFile  
					 "blocks-begin"
					 blocksEndFile
					 nil 
					 blocksInstFile nil)) ; timed inside
    (format outfilestr "~%")
    (prin1 beliefState outfilestr)
    (close outfilestr)
    (format t "Finished processing ~A~%" randSeqFile)
    )
  (setq *trace-output* old-trace-output)
)



;======================================================================

(defun single-exper (num-objects seq-len)
  (setq old-trace-output *trace-output*)
  (setq randSeqFile 
	(format nil "ijcai03-exper-dir/ijcai03-breadth/blocks-rand-seq~Ax~A" num-objects seq-len))
  (setq blocksInstFile 
	(format nil "ijcai03-exper-dir/ijcai03-exper/blocks-inst~A" num-objects))
  (setq blocksEndFile 
	(format nil "ijcai03-exper-dir/ijcai03-breadth/blocks-end~Ax~A" num-objects seq-len))
  
  (random-sequence "blocks-domain" seq-len nil
		   blocksInstFile
		   randSeqFile)
  
  (setq outfile (format nil "~A.bel" blocksEndFile))
  (setq outfilestr 
	(open outfile
	      :direction :output
	      :if-exists :supersede
	      :if-does-not-exist :create))
  (format outfilestr "; Filtered belief state for domain ~A with ~A object after ~A actions/observations is presented below.~%" "blocks-domain" num-objects seq-len)
  (setq *trace-output* outfilestr)
  (setq beliefState (STRIPS-filter-ret "blocks-domain"
				       randSeqFile  
				       "blocks-begin"
				       blocksEndFile
				       nil 
				       blocksInstFile nil)) ; timed inside
  (format outfilestr "~%")
  (prin1 beliefState outfilestr)
  (close outfilestr)
  (format t "Finished processing ~A~%" randSeqFile)
  
  (setq *trace-output* old-trace-output)
)


;======================================================================

(defun single-collect-random (num-objects seq-len)
  (setq old-trace-output *trace-output*)

  (setq randSeqFile 
	(format nil "ijcai03-exper-dir/ijcai03-exper3/blocks-rand-seq~Ax~A" num-objects seq-len))
  (setq blocksInstFile 
	(format nil "ijcai03-exper-dir/ijcai03-domains/blocks-inst~A" num-objects))
  (setq blocksEndFile 
	(format nil "ijcai03-exper-dir/ijcai03-exper3/blocks-end~Ax~A" num-objects seq-len))
  
  (random-sequence "blocks-domain" seq-len nil
		   blocksInstFile
		   randSeqFile)
  
  (setq outfile (format nil "~A.bel" blocksEndFile))
  (setq outfilestr 
	(open outfile
	      :direction :output
	      :if-exists :supersede
	      :if-does-not-exist :create))
  (format outfilestr "; Filtered belief state for domain ~A with ~A object after ~A actions/observations is presented below.~%" "blocks-domain" num-objects seq-len)
;  (setq *trace-output* outfilestr)
;  (setq *trace-output* nil)
  (setq filter-pack (STRIPS-filter-ret "blocks-domain"
				       randSeqFile  
				       "blocks-begin"
				       blocksEndFile
				       nil 
				       blocksInstFile T)) ; timed inside
  (setq time-data (car filter-pack))
  (setq space-data (cadr filter-pack))
  (setq beliefState (caddr filter-pack))
  
  (format outfilestr "; Time per step~%")
  (prin1 time-data outfilestr)
  (format outfilestr "~%; Space (#literals) per step~%")
  (prin1 space-data outfilestr)
  (format outfilestr "~%; Final belief state~%")
  (prin1 beliefState outfilestr)
  (close outfilestr)
  (format t "Finished processing ~A~%" randSeqFile)
  
;  (setq *trace-output* old-trace-output)
)


;======================================================================

(defun main ()
  (STRIPS-filter "blocks-domain"
		 "ijcai03-exper-dir/ijcai03-exper/blocks-rand-seq3x10"
		 "blocks-begin"
;		 "ijcai03-exper-dir/ijcai03-exper/blocks-end3x10"
		 "ijcai03-exper-dir/for-alex-out/blocks-end3x10"
		 nil "ijcai03-exper-dir/ijcai03-exper/blocks-inst3")
)

;======================================================================

(defun main-collect ()
  (loop for num-objects from 3 to 20
    do
    (single-collect-random num-objects 4000)
    )
)

; an example of how to run experiments:
; (single-collect-random 10 4000)