;;; 1. Added INEXACT->EXACT wrappers around READ.
;;; 2. Changed FOR-EACH-N to DO.
;;; 3. Eliminated SQR, MATRIX-REF, MATRIX-SET!, MATRIX-ROWS, MATRIX-COLUMNS,
;;;    MAKE-MATRIX, WHEN, and UNLESS.

;;; begin Stalin
(define make-pbm (primitive-procedure make-structure pbm 2))
(define pbm? (primitive-procedure structure? pbm))
(define pbm-raw? (primitive-procedure structure-ref pbm 0))
(define pbm-bitmap (primitive-procedure structure-ref pbm 1))
(define make-pgm (primitive-procedure make-structure pgm 3))
(define pgm? (primitive-procedure structure? pgm))
(define pgm-raw? (primitive-procedure structure-ref pgm 0))
(define pgm-maxval (primitive-procedure structure-ref pgm 1))
(define pgm-grey (primitive-procedure structure-ref pgm 2))
(define make-ppm (primitive-procedure make-structure ppm 5))
(define ppm? (primitive-procedure structure? ppm))
(define ppm-raw? (primitive-procedure structure-ref ppm 0))
(define ppm-maxval (primitive-procedure structure-ref ppm 1))
(define ppm-red (primitive-procedure structure-ref ppm 2))
(define ppm-green (primitive-procedure structure-ref ppm 3))
(define ppm-blue (primitive-procedure structure-ref ppm 4))
;;; end Stalin
;;; begin Scheme->C
(define (make-pbm raw? bitmap) (vector 'pbm raw? bitmap))
(define (pbm? pnm) (eq? (vector-ref pnm 0) 'pbm))
(define (pbm-raw? pnm) (vector-ref pnm 1))
(define (pbm-bitmap pnm) (vector-ref pnm 2))
(define (make-pgm raw? maxval grey) (vector 'pgm raw? maxval grey))
(define (pgm? pnm) (eq? (vector-ref pnm 0) 'pgm))
(define (pgm-raw? pnm) (vector-ref pnm 1))
(define (pgm-maxval pnm) (vector-ref pnm 2))
(define (pgm-grey pnm) (vector-ref pnm 3))
(define (make-ppm raw? maxval r g b) (vector 'ppm raw? maxval r g b))
(define (ppm? pnm) (eq? (vector-ref pnm 0) 'ppm))
(define (ppm-raw? pnm) (vector-ref pnm 1))
(define (ppm-maxval pnm) (vector-ref pnm 2))
(define (ppm-red pnm) (vector-ref pnm 3))
(define (ppm-green pnm) (vector-ref pnm 4))
(define (ppm-blue pnm) (vector-ref pnm 5))
(define (panic s) (error 'panic s))
;;; end Scheme->C
;;; begin Gambit-C
(define-structure pbm raw? bitmap)
(define-structure pgm raw? maxval grey)
(define-structure ppm raw? maxval red green blue)
(define (panic s) (error s))
;;; end Gambit-C
;;; begin Bigloo
(define (make-pbm raw? bitmap) (vector 'pbm raw? bitmap))
(define (pbm? pnm) (eq? (vector-ref pnm 0) 'pbm))
(define (pbm-raw? pnm) (vector-ref pnm 1))
(define (pbm-bitmap pnm) (vector-ref pnm 2))
(define (make-pgm raw? maxval grey) (vector 'pgm raw? maxval grey))
(define (pgm? pnm) (eq? (vector-ref pnm 0) 'pgm))
(define (pgm-raw? pnm) (vector-ref pnm 1))
(define (pgm-maxval pnm) (vector-ref pnm 2))
(define (pgm-grey pnm) (vector-ref pnm 3))
(define (make-ppm raw? maxval r g b) (vector 'ppm raw? maxval r g b))
(define (ppm? pnm) (eq? (vector-ref pnm 0) 'ppm))
(define (ppm-raw? pnm) (vector-ref pnm 1))
(define (ppm-maxval pnm) (vector-ref pnm 2))
(define (ppm-red pnm) (vector-ref pnm 3))
(define (ppm-green pnm) (vector-ref pnm 4))
(define (ppm-blue pnm) (vector-ref pnm 5))
(define (panic s) (error s 'panic 'panic))
;;; end Bigloo
;;; begin Chez
(define (make-pbm raw? bitmap) (vector 'pbm raw? bitmap))
(define (pbm? pnm) (eq? (vector-ref pnm 0) 'pbm))
(define (pbm-raw? pnm) (vector-ref pnm 1))
(define (pbm-bitmap pnm) (vector-ref pnm 2))
(define (make-pgm raw? maxval grey) (vector 'pgm raw? maxval grey))
(define (pgm? pnm) (eq? (vector-ref pnm 0) 'pgm))
(define (pgm-raw? pnm) (vector-ref pnm 1))
(define (pgm-maxval pnm) (vector-ref pnm 2))
(define (pgm-grey pnm) (vector-ref pnm 3))
(define (make-ppm raw? maxval r g b) (vector 'ppm raw? maxval r g b))
(define (ppm? pnm) (eq? (vector-ref pnm 0) 'ppm))
(define (ppm-raw? pnm) (vector-ref pnm 1))
(define (ppm-maxval pnm) (vector-ref pnm 2))
(define (ppm-red pnm) (vector-ref pnm 3))
(define (ppm-green pnm) (vector-ref pnm 4))
(define (ppm-blue pnm) (vector-ref pnm 5))
(define (panic s) (error 'panic s))
;;; end Chez
;;; begin Chicken
(define-record-type pbm raw? bitmap)
(define-record-type pgm raw? maxval grey)
(define-record-type ppm raw? maxval red green blue)
(define (panic s) (error s))
;;; end Chicken

(define (fuck-up) (panic "This shouldn't happen"))

(define first car)
(define second cadr)
(define third caddr)
(define rest cdr)

(define *max-grey* 255)

(define (pnm-width pnm)
 (vector-length (vector-ref (cond ((pbm? pnm) (pbm-bitmap pnm))
				  ((pgm? pnm) (pgm-grey pnm))
				  ((ppm? pnm) (ppm-red pnm))
				  (else (panic "Argument not PNM")))
			    0)))

(define (pnm-height pnm)
 (vector-length (cond ((pbm? pnm) (pbm-bitmap pnm))
		      ((pgm? pnm) (pgm-grey pnm))
		      ((ppm? pnm) (ppm-red pnm))
		      (else (panic "Argument not PNM")))))

(define (read-pnm pathname)
 (define (read-pnm port)
  (define (read-pbm raw?)
   (let* ((width (inexact->exact (read port)))
	  (height (inexact->exact (read port)))
	  (bitmap (let ((v (make-vector height)))
		   (let loop ((i 0))
		    (if (< i height)
			(begin (vector-set! v i (make-vector width))
			       (loop (+ i 1)))))
		   v)))
    (call-with-current-continuation
     (lambda (return)
      (cond
       (raw? (panic "Cannot (yet) read a raw pbm image"))
       (else
	(do ((y 0 (+ y 1))) ((= y height))
	 (do ((x 0 (+ x 1))) ((= x width))
	  (let ((v (read port)))
	   (if (eof-object? v) (return #f))
	   (vector-set! (vector-ref bitmap y) x (not (zero? v))))))))))
    (make-pbm raw? bitmap)))
  (define (read-pgm raw?)
   (let* ((width (inexact->exact (read port)))
	  (height (inexact->exact (read port)))
	  (maxval (inexact->exact (read port)))
	  (size (* width height))
	  (grey (let ((v (make-vector height)))
		 (let loop ((i 0))
		  (if (< i height)
		      (begin (vector-set! v i (make-vector width))
			     (loop (+ i 1)))))
		 v)))
    (call-with-current-continuation
     (lambda (return)
      (cond
       (raw? (read-char port)
	     (do ((y 0 (+ y 1))) ((= y height))
	      (do ((x 0 (+ x 1))) ((= x width))
	       (let ((c (read-char port)))
		(if (eof-object? c) (return #f))
		(vector-set! (vector-ref grey y) x (char->integer c))))))
       (else (do ((y 0 (+ y 1))) ((= y height))
	      (do ((x 0 (+ x 1))) ((= x width))
	       (let ((v (read port)))
		(if (eof-object? v) (return #f))
		(vector-set! (vector-ref grey y) x (inexact->exact v)))))))))
    (make-pgm raw? maxval grey)))
  (define (read-ppm raw?)
   (let* ((width (inexact->exact (read port)))
	  (height (inexact->exact (read port)))
	  (maxval (inexact->exact (read port)))
	  (size (* width height))
	  (red (let ((v (make-vector height)))
		(let loop ((i 0))
		 (if (< i height)
		     (begin (vector-set! v i (make-vector width))
			    (loop (+ i 1)))))
		v))
	  (green (let ((v (make-vector height)))
		  (let loop ((i 0))
		   (if (< i height)
		       (begin (vector-set! v i (make-vector width))
			      (loop (+ i 1)))))
		  v))
	  (blue (let ((v (make-vector height)))
		 (let loop ((i 0))
		  (if (< i height)
		      (begin (vector-set! v i (make-vector width))
			     (loop (+ i 1)))))
		 v)))
    (call-with-current-continuation
     (lambda (return)
      (cond
       (raw? (read-char port)
	     (do ((y 0 (+ y 1))) ((= y height))
	      (do ((x 0 (+ x 1))) ((= x width))
	       (let* ((c1 (read-char port))
		      (c2 (read-char port))
		      (c3 (read-char port)))
		(if (eof-object? c1) (return #f))
		(vector-set! (vector-ref red y) x (char->integer c1))
		(vector-set! (vector-ref green y) x (char->integer c2))
		(vector-set! (vector-ref blue y) x (char->integer c3))))))
       (else (do ((y 0 (+ y 1))) ((= y height))
	      (do ((x 0 (+ x 1))) ((= x width))
	       (let* ((v1 (read port))
		      (v2 (read port))
		      (v3 (read port)))
		(if (eof-object? v1) (return #f))
		(vector-set! (vector-ref red y) x (inexact->exact v1))
		(vector-set! (vector-ref green y) x (inexact->exact v2))
		(vector-set! (vector-ref blue y) x (inexact->exact v3)))))))))
    (make-ppm raw? maxval red green blue)))
  (let ((format (read port)))
   (case format
    ((P1) (read-pbm #f))
    ((P2) (read-pgm #f))
    ((P3) (read-ppm #f))
    ((P4) (read-pbm #t))
    ((P5) (read-pgm #t))
    ((P6) (read-ppm #t))
    (else (panic "Incorrect format for a pnm image")))))
 (if (string=? pathname "-")
     (read-pnm (current-input-port))
     (call-with-input-file pathname read-pnm)))

(define (write-pnm pnm pathname)
 (define (write-pnm port)
  (define (write-pbm pbm)
   (let ((width (pnm-width pbm))
	 (height (pnm-height pbm))
	 (bitmap (pbm-bitmap pbm)))
    (write (if (pbm-raw? pbm) 'P4 'P1) port)
    (newline port)
    (write width port)
    (write-char #\space port)
    (write height port)
    (newline port)
    (if (pbm-raw? pbm)
	(panic "Cannot (yet) write a raw pbm image")
	(do ((y 0 (+ y 1))) ((= y height))
	 (do ((x 0 (+ x 1))) ((= x width))
	  (write (if (vector-ref (vector-ref bitmap y) x) 1 0) port)
	  (newline port))))))
  (define (write-pgm pgm)
   (let ((width (pnm-width pgm))
	 (height (pnm-height pgm))
	 (grey (pgm-grey pgm)))
    (if (pgm-raw? pgm)
	(do ((y 0 (+ y 1))) ((= y height))
	 (do ((x 0 (+ x 1))) ((= x width))
	  (if (> (vector-ref (vector-ref grey y) x) 255)
	      (panic "Grey value too large for raw pgm file format")))))
    (write (if (pgm-raw? pgm) 'P5 'P2) port)
    (newline port)
    (write width port)
    (write-char #\space port)
    (write height port)
    (newline port)
    (write (pgm-maxval pgm) port)
    (newline port)
    (if (pgm-raw? pgm)
	(do ((y 0 (+ y 1))) ((= y height))
	 (do ((x 0 (+ x 1))) ((= x width))
	  (write-char
	   (integer->char (vector-ref (vector-ref grey y) x)) port)))
	(do ((y 0 (+ y 1))) ((= y height))
	 (do ((x 0 (+ x 1))) ((= x width))
	  (write (vector-ref (vector-ref grey y) x) port)
	  (newline port))))))
  (define (write-ppm ppm)
   (let ((width (pnm-width ppm))
	 (height (pnm-height ppm))
	 (red (ppm-red ppm))
	 (green (ppm-green ppm))
	 (blue (ppm-blue ppm)))
    (if (ppm-raw? ppm)
	(do ((y 0 (+ y 1))) ((= y height))
	 (do ((x 0 (+ x 1))) ((= x width))
	  (if (or (> (vector-ref (vector-ref red y) x) 255)
		  (> (vector-ref (vector-ref green y) x) 255)
		  (> (vector-ref (vector-ref blue y) x) 255))
	      (panic "Color value too large for raw ppm file format")))))
    (write (if (ppm-raw? ppm) 'P6 'P3) port)
    (newline port)
    (write width port)
    (write-char #\space port)
    (write height port)
    (newline port)
    (write (ppm-maxval ppm) port)
    (newline port)
    (if (ppm-raw? ppm)
	(do ((y 0 (+ y 1))) ((= y height))
	 (do ((x 0 (+ x 1))) ((= x width))
	  (write-char (integer->char (vector-ref (vector-ref red y) x)) port)
	  (write-char (integer->char (vector-ref (vector-ref green y) x)) port)
	  (write-char
	   (integer->char (vector-ref (vector-ref blue y) x)) port)))
	(do ((y 0 (+ y 1))) ((= y height))
	 (do ((x 0 (+ x 1))) ((= x width))
	  (write (vector-ref (vector-ref red y) x) port)
	  (newline port)
	  (write (vector-ref (vector-ref green y) x) port)
	  (newline port)
	  (write (vector-ref (vector-ref blue y) x) port)
	  (newline port))))))
  (cond ((pbm? pnm) (write-pbm pnm))
	((pgm? pnm) (write-pgm pnm))
	((ppm? pnm) (write-ppm pnm))
	(else (panic "Non-PNM argument to WRITE-PNM"))))
 (if (string=? pathname "-")
     (write-pnm (current-output-port))
     (call-with-output-file (string-append pathname
					   (cond ((pbm? pnm) ".pbm")
						 ((pgm? pnm) ".pgm")
						 ((ppm? pnm) ".ppm")
						 (else (fuck-up))))
      write-pnm
;;; begin Chez
      'replace
;;; end Chez
      )))

(define (pgm-smooth pgm sigma)
 (if (not (pgm? pgm)) (panic "Argument to PGM-SMOOTH is not a PGM"))
 (let* ((height (pnm-height pgm))
	(width (pnm-width pgm))
	(grey1 (pgm-grey pgm))
	(grey2 (let ((v (make-vector height)))
		(let loop ((i 0))
		 (if (< i height)
		     (begin (vector-set! v i (make-vector width 0))
			    (loop (+ i 1)))))
		v)))
  (do ((y sigma (+ y 1))) ((= y (- height sigma)))
   (do ((x sigma (+ x 1))) ((= x (- width sigma)))
    (do ((i (- y sigma) (+ i 1))) ((= i (+ y sigma 1)))
     (do ((j (- x sigma) (+ j 1))) ((= j (+ x sigma 1)))
      (vector-set!
       (vector-ref grey2 y) x (+ (vector-ref (vector-ref grey2 y) x)
				 (vector-ref (vector-ref grey1 i) j)))))
    (vector-set! (vector-ref grey2 y) x
		 (inexact->exact
		  (floor (/ (vector-ref (vector-ref grey2 y) x)
			    (* (+ sigma sigma 1) (+ sigma sigma 1))))))))
  (make-pgm (pgm-raw? pgm) *max-grey* grey2)))

(define (normal-flow-magnitude pgm1 pgm2 epsilon sigma sensitivity)
 (if (not (and (pgm? pgm1)
	       (pgm? pgm2)
	       (= (pgm-maxval pgm1) (pgm-maxval pgm2))
	       (eq? (pgm-raw? pgm1) (pgm-raw? pgm2))
	       (= (pnm-width pgm1) (pnm-width pgm2))
	       (= (pnm-height pgm1) (pnm-height pgm2))))
     (panic "Arguments to NORMAL-FLOW-MAGNITUDE are not matching PGMs"))
 (let* ((width (pnm-width pgm1))
	(height (pnm-height pgm1))
	(e1 (pgm-grey (pgm-smooth pgm1 sigma)))
	(e2 (pgm-grey (pgm-smooth pgm2 sigma)))
	(m (let ((v (make-vector height)))
	    (let loop ((i 0))
	     (if (< i height)
		 (begin (vector-set! v i (make-vector width 0))
			(loop (+ i 1)))))
	    v)))
  (do ((i 0 (+ i 1))) ((= i (- height 1)))
   (do ((j 0 (+ j 1))) ((= j (- width 1)))
    (let* ((ex (/ (- (+ (vector-ref (vector-ref e1 (+ i 1)) j)
			(vector-ref (vector-ref e1 (+ i 1)) (+ j 1))
			(vector-ref (vector-ref e2 (+ i 1)) j)
			(vector-ref (vector-ref e2 (+ i 1)) (+ j 1)))
		     (+ (vector-ref (vector-ref e1 i) j)
			(vector-ref (vector-ref e1 i) (+ j 1))
			(vector-ref (vector-ref e2 i) j)
			(vector-ref (vector-ref e2 i) (+ j 1))))
		  4.0))
	   (ey (/ (- (+ (vector-ref (vector-ref e1 i) (+ j 1))
			(vector-ref (vector-ref e1 (+ i 1)) (+ j 1))
			(vector-ref (vector-ref e2 i) (+ j 1))
			(vector-ref (vector-ref e2 (+ i 1)) (+ j 1)))
		     (+ (vector-ref (vector-ref e1 i) j)
			(vector-ref (vector-ref e1 (+ i 1)) j)
			(vector-ref (vector-ref e2 i) j)
			(vector-ref (vector-ref e2 (+ i 1)) j)))
		  4.0))
	   (et (/ (- (+ (vector-ref (vector-ref e2 i) j)
			(vector-ref (vector-ref e2 i) (+ j 1))
			(vector-ref (vector-ref e2 (+ i 1)) j)
			(vector-ref (vector-ref e2 (+ i 1)) (+ j 1)))
		     (+ (vector-ref (vector-ref e1 i) j)
			(vector-ref (vector-ref e1 i) (+ j 1))
			(vector-ref (vector-ref e1 (+ i 1)) j)
			(vector-ref (vector-ref e1 (+ i 1)) (+ j 1))))
		  4.0))
	   (l (sqrt (+ (* ex ex) (* ey ey)))))
     (vector-set!
      (vector-ref m i) j
      (min *max-grey*
	   (inexact->exact
	    (floor
	     (* *max-grey*
		(/ (if (< l epsilon) 0.0 (/ (abs et) l)) sensitivity)))))))))
  (pgm-smooth (make-pgm (pgm-raw? pgm1) *max-grey* m) sigma)))

(define (test pgm1 pgm2)
 (write-pnm (normal-flow-magnitude pgm1 pgm2 1.25 1 20.0) "pick-up-flow")
 #f)

(let ((pgm1 (read-pnm "pick-up00-0.pgm"))
      (pgm2 (read-pnm "pick-up00-1.pgm")))
 (do ((i 0 (+ i 1))) ((= i 10))
  (test pgm1 pgm2)
  (test pgm1 pgm2)))