(define (run-bench name count ok? run)
  (let loop ((i count) (result '(undefined)))
    (if (< 0 i)
      (loop (- i 1) (run))
      result)))

(define (run-benchmark name count ok? run-maker . args)
  (newline)
  (let* ((run (apply run-maker args))
         (result (run-bench name count ok? run)))
    (if (not (ok? result))
      (begin
        (display "*** wrong result ***")
        (newline)
        (display "*** got: ")
        (write result)
        (newline)))))

(define (fatal-error . args)
  (for-each display args)
  (newline)
  (exit 1))

 (define (call-with-output-file/truncate filename proc)
   (call-with-output-file filename proc))

(define-macro (bitwise-or . lst) `(logior ,@lst))
(define-macro (bitwise-and . lst) `(logand ,@lst))
(define-macro (bitwise-not . lst) `(lognot ,@lst))


; Don't specialize fixnum and flonum arithmetic.

(define-macro (FLOATvector-const . lst)   `',(list->vector lst))
(define-macro (FLOATvector? x)            `(vector? ,x))
(define-macro (FLOATvector . lst)         `(vector ,@lst))
(define-macro (FLOATmake-vector n . init) `(make-vector ,n ,@init))
(define-macro (FLOATvector-ref v i)       `(vector-ref ,v ,i))
(define-macro (FLOATvector-set! v i x)    `(vector-set! ,v ,i ,x))
(define-macro (FLOATvector-length v)      `(vector-length ,v))

(define-macro (nuc-const . lst)
  `',(list->vector lst))

(define-macro (FLOAT+ . lst) `(+ ,@lst))
(define-macro (FLOAT- . lst) `(- ,@lst))
(define-macro (FLOAT* . lst) `(* ,@lst))
(define-macro (FLOAT/ . lst) `(/ ,@lst))
(define-macro (FLOAT= . lst)  `(= ,@lst))
(define-macro (FLOAT< . lst)  `(< ,@lst))
(define-macro (FLOAT<= . lst) `(<= ,@lst))
(define-macro (FLOAT> . lst)  `(> ,@lst))
(define-macro (FLOAT>= . lst) `(>= ,@lst))
(define-macro (FLOATnegative? . lst) `(negative? ,@lst))
(define-macro (FLOATpositive? . lst) `(positive? ,@lst))
(define-macro (FLOATzero? . lst)     `(zero? ,@lst))
(define-macro (FLOATabs . lst) `(abs ,@lst))
(define-macro (FLOATsin . lst) `(sin ,@lst))
(define-macro (FLOATcos . lst) `(cos ,@lst))
(define-macro (FLOATatan . lst) `(atan ,@lst))
(define-macro (FLOATsqrt . lst) `(sqrt ,@lst))
(define-macro (FLOATmin . lst) `(min ,@lst))
(define-macro (FLOATmax . lst) `(max ,@lst))
(define-macro (FLOATround . lst) `(round ,@lst))
(define-macro (FLOATinexact->exact . lst) `(inexact->exact ,@lst))

(define simplex-iters   1)

(define (matrix-rows a) (vector-length a))
(define (matrix-columns a) (FLOATvector-length (vector-ref a 0)))
(define (matrix-ref a i j) (FLOATvector-ref (vector-ref a i) j))
(define (matrix-set! a i j x) (FLOATvector-set! (vector-ref a i) j x))

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

(define (simplex a m1 m2 m3)
 (define *epsilon* 1e-6)
 (if (not (and (>= m1 0)
               (>= m2 0)
               (>= m3 0)
               (= (matrix-rows a) (+ m1 m2 m3 2))))
  (fuck-up))
 (let* ((m12 (+ m1 m2 1))
        (m (- (matrix-rows a) 2))
        (n (- (matrix-columns a) 1))
        (l1 (make-vector n))
        (l2 (make-vector m))
        (l3 (make-vector m2))
        (nl1 n)
        (iposv (make-vector m))
        (izrov (make-vector n))
        (ip 0)
        (kp 0)
        (bmax 0.0)
        (one? #f)
        (pass2? #t))
  (define (simp1 mm abs?)
   (set! kp (vector-ref l1 0))
   (set! bmax (matrix-ref a mm kp))
   (do ((k 1 (+ k 1))) ((>= k nl1))
    (if (FLOATpositive?
         (if abs?
             (FLOAT- (FLOATabs (matrix-ref a mm (vector-ref l1 k)))
                     (FLOATabs bmax))
             (FLOAT- (matrix-ref a mm (vector-ref l1 k)) bmax)))
        (begin
         (set! kp (vector-ref l1 k))
         (set! bmax (matrix-ref a mm (vector-ref l1 k)))))))
  (define (simp2)
   (set! ip 0)
   (let ((q1 0.0)
         (flag? #f))
    (do ((i 0 (+ i 1))) ((= i m))
     (if flag?
         (if (FLOAT< (matrix-ref a (vector-ref l2 i) kp) (FLOAT- *epsilon*))
             (begin
              (let ((q (FLOAT/ (FLOAT- (matrix-ref a (vector-ref l2 i) 0))
                               (matrix-ref a (vector-ref l2 i) kp))))
               (cond
                ((FLOAT< q q1)
                 (set! ip (vector-ref l2 i))
                 (set! q1 q))
                ((FLOAT= q q1)
                 (let ((qp 0.0)
                       (q0 0.0))
                  (let loop ((k 1))
                   (if (<= k n)
                       (begin
                        (set! qp (FLOAT/ (FLOAT- (matrix-ref a ip k))
                                         (matrix-ref a ip kp)))
                        (set! q0 (FLOAT/ (FLOAT-
                                           (matrix-ref a (vector-ref l2 i) k))
                                         (matrix-ref a (vector-ref l2 i) kp)))
                        (if (FLOAT= q0 qp)
                            (loop (+ k 1))))))
                  (if (FLOAT< q0 qp)
                      (set! ip (vector-ref l2 i)))))))))
         (if (FLOAT< (matrix-ref a (vector-ref l2 i) kp) (FLOAT- *epsilon*))
             (begin
              (set! q1 (FLOAT/ (FLOAT- (matrix-ref a (vector-ref l2 i) 0))
                               (matrix-ref a (vector-ref l2 i) kp)))
              (set! ip (vector-ref l2 i))
              (set! flag? #t)))))))
  (define (simp3 one?)
   (let ((piv (FLOAT/ (matrix-ref a ip kp))))
    (do ((ii 0 (+ ii 1))) ((= ii (+ m (if one? 2 1))))
     (if (not (= ii ip))
         (begin
          (matrix-set! a ii kp (FLOAT* piv (matrix-ref a ii kp)))
          (do ((kk 0 (+ kk 1))) ((= kk (+ n 1)))
           (if (not (= kk kp))
               (matrix-set! a ii kk (FLOAT- (matrix-ref a ii kk)
                                            (FLOAT* (matrix-ref a ip kk)
                                                    (matrix-ref a ii kp)))))))))
    (do ((kk 0 (+ kk 1))) ((= kk (+ n 1)))
     (if (not (= kk kp))
         (matrix-set! a ip kk (FLOAT* (FLOAT- piv) (matrix-ref a ip kk)))))
    (matrix-set! a ip kp piv)))
  (do ((k 0 (+ k 1))) ((= k n))
   (vector-set! l1 k (+ k 1))
   (vector-set! izrov k k))
  (do ((i 0 (+ i 1))) ((= i m))
   (if (FLOATnegative? (matrix-ref a (+ i 1) 0))
       (fuck-up))
   (vector-set! l2 i (+ i 1))
   (vector-set! iposv i (+ n i)))
  (do ((i 0 (+ i 1))) ((= i m2)) (vector-set! l3 i #t))
  (if (positive? (+ m2 m3))
      (begin
       (do ((k 0 (+ k 1))) ((= k (+ n 1)))
        (do ((i (+ m1 1) (+ i 1)) (sum 0.0 (FLOAT+ sum (matrix-ref a i k))))
          ((> i m) (matrix-set! a (+ m 1) k (FLOAT- sum)))))
       (let loop ()
        (simp1 (+ m 1) #f)
        (cond
         ((FLOAT<= bmax *epsilon*)
          (cond ((FLOAT< (matrix-ref a (+ m 1) 0) (FLOAT- *epsilon*))
                 (set! pass2? #f))
                ((FLOAT<= (matrix-ref a (+ m 1) 0) *epsilon*)
                 (let loop ((ip1 m12))
                  (if (<= ip1 m)
                      (cond ((= (vector-ref iposv (- ip1 1)) (+ ip n -1))
                             (simp1 ip1 #t)
                             (cond ((FLOATpositive? bmax)
                                    (set! ip ip1)
                                    (set! one? #t))
                                   (else
                                    (loop (+ ip1 1)))))
                            (else
                             (loop (+ ip1 1))))
                      (do ((i (+ m1 1) (+ i 1))) ((>= i m12))
                       (if (vector-ref l3 (- i (+ m1 1)))
                           (do ((k 0 (+ k 1))) ((= k (+ n 1)))
                            (matrix-set! a i k (FLOAT- (matrix-ref a i k)))))))))
                (else (simp2) (if (zero? ip) (set! pass2? #f) (set! one? #t)))))
         (else (simp2) (if (zero? ip) (set! pass2? #f) (set! one? #t))))
        (if one?
            (begin
             (set! one? #f)
             (simp3 #t)
             (cond
              ((>= (vector-ref iposv (- ip 1)) (+ n m12 -1))
               (let loop ((k 0))
                (cond
                 ((and (< k nl1) (not (= kp (vector-ref l1 k))))
                  (loop (+ k 1)))
                 (else
                  (set! nl1 (- nl1 1))
                  (do ((is k (+ is 1))) ((>= is nl1))
                   (vector-set! l1 is (vector-ref l1 (+ is 1))))
                  (matrix-set! a (+ m 1) kp (FLOAT+ (matrix-ref a (+ m 1) kp) 1.0))
                  (do ((i 0 (+ i 1))) ((= i (+ m 2)))
                   (matrix-set! a i kp (FLOAT- (matrix-ref a i kp))))))))
              ((and (>= (vector-ref iposv (- ip 1)) (+ n m1))
                    (vector-ref l3 (- (vector-ref iposv (- ip 1)) (+ m1 n))))
               (vector-set! l3 (- (vector-ref iposv (- ip 1)) (+ m1 n)) #f)
               (matrix-set! a (+ m 1) kp (FLOAT+ (matrix-ref a (+ m 1) kp) 1.0))
               (do ((i 0 (+ i 1))) ((= i (+ m 2)))
                (matrix-set! a i kp (FLOAT- (matrix-ref a i kp))))))
             (let ((t (vector-ref izrov (- kp 1))))
              (vector-set! izrov (- kp 1) (vector-ref iposv (- ip 1)))
              (vector-set! iposv (- ip 1) t))
             (loop))))))
  (and pass2?
       (let loop ()
        (simp1 0 #f)
        (cond
         ((FLOATpositive? bmax)
          (simp2)
          (cond ((zero? ip) #t)
                (else (simp3 #f)
                      (let ((t (vector-ref izrov (- kp 1))))
                       (vector-set! izrov (- kp 1) (vector-ref iposv (- ip 1)))
                       (vector-set! iposv (- ip 1) t))
                      (loop))))
         (else (list iposv izrov)))))))

(define (test)
 (simplex (vector (FLOATvector 0.0 1.0 1.0 3.0 -0.5)
                  (FLOATvector 740.0 -1.0 0.0 -2.0 0.0)
                  (FLOATvector 0.0 0.0 -2.0 0.0 7.0)
                  (FLOATvector 0.5 0.0 -1.0 1.0 -2.0)
                  (FLOATvector 9.0 -1.0 -1.0 -1.0 -1.0)
                  (FLOATvector 0.0 0.0 0.0 0.0 0.0))
          2 1 1))

(define (main . args)
  (run-benchmark
    "simplex"
    simplex-iters
    (lambda (result) (equal? result '(#(4 1 3 2) #(0 5 7 6))))
    (lambda () (lambda () (test)))))
(main)