(require "f2cl_macros")

(defun jacobi (a n np d v nrot &key (nmax 100))
 (declare (type (simple-array double-float (* *)) a)) (declare (type fixnum n))
 (declare (type fixnum np)) (declare (type (simple-array double-float (*)) d))
 (declare (type (simple-array double-float (* *)) v))
 (declare (type fixnum nrot)) (declare (type fixnum nmax))
 (prog
  ((b (make-array nmax :element-type 'double-float))
   (z (make-array nmax :element-type 'double-float)) (j 0) (tau 0.0d0)
   (s 0.0d0) (c 0.0d0) (theta 0.0d0) (t_ 0.0d0) (h 0.0d0) (g 0.0d0)
   (tresh 0.0d0) (sm 0.0d0) (i 0) (iq 0) (ip 0)
  )
  (declare (type (simple-array double-float (*)) b))
  (declare (type (simple-array double-float (*)) z)) (declare (type fixnum j))
  (declare (type double-float tau)) (declare (type double-float s))
  (declare (type double-float c)) (declare (type double-float theta))
  (declare (type double-float t_)) (declare (type double-float h))
  (declare (type double-float g)) (declare (type double-float tresh))
  (declare (type double-float sm)) (declare (type fixnum i))
  (declare (type fixnum iq)) (declare (type fixnum ip))
  (fdo ((ip 1 (+ ip 1))) ((> ip n) nil)
   (tagbody
    (fdo ((iq 1 (+ iq 1))) ((> iq n) nil) (tagbody (fset (fref v ip iq) 0.0)))
    (fset (fref v ip ip) 1.0)
  ))
  (fdo ((ip 1 (+ ip 1))) ((> ip n) nil)
   (tagbody (fset (fref b ip) (fref a ip ip)) (fset (fref d ip) (fref b ip))
    (fset (fref z ip) 0.0)
  ))
  (setf nrot 0)
  (fdo ((i 1 (+ i 1))) ((> i 50) nil)
   (tagbody (setf sm 0.0)
    (fdo ((ip 1 (+ ip 1))) ((> ip (+ n (- 1))) nil)
     (tagbody
      (fdo ((iq (+ ip 1) (+ iq 1))) ((> iq n) nil)
       (tagbody (setf sm (+ sm (abs (fref a ip iq)))))
    )))
    (if (= sm 0.0) (go end_label))
    (cond ((< i 4) (setf tresh (/ (* 0.2 sm) (expt n 2)))) (t (setf tresh 0.0))
    )
    (fdo ((ip 1 (+ ip 1))) ((> ip (+ n (- 1))) nil)
     (tagbody
      (fdo ((iq (+ ip 1) (+ iq 1))) ((> iq n) nil)
       (tagbody (setf g (* 100.0 (abs (fref a ip iq))))
        (cond
         ((and (> i 4) (= (+ (abs (fref d ip)) g) (abs (fref d ip)))
           (= (+ (abs (fref d iq)) g) (abs (fref d iq)))
          )
          (fset (fref a ip iq) 0.0)
         )
         ((> (abs (fref a ip iq)) tresh)
          (setf h (+ (fref d iq) (- (fref d ip))))
          (cond ((= (+ (abs h) g) (abs h)) (setf t_ (/ (fref a ip iq) h)))
           (t (setf theta (/ (* 0.5 h) (fref a ip iq)))
            (setf t_ (/ 1.0 (+ (abs theta) (sqrt (+ 1.0 (expt theta 2))))))
            (if (< theta 0.0) (setf t_ (- t_)))
          ))
          (setf c (/ 1.0 (sqrt (+ 1 (expt t_ 2))))) (setf s (* t_ c))
          (setf tau (/ s (+ 1.0 c))) (setf h (* t_ (fref a ip iq)))
          (fset (fref z ip) (+ (fref z ip) (- h)))
          (fset (fref z iq) (+ (fref z iq) h))
          (fset (fref d ip) (+ (fref d ip) (- h)))
          (fset (fref d iq) (+ (fref d iq) h)) (fset (fref a ip iq) 0.0)
          (fdo ((j 1 (+ j 1))) ((> j (+ ip (- 1))) nil)
           (tagbody (setf g (fref a j ip)) (setf h (fref a j iq))
            (fset (fref a j ip) (+ g (* (* -1 s) (+ h (* g tau)))))
            (fset (fref a j iq) (+ h (* s (+ g (* (* -1 h) tau)))))
          ))
          (fdo ((j (+ ip 1) (+ j 1))) ((> j (+ iq (- 1))) nil)
           (tagbody (setf g (fref a ip j)) (setf h (fref a j iq))
            (fset (fref a ip j) (+ g (* (* -1 s) (+ h (* g tau)))))
            (fset (fref a j iq) (+ h (* s (+ g (* (* -1 h) tau)))))
          ))
          (fdo ((j (+ iq 1) (+ j 1))) ((> j n) nil)
           (tagbody (setf g (fref a ip j)) (setf h (fref a iq j))
            (fset (fref a ip j) (+ g (* (* -1 s) (+ h (* g tau)))))
            (fset (fref a iq j) (+ h (* s (+ g (* (* -1 h) tau)))))
          ))
          (fdo ((j 1 (+ j 1))) ((> j n) nil)
           (tagbody (setf g (fref v j ip)) (setf h (fref v j iq))
            (fset (fref v j ip) (+ g (* (* -1 s) (+ h (* g tau)))))
            (fset (fref v j iq) (+ h (* s (+ g (* (* -1 h) tau)))))
          ))
          (setf nrot (+ nrot 1))
    ))))))
    (fdo ((ip 1 (+ ip 1))) ((> ip n) nil)
     (tagbody (fset (fref b ip) (+ (fref b ip) (fref z ip)))
      (fset (fref d ip) (fref b ip)) (fset (fref z ip) 0.0)
  ))))
  (error "50 iterations should never happen") (go end_label) end_label
  (return (values a n np d v nrot))
))