;; Copyright (C) 2012, SRI International.  All Rights Reserved.

;; This program is free software; you can redistribute it and/or
;; modify it under the terms of the GNU General Public License
;; as published by the Free Software Foundation; either version 2
;; of the License, or (at your option) any later version.

;; This program is distributed in the hope that it will be useful,
;; but WITHOUT ANY WARRANTY; without even the implied warranty of
;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
;; GNU General Public License for more details.

;;(in-package 'pvs)

;;Shostak-style online Gaussian elimination algorithm

(defvar *block-hash* (make-hash-table));;maps block names to GE-state
(defvar *number-of-dimensions* 5)
(defvar *free-internal-vars* t)

;;A monomial is a pair of a coefficient (any number) and a
;;variable (a natural number) 
(defstruct monom
  coeff
   variable)

(defstruct ge-state
  external-vars
  internal-vars
  solution ; was solution-array
  num-constraints
  constraint-stack)

;;allows the dimension constant to be represented by monom variable 0
(defun lin-monom-variable (monom)
  (let ((var (monom-variable monom)))
    (if (integerp var) var 0)))

;;A linear polynomial: a decreasingly sorted list of monoms
;;The variable 0 represents a constant, the coefficient must be nonzero.
(defun linomial? (monoms &optional maxvar)
  (cond ((consp monoms)
	 (let ((monom (car monoms)))
	   (and (numberp (monom-coeff  monom))
		(not (zerop  (monom-coeff  monom)))
		(or (null maxvar)
		    (< (lin-monom-variable  monom) maxvar));;was monom-variable
		(linomial? (cdr monoms) (monom-variable monom)))))
	(t)))

;;substitute solution into linom by substituting into monomials
;;and merging the results.
(defun subst-linom (solution linom)
  (cond ((consp linom)
	 (merge-linom  (subst-monom solution (car linom))
		       (subst-linom solution (cdr linom))))
	(t nil)))

;;merges two order linomials 
(defun merge-linom (linom1 linom2 &optional (scale1 1) (scale2 1))
  (cond ((and (consp linom1)(consp linom2))
	 (if (> (lin-monom-variable (car linom1))
		(lin-monom-variable (car linom2)))
	     (cons (scale-monom scale1 (car linom1))
		   (merge-linom (cdr linom1) linom2 scale1 scale2))
	   (if (< (lin-monom-variable (car linom1))
		  (lin-monom-variable (car linom2)))
	       (cons (scale-monom scale2 (car linom2))
		     (merge-linom linom1  (cdr linom2) scale1 scale2))
	     ;;the variables are the same
	     (let* ((new-coeff (+ (* scale1 (monom-coeff (car linom1)))
				     (* scale2 (monom-coeff (car linom2)))))
		   (new-monom (if (integerp (monom-variable (car linom1)))
				  (make-monom :coeff new-coeff :variable (monom-variable (car linom1)))
				(make-monom :coeff 1
					    :variable (vector-sum (monom-variable (car linom1)) (* scale1 (monom-coeff (car linom1)))
								  (monom-variable (car linom2)) (* scale2 (monom-coeff (car linom2)))))))
		   (new-cdr (merge-linom (cdr linom1)(cdr linom2) scale1 scale2)))
	       (if (and (zerop new-coeff)(integerp (monom-variable (car linom1))))
		   new-cdr
		 (cons new-monom new-cdr))))))
	((consp linom1) (scale-linom scale1 linom1))
	(t (scale-linom scale2 linom2))))

;push a monomial into an accumulator linomial
(defun push-monomial (monom linom)
  (push-monomial* monom linom))

;push-monomial* does the work of pushing the monom into the right position
(defun push-monomial* (monom linom)
  (cond ((not (consp linom))
	 (push monom linom))
	((> (lin-monom-variable monom)
	    (lin-monom-variable (car linom)))
	 (push monom linom))
	((< (lin-monom-variable monom)
	    (lin-monom-variable (car linom)))
	 (setf (cdr linom) (push-monomial* monom (cdr linom)))
	 linom)
	(t (incf (monom-coeff (car linom))
		 (monom-coeff (cdr linom)))
	   (if (zerop (monom-coeff (car linom)))
	       (pop linom))
	   linom
	   ))
  )
		 
		       
	     

(defun subst-monom (solution monom)
  (let ((monom-solution (unless (<= (lin-monom-variable monom) 0)
			  (svref solution (1- (monom-variable monom))))))
    (when monom-solution
      (format t "~%Substituting ")
      (print-linomial monom-solution)
      (format t " for ")
      (monom-variable-string (monom-variable monom)))
    (if monom-solution  ;;scale the rest of the solution
	(if (cdr monom-solution)
	    (scale-linom (/ (monom-coeff monom) (- (monom-coeff (car monom-solution))))
			 (cdr monom-solution))
	  (list (make-monom :coeff 0 :variable 0)))
      (list monom))))

(defun scale-monom (scale monom)
  (or (and (eql scale 1) monom)
      (make-monom :coeff (* scale (monom-coeff monom))
		  :variable (monom-variable monom))))
  
(defun scale-linom (factor linom)
  (loop for monom in linom
	collect (scale-monom factor monom)))

(defun report-inconsistency (linom)
  (format t "~%Detected an inconsistency involving constraints: ")
  (loop for monom in (cdr linom) do
	(format t "~%~d, " (abs (monom-variable monom))))
  (throw 'inconsistency nil))

(defun zero-vector? (v)
  (loop for i from 0 to (1- *number-of-dimensions*)
	always (zerop (svref v i))))

;;linom must be nonempty and have a variable
(defun add-linomial-to-solution (linom solution)
  (let ((solve-linom (subst-linom solution linom)))
    (format t "~%Solved ")
    (print-linomial linom)
    (format t "~%as ")
    (print-linomial solve-linom)
    (cond ((constant-linomial? solve-linom)
	   (if (and (= (lin-monom-variable (car solve-linom)) 0)
		    (not (zero-vector? (monom-variable (car solve-linom)))))
	       (report-inconsistency solve-linom)
	     t));;otherwise, the constraint is redundant
	  ((consp solve-linom)
	   (setf (svref solution (1- (monom-variable (car solve-linom))))
		 solve-linom)
	   (loop for index from (monom-variable (car solve-linom)) to (1- (length solution))
		 do (let ((index-entry (svref solution index)))
		      (when index-entry
			(format t "~%Replacing ")
			(print-linomial index-entry)
			(format t "~%with ")
			(setf (svref solution index)
			      (cons (car index-entry)
				    (subst-linom solution (cdr index-entry))))
			(print-linomial (svref solution index)))))
	   t)
	  (t))))

(defun constant-monom? (monom)
  (eql (lin-monom-variable monom) 0))

;checks that linom has a constant leading monomial
(defun constant-linomial? (linom)
  (and (consp linom)
       (<= (lin-monom-variable (car linom)) 0)))

(defun print-linomial (linom &optional no-zerovars?)
  (cond ((consp linom)(print-linomial* linom no-zerovars?))
	(t (format t "0"))))

(defun print-linomial* (linom no-zerovars?)
  (cond ((consp linom)
	 (cond ((and (consp (cdr linom))
		     (or (null no-zerovars?)
			 (>= (lin-monom-variable (cadr linom)) 0)))
		(print-monomial (car linom))
		(format t " + ")
		(print-linomial* (cdr linom) no-zerovars?))
	       (t (print-monomial (car linom)))))
	(t)))

(defun monom-variable-string (monom-variable)
  (if (< monom-variable 0)
      (format t "zerovar(~d)" (- monom-variable))
    (if (integerp monom-variable) 
	(format t "var(~d)" monom-variable)
      (format t "~s" monom-variable))))

(defun print-monomial (monom)
  (cond ((zerop (lin-monom-variable monom))
	 (format t "~d * ~s" (monom-coeff monom)(monom-variable monom)))
	((eql (monom-coeff monom) 1)
	 (monom-variable-string (monom-variable monom)))
	(t (format t "~d * " (monom-coeff monom))
	   (monom-variable-string (monom-variable monom)))))

(defun index-to-var (ge-state index)
  (with-slots (external-vars internal-vars) ge-state
	      (let ((elen (length external-vars))
		    ;(ilen (length internal-vars))
		    )
		(if (<= index elen)
		    (nth (1- index) external-vars)
		  (nth (- (1- index) elen) internal-vars)))))

(defun show-linomial (ge-state linom)
  (cond ((consp linom)
	 (cond ((consp (cdr linom))
		(show-monomial ge-state (car linom))
		(format t " + ")
		(show-linomial ge-state (cdr linom)))
	       (t (show-monomial ge-state (car linom)))))
	(t)))

(defun show-monomial (ge-state monom)
  (if (zerop (lin-monom-variable monom))
      (format t "~d * ~s" (monom-coeff monom) (monom-variable monom))
    (if (< (lin-monom-variable monom) 0)
	(format t "~d * zerovar(~d)"
		(monom-coeff monom)
		(monom-variable monom))
      (if (eql (monom-coeff monom) 1)
	  (format t "~s" (index-to-var ge-state (monom-variable monom)))
	
	    (format t "~d * ~s" (monom-coeff monom)
		    (index-to-var ge-state (monom-variable monom)))))))

	   
(defun push-free-internal-vars (ge-state num-external-vars linom)
  (cond ((consp linom)
	 (when (> (lin-monom-variable (car linom)) num-external-vars)
	   (push (index-to-var ge-state (monom-variable (car linom)))
		 *free-internal-vars*)
	   (push-free-internal-vars ge-state num-external-vars (cdr linom))))
	;if leading variable is not internal, no need to look further
	(t)))

(defun print-constraints (ge-state constraint-stack)
  (cond ((consp constraint-stack)
	 (let ((num (1+ (print-constraints ge-state (cdr constraint-stack))))
	       (top-constraint (car constraint-stack)))
	   (format t "~%~d:" num)
	   (if (and (consp top-constraint)
		    (monom-p (car top-constraint)))
	       (show-linomial ge-state top-constraint)
	     (format t "~s" top-constraint))
	   num))
	(t 0)))

(defun print-ge-state (ge-state)
  (format t "~%External vars: ~{ ~s,~}" (ge-state-external-vars ge-state))
  (format t "~%Internal vars: ~{ ~s,~}" (ge-state-internal-vars ge-state))
  (format t "~%Constraints:" )
  (print-constraints ge-state (ge-state-constraint-stack ge-state))
  (let ((dim-array (ge-state-solution ge-state))
	(num-external-vars (length (ge-state-external-vars ge-state))))
    (loop for index from 1 to num-external-vars
	  do (progn ()(format t "~%")
		    (show-linomial ge-state (svref dim-array (1- index)))))
    (let ((*free-internal-vars* nil))
      (loop for index from (1+ num-external-vars)
	    to (length (ge-state-internal-vars ge-state))
	    do (let ((linom (svref dim-array (1- index))))
		 (format t "~%") ;check for free internal vars in soln
		 (push-free-internal-vars ge-state
					  num-external-vars
					  (when linom (cdr linom)))
		 (show-linomial ge-state linom)))
      (when *free-internal-vars*
	(format t "~%Non-unique solution; contains: ~{ ~s,~}" *free-internal-vars*) ))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;For each block, the solutions are kept in a GE-state with a list of 
;;external and internal variables, and an array of solution states
;;Each solution state is itself an array.  Each variable is assigned
;;a number according to its position in the list of variables, with the external
;;variables getting the low numbers



;;Since index 0 represents a constant (monomial), the variables are numbered
;;from 1 upwards with the external ones first
(defun index-of-var (ge-state var)
  (let* ((external-vars (ge-state-external-vars ge-state))
	 (evar (position var external-vars)))
    (or (and evar (1+ evar))
	(let ((pos-ivar (position var (ge-state-internal-vars ge-state))))
	  (if pos-ivar (+ 1 (length external-vars) pos-ivar)
	    (format t "~%~a does not appear as an internal or external variable" var))))))

(defun vector-ref (v1 i);;treat non-vector as everywhere 0.
  (if (arrayp v1) (svref v1 i) 0))

(defun vector-sum (v1 scale1 v2 scale2)
  (let ((new-vector (make-array *number-of-dimensions*)))
    (loop for i from 1 to *number-of-dimensions*
	  do (setf (svref new-vector (1- i))(+ (* scale1 (svref v1 (1- i)))
					       (* scale2 (svref v2 (1- i))))))
    new-vector))
	

(defun insert-monomial (monom linom)
  (cond ((consp linom)
	 (if (< (lin-monom-variable monom)(lin-monom-variable (car linom)))
	     (cons (car linom)(insert-monomial monom (cdr linom)))
	   (if (> (lin-monom-variable monom)(lin-monom-variable (car linom)))
	       (cons monom linom)
	     (let* ((new-coeff (+ (monom-coeff monom)(monom-coeff (car linom))))
		    (mvar (monom-variable monom))
		    (new-mvar (if (zerop (lin-monom-variable monom))
				 (vector-sum mvar 1 (monom-variable (car linom)) 1)
			       mvar)))
	       (if (zerop new-coeff)
		   (cdr linom)
		 (cons (make-monom :coeff new-coeff
				   :variable new-mvar)
		       (cdr linom)))))))
	(t (list monom))))


;;renames the variables in linom (assuming they are all external)
;;to the variables in the list new-evars so that var i goes to the (i-1)th
;;entry in new-evars
(defun rename-external-vars (ge-state linom new-evars)
  (cond ((and (consp linom)
	      (>= (lin-monom-variable (car linom)) 0))
	 (let ((new-monom
		(if (eql (lin-monom-variable (car linom)) 0)
		    (car linom)
		  (make-monom :coeff (monom-coeff (car linom))
			     :variable (nth (1- (monom-variable (car linom)))
					     new-evars)))))
	 (insert-monomial new-monom
		 (rename-external-vars ge-state (cdr linom) new-evars))))
	(t nil)))
		     
		 

(defun make-defn-linomial (ge-state dim-expr scale linom)
  (cond ((symbolp dim-expr)
	 (push-monomial (make-monom :coeff scale
			 :variable (index-of-var ge-state dim-expr))
			linom))
	((eq (car dim-expr) '+)
	 (make-defn-linomial ge-state (caddr dim-expr)  scale
			     (make-defn-linomial ge-state (cadr dim-expr)  scale linom)))
	((eq (car dim-expr) '-)
	 (make-defn-linomial ge-state (caddr dim-expr)  (- scale)
			     (make-defn-linomial ge-state (cadr dim-expr) 
						 scale linom)))
	((eq (car dim-expr) 'inverse)
	 (make-defn-linomial ge-state (cadr dim-expr)  (- scale) linom))
	((eq (car dim-expr) 'mk-record)
	 (let ((vec (make-array *number-of-dimensions*)))
	   (loop for i from 1 to *number-of-dimensions*
		 do (setf (svref vec (1- i)) (nth (1- i) (cdr dim-expr))))
	   (push-monomial (make-monom :coeff scale :variable vec)
			  linom)))
	((eq (car dim-expr) 'increment)
	 (let ((new-vector (make-array *number-of-dimensions*)))
	   (loop for i from 0 to (1- *number-of-dimensions*)
		 do (setf (svref new-vector i)
			  (if (eql i (caddr dim-expr)) (cadddr dim-expr) 0)))
	     (make-defn-linomial ge-state (cadr dim-expr) scale
				 (push-monomial
				  (make-monom :coeff scale
					      :variable new-vector)
				  linom))))))


 (defun process-equality (ge-state solution constraint)
   (incf (ge-state-num-constraints ge-state))
   (push constraint (ge-state-constraint-stack ge-state))
   (process-dimension-equality ge-state solution constraint))

;;returns nil if the new constraint is inconsistent

(defun process-dimension-equality (ge-state dimension-array constraint)
  (or (null constraint)
      (let* ((linomial (make-defn-linomial ge-state constraint 1
					   (list (make-monom :coeff -1
							     :variable (- (ge-state-num-constraints ge-state)))))))
	(format t "~%Converted constraint ~s ~%to " constraint)
	(print-linomial linomial)
	(let ((result
	       (catch 'inconsistency 
		 (add-linomial-to-solution linomial dimension-array))))
	  (cond ((null result)
		 (format t "~%Inconsistency"))
		(t result))))))



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

(defun init-state (number-of-dimensions)
  (setq *block-hash* (make-hash-table))
  (setq *number-of-dimensions* number-of-dimensions)
  t)

(defmacro define-block (block-name external-vars
	       		 internal-vars
			 sub-blocks 
			 constraints)
  `(define-block* 
     (quote ,block-name)(quote ,external-vars)
     (quote ,internal-vars)(quote ,sub-blocks)(quote ,constraints)))



;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;The variables for a block are declared

(defun declare-variables (ge-state external-vars internal-vars)
  (let ((numvars (+ (length external-vars)(length internal-vars))))
    (setf (ge-state-external-vars ge-state) external-vars)
    (setf (ge-state-internal-vars ge-state) internal-vars)
    (setf (ge-state-solution ge-state) (make-array numvars))
    (setf (ge-state-num-constraints ge-state) 0)
    ge-state))

    
(defun define-block* (block-name external-vars
		      internal-vars
		      sub-blocks 
		      constraints)
  (let* ((ge-state (make-ge-state)))
    (declare-variables ge-state external-vars internal-vars)
      (when (and (import-sub-block-constraints ge-state sub-blocks)
		 (process-block-constraints ge-state constraints))
	(setf (gethash block-name *block-hash*) ge-state))
      (print-ge-state ge-state)))


(defun process-block-constraints (ge-state constraints)
  (loop for constr in constraints always
	(process-equality ge-state (ge-state-solution ge-state) constr)))


(defun import-sub-block-constraints (ge-state sub-blocks)
  (loop for sblock in sub-blocks
	always (import-constraints ge-state sblock)))

(defun import-constraints (ge-state sblock)
  (let* ((sb-name (car sblock))
	 (sb-args (cdr sblock))
	 (sb-args-evars (loop for var in sb-args
			      collect (index-of-var ge-state var))))
    (let ((sb-ge-state (gethash sb-name *block-hash*)))
      (assert (= (length sb-args)(length (ge-state-external-vars sb-ge-state))))
      (let ((ge-state-solution (ge-state-solution ge-state))
	    (sb-solution  (ge-state-solution sb-ge-state)))
	(loop for var in sb-args-evars
		    as var-pos from 0
		    always
		    (let* ((var-constraint (svref sb-solution var-pos))
			   (renamed-constraint (rename-external-vars ge-state
								     var-constraint sb-args-evars)))
		      (format t "~%Importing from ~s" sb-name)
		      (print-linomial var-constraint t)
		      (format t "~%as ")
		      (print-linomial renamed-constraint)
		      (incf (ge-state-num-constraints ge-state))
		      (push renamed-constraint (ge-state-constraint-stack ge-state))
		      (add-linomial-to-solution
		       (push-monomial
			(make-monom :coeff -1 :variable (- (ge-state-num-constraints ge-state)))
			renamed-constraint)
		       ge-state-solution)))))))


;; (defvar *block-hash*);;maps block names to GE-state
;; (defar *GE-var-array* (make-array 100 :adjustable t :fill-pointer t))
;; (defvar *GE-solution* (make-hash-table))

;; (defun add-GE-var (var GE-var-hash GE-var-array)
;;   (let ((entry (gethash var GE-var-hash)))
;;     (and (null entry)
;; 	 (let ((index (vector-push-extend var GE-var-array)))
;; 	   (setf (gethash var GE-var-hash) index))
;; 	 t)))

;; (defun make-dim-block(external internal constraints)
;;   (let ((block-state (make-ge-state
;; 		      :external external
;; 		      :internal internal
;; 		      :solution-array (make-array 5))))
;;    (process-block-constraints block-state constraints))
;;   block-state)

;; (defun process-block-constraints (block-state constraints)
;;   (loop for constr in constraints do 
;; 	(process-equality block-state constraints))
;;   block-state)
;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; (defun GE-dimension-constraints (file)
;;   (clrhash *GE-var-hash*)
;;   (clrhash *GE-solution*)
;;   (setf (fill-pointer *GE-var-array*) 0)
;;   (with-open-file (stream file :direction :input)
;; 		  (process-GE-input stream)))

;; (defun process-GE-input (stream GE-var-hash GE-var-array GE-solution)
;;   (let ((input (read stream)))
;;     (cond ((not (consp input))
;; 	   (format t "~%Bad input ~s" input))
;; 	  ((eq (car input) 'define)
;; 	   (add-GE-variables (cadr input)
;; 			     (caddr input)
;; 			     GE-var-hash GE-var-array GE-solution))
;; 	  ((and (eq (car input) 'assert)
;; 		(consp (cadr input))
;; 		(eq (car (cadr input)) '=))
;; 	   (add-GE-equality (cadr (cadr input))
;; 			    (caddr (cadr input))
;; 			    GE-var-hash GE-var-array GE-solution)))))

;; (defstruct dimensions length mass time temperature currency)

;; ;;adds variables for each dimension, and then adds equality with rhs, if any
;; (defun add-GE-variables (var rhs GE-var-hash GE-var-array GE-solution)
;;   (add-GE-var (intern (format t "~a__~a" var "length")) GE-var-hash var)
;;   (add-GE-var (intern (format t "~a__~a" var "mass")))
;;   (add-GE-var (intern (format t "~a__~a" var "time")))
;;   (add-GE-var (intern (format t "~a__~a" var "temperature")))
;;   (add-GE-var (intern (format t "~a__~a" var "currency")))
;;   (or (null (consp rhs))
;;       (add-GE-equality var rhs GE-var-hash GE-var-array GE-solution)))

;; (defun rhs-linomial (rhs dimension)
;;   (cond ((consp rhs)
;; 	 (case (car rhs)
;; 	   (mk-record (

;; (defun add-GE-equality (var rhs GE-var-hash GE-var-array GE-solution)
;;   (case (car rhs)
;;     (mk-record (add-GE-dimensionwise-equality-record var (cdr rhs) GE-var-hash GE-var-array GE-solution))
;;     (add-dim (add-GE-dimensionwise-equality-plus var rhs
		      
  
	     



	
	      
