*> \brief \b CCHKGB
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*  Definition:
*  ===========
*
*       SUBROUTINE CCHKGB( DOTYPE, NM, MVAL, NN, NVAL, NNB, NBVAL, NNS,
*                          NSVAL, THRESH, TSTERR, A, LA, AFAC, LAFAC, B,
*                          X, XACT, WORK, RWORK, IWORK, NOUT )
*
*       .. Scalar Arguments ..
*       LOGICAL            TSTERR
*       INTEGER            LA, LAFAC, NM, NN, NNB, NNS, NOUT
*       REAL               THRESH
*       ..
*       .. Array Arguments ..
*       LOGICAL            DOTYPE( * )
*       INTEGER            IWORK( * ), MVAL( * ), NBVAL( * ), NSVAL( * ),
*      $                   NVAL( * )
*       REAL               RWORK( * )
*       COMPLEX            A( * ), AFAC( * ), B( * ), WORK( * ), X( * ),
*      $                   XACT( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> CCHKGB tests CGBTRF, -TRS, -RFS, and -CON
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] DOTYPE
*> \verbatim
*>          DOTYPE is LOGICAL array, dimension (NTYPES)
*>          The matrix types to be used for testing.  Matrices of type j
*>          (for 1 <= j <= NTYPES) are used for testing if DOTYPE(j) =
*>          .TRUE.; if DOTYPE(j) = .FALSE., then type j is not used.
*> \endverbatim
*>
*> \param[in] NM
*> \verbatim
*>          NM is INTEGER
*>          The number of values of M contained in the vector MVAL.
*> \endverbatim
*>
*> \param[in] MVAL
*> \verbatim
*>          MVAL is INTEGER array, dimension (NM)
*>          The values of the matrix row dimension M.
*> \endverbatim
*>
*> \param[in] NN
*> \verbatim
*>          NN is INTEGER
*>          The number of values of N contained in the vector NVAL.
*> \endverbatim
*>
*> \param[in] NVAL
*> \verbatim
*>          NVAL is INTEGER array, dimension (NN)
*>          The values of the matrix column dimension N.
*> \endverbatim
*>
*> \param[in] NNB
*> \verbatim
*>          NNB is INTEGER
*>          The number of values of NB contained in the vector NBVAL.
*> \endverbatim
*>
*> \param[in] NBVAL
*> \verbatim
*>          NBVAL is INTEGER array, dimension (NNB)
*>          The values of the blocksize NB.
*> \endverbatim
*>
*> \param[in] NNS
*> \verbatim
*>          NNS is INTEGER
*>          The number of values of NRHS contained in the vector NSVAL.
*> \endverbatim
*>
*> \param[in] NSVAL
*> \verbatim
*>          NSVAL is INTEGER array, dimension (NNS)
*>          The values of the number of right hand sides NRHS.
*> \endverbatim
*>
*> \param[in] THRESH
*> \verbatim
*>          THRESH is REAL
*>          The threshold value for the test ratios.  A result is
*>          included in the output file if RESULT >= THRESH.  To have
*>          every test ratio printed, use THRESH = 0.
*> \endverbatim
*>
*> \param[in] TSTERR
*> \verbatim
*>          TSTERR is LOGICAL
*>          Flag that indicates whether error exits are to be tested.
*> \endverbatim
*>
*> \param[out] A
*> \verbatim
*>          A is COMPLEX array, dimension (LA)
*> \endverbatim
*>
*> \param[in] LA
*> \verbatim
*>          LA is INTEGER
*>          The length of the array A.  LA >= (KLMAX+KUMAX+1)*NMAX
*>          where KLMAX is the largest entry in the local array KLVAL,
*>                KUMAX is the largest entry in the local array KUVAL and
*>                NMAX is the largest entry in the input array NVAL.
*> \endverbatim
*>
*> \param[out] AFAC
*> \verbatim
*>          AFAC is COMPLEX array, dimension (LAFAC)
*> \endverbatim
*>
*> \param[in] LAFAC
*> \verbatim
*>          LAFAC is INTEGER
*>          The length of the array AFAC. LAFAC >= (2*KLMAX+KUMAX+1)*NMAX
*>          where KLMAX is the largest entry in the local array KLVAL,
*>                KUMAX is the largest entry in the local array KUVAL and
*>                NMAX is the largest entry in the input array NVAL.
*> \endverbatim
*>
*> \param[out] B
*> \verbatim
*>          B is COMPLEX array, dimension (NMAX*NSMAX)
*> \endverbatim
*>
*> \param[out] X
*> \verbatim
*>          X is COMPLEX array, dimension (NMAX*NSMAX)
*> \endverbatim
*>
*> \param[out] XACT
*> \verbatim
*>          XACT is COMPLEX array, dimension (NMAX*NSMAX)
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is COMPLEX array, dimension
*>                      (NMAX*max(3,NSMAX,NMAX))
*> \endverbatim
*>
*> \param[out] RWORK
*> \verbatim
*>          RWORK is REAL array, dimension
*>                      (max(NMAX,2*NSMAX))
*> \endverbatim
*>
*> \param[out] IWORK
*> \verbatim
*>          IWORK is INTEGER array, dimension (NMAX)
*> \endverbatim
*>
*> \param[in] NOUT
*> \verbatim
*>          NOUT is INTEGER
*>          The unit number for output.
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \date December 2016
*
*> \ingroup complex_lin
*
*  =====================================================================
      SUBROUTINE CCHKGB( DOTYPE, NM, MVAL, NN, NVAL, NNB, NBVAL, NNS,
     $                   NSVAL, THRESH, TSTERR, A, LA, AFAC, LAFAC, B,
     $                   X, XACT, WORK, RWORK, IWORK, NOUT )
*
*  -- LAPACK test routine (version 3.7.0) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     December 2016
*
*     .. Scalar Arguments ..
      LOGICAL            TSTERR
      INTEGER            LA, LAFAC, NM, NN, NNB, NNS, NOUT
      REAL               THRESH
*     ..
*     .. Array Arguments ..
      LOGICAL            DOTYPE( * )
      INTEGER            IWORK( * ), MVAL( * ), NBVAL( * ), NSVAL( * ),
     $                   NVAL( * )
      REAL               RWORK( * )
      COMPLEX            A( * ), AFAC( * ), B( * ), WORK( * ), X( * ),
     $                   XACT( * )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      REAL               ONE, ZERO
      PARAMETER          ( ONE = 1.0E+0, ZERO = 0.0E+0 )
      INTEGER            NTYPES, NTESTS
      PARAMETER          ( NTYPES = 8, NTESTS = 7 )
      INTEGER            NBW, NTRAN
      PARAMETER          ( NBW = 4, NTRAN = 3 )
*     ..
*     .. Local Scalars ..
      LOGICAL            TRFCON, ZEROT
      CHARACTER          DIST, NORM, TRANS, TYPE, XTYPE
      CHARACTER*3        PATH
      INTEGER            I, I1, I2, IKL, IKU, IM, IMAT, IN, INB, INFO,
     $                   IOFF, IRHS, ITRAN, IZERO, J, K, KL, KOFF, KU,
     $                   LDA, LDAFAC, LDB, M, MODE, N, NB, NERRS, NFAIL,
     $                   NIMAT, NKL, NKU, NRHS, NRUN
      REAL               AINVNM, ANORM, ANORMI, ANORMO, CNDNUM, RCOND,
     $                   RCONDC, RCONDI, RCONDO
*     ..
*     .. Local Arrays ..
      CHARACTER          TRANSS( NTRAN )
      INTEGER            ISEED( 4 ), ISEEDY( 4 ), KLVAL( NBW ),
     $                   KUVAL( NBW )
      REAL               RESULT( NTESTS )
*     ..
*     .. External Functions ..
      REAL               CLANGB, CLANGE, SGET06
      EXTERNAL           CLANGB, CLANGE, SGET06
*     ..
*     .. External Subroutines ..
      EXTERNAL           ALAERH, ALAHD, ALASUM, CCOPY, CERRGE, CGBCON,
     $                   CGBRFS, CGBT01, CGBT02, CGBT05, CGBTRF, CGBTRS,
     $                   CGET04, CLACPY, CLARHS, CLASET, CLATB4, CLATMS,
     $                   XLAENV
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          CMPLX, MAX, MIN
*     ..
*     .. Scalars in Common ..
      LOGICAL            LERR, OK
      CHARACTER*32       SRNAMT
      INTEGER            INFOT, NUNIT
*     ..
*     .. Common blocks ..
      COMMON             / INFOC / INFOT, NUNIT, OK, LERR
      COMMON             / SRNAMC / SRNAMT
*     ..
*     .. Data statements ..
      DATA               ISEEDY / 1988, 1989, 1990, 1991 / ,
     $                   TRANSS / 'N', 'T', 'C' /
*     ..
*     .. Executable Statements ..
*
*     Initialize constants and the random number seed.
*
      PATH( 1: 1 ) = 'Complex precision'
      PATH( 2: 3 ) = 'GB'
      NRUN = 0
      NFAIL = 0
      NERRS = 0
      DO 10 I = 1, 4
         ISEED( I ) = ISEEDY( I )
   10 CONTINUE
*
*     Test the error exits
*
      IF( TSTERR )
     $   CALL CERRGE( PATH, NOUT )
      INFOT = 0
*
*     Initialize the first value for the lower and upper bandwidths.
*
      KLVAL( 1 ) = 0
      KUVAL( 1 ) = 0
*
*     Do for each value of M in MVAL
*
      DO 160 IM = 1, NM
         M = MVAL( IM )
*
*        Set values to use for the lower bandwidth.
*
         KLVAL( 2 ) = M + ( M+1 ) / 4
*
*        KLVAL( 2 ) = MAX( M-1, 0 )
*
         KLVAL( 3 ) = ( 3*M-1 ) / 4
         KLVAL( 4 ) = ( M+1 ) / 4
*
*        Do for each value of N in NVAL
*
         DO 150 IN = 1, NN
            N = NVAL( IN )
            XTYPE = 'N'
*
*           Set values to use for the upper bandwidth.
*
            KUVAL( 2 ) = N + ( N+1 ) / 4
*
*           KUVAL( 2 ) = MAX( N-1, 0 )
*
            KUVAL( 3 ) = ( 3*N-1 ) / 4
            KUVAL( 4 ) = ( N+1 ) / 4
*
*           Set limits on the number of loop iterations.
*
            NKL = MIN( M+1, 4 )
            IF( N.EQ.0 )
     $         NKL = 2
            NKU = MIN( N+1, 4 )
            IF( M.EQ.0 )
     $         NKU = 2
            NIMAT = NTYPES
            IF( M.LE.0 .OR. N.LE.0 )
     $         NIMAT = 1
*
            DO 140 IKL = 1, NKL
*
*              Do for KL = 0, (5*M+1)/4, (3M-1)/4, and (M+1)/4. This
*              order makes it easier to skip redundant values for small
*              values of M.
*
               KL = KLVAL( IKL )
               DO 130 IKU = 1, NKU
*
*                 Do for KU = 0, (5*N+1)/4, (3N-1)/4, and (N+1)/4. This
*                 order makes it easier to skip redundant values for
*                 small values of N.
*
                  KU = KUVAL( IKU )
*
*                 Check that A and AFAC are big enough to generate this
*                 matrix.
*
                  LDA = KL + KU + 1
                  LDAFAC = 2*KL + KU + 1
                  IF( ( LDA*N ).GT.LA .OR. ( LDAFAC*N ).GT.LAFAC ) THEN
                     IF( NFAIL.EQ.0 .AND. NERRS.EQ.0 )
     $                  CALL ALAHD( NOUT, PATH )
                     IF( N*( KL+KU+1 ).GT.LA ) THEN
                        WRITE( NOUT, FMT = 9999 )LA, M, N, KL, KU,
     $                     N*( KL+KU+1 )
                        NERRS = NERRS + 1
                     END IF
                     IF( N*( 2*KL+KU+1 ).GT.LAFAC ) THEN
                        WRITE( NOUT, FMT = 9998 )LAFAC, M, N, KL, KU,
     $                     N*( 2*KL+KU+1 )
                        NERRS = NERRS + 1
                     END IF
                     GO TO 130
                  END IF
*
                  DO 120 IMAT = 1, NIMAT
*
*                    Do the tests only if DOTYPE( IMAT ) is true.
*
                     IF( .NOT.DOTYPE( IMAT ) )
     $                  GO TO 120
*
*                    Skip types 2, 3, or 4 if the matrix size is too
*                    small.
*
                     ZEROT = IMAT.GE.2 .AND. IMAT.LE.4
                     IF( ZEROT .AND. N.LT.IMAT-1 )
     $                  GO TO 120
*
                     IF( .NOT.ZEROT .OR. .NOT.DOTYPE( 1 ) ) THEN
*
*                       Set up parameters with CLATB4 and generate a
*                       test matrix with CLATMS.
*
                        CALL CLATB4( PATH, IMAT, M, N, TYPE, KL, KU,
     $                               ANORM, MODE, CNDNUM, DIST )
*
                        KOFF = MAX( 1, KU+2-N )
                        DO 20 I = 1, KOFF - 1
                           A( I ) = ZERO
   20                   CONTINUE
                        SRNAMT = 'CLATMS'
                        CALL CLATMS( M, N, DIST, ISEED, TYPE, RWORK,
     $                               MODE, CNDNUM, ANORM, KL, KU, 'Z',
     $                               A( KOFF ), LDA, WORK, INFO )
*
*                       Check the error code from CLATMS.
*
                        IF( INFO.NE.0 ) THEN
                           CALL ALAERH( PATH, 'CLATMS', INFO, 0, ' ', M,
     $                                  N, KL, KU, -1, IMAT, NFAIL,
     $                                  NERRS, NOUT )
                           GO TO 120
                        END IF
                     ELSE IF( IZERO.GT.0 ) THEN
*
*                       Use the same matrix for types 3 and 4 as for
*                       type 2 by copying back the zeroed out column.
*
                        CALL CCOPY( I2-I1+1, B, 1, A( IOFF+I1 ), 1 )
                     END IF
*
*                    For types 2, 3, and 4, zero one or more columns of
*                    the matrix to test that INFO is returned correctly.
*
                     IZERO = 0
                     IF( ZEROT ) THEN
                        IF( IMAT.EQ.2 ) THEN
                           IZERO = 1
                        ELSE IF( IMAT.EQ.3 ) THEN
                           IZERO = MIN( M, N )
                        ELSE
                           IZERO = MIN( M, N ) / 2 + 1
                        END IF
                        IOFF = ( IZERO-1 )*LDA
                        IF( IMAT.LT.4 ) THEN
*
*                          Store the column to be zeroed out in B.
*
                           I1 = MAX( 1, KU+2-IZERO )
                           I2 = MIN( KL+KU+1, KU+1+( M-IZERO ) )
                           CALL CCOPY( I2-I1+1, A( IOFF+I1 ), 1, B, 1 )
*
                           DO 30 I = I1, I2
                              A( IOFF+I ) = ZERO
   30                      CONTINUE
                        ELSE
                           DO 50 J = IZERO, N
                              DO 40 I = MAX( 1, KU+2-J ),
     $                                MIN( KL+KU+1, KU+1+( M-J ) )
                                 A( IOFF+I ) = ZERO
   40                         CONTINUE
                              IOFF = IOFF + LDA
   50                      CONTINUE
                        END IF
                     END IF
*
*                    These lines, if used in place of the calls in the
*                    loop over INB, cause the code to bomb on a Sun
*                    SPARCstation.
*
*                     ANORMO = CLANGB( 'O', N, KL, KU, A, LDA, RWORK )
*                     ANORMI = CLANGB( 'I', N, KL, KU, A, LDA, RWORK )
*
*                    Do for each blocksize in NBVAL
*
                     DO 110 INB = 1, NNB
                        NB = NBVAL( INB )
                        CALL XLAENV( 1, NB )
*
*                       Compute the LU factorization of the band matrix.
*
                        IF( M.GT.0 .AND. N.GT.0 )
     $                     CALL CLACPY( 'Full', KL+KU+1, N, A, LDA,
     $                                  AFAC( KL+1 ), LDAFAC )
                        SRNAMT = 'CGBTRF'
                        CALL CGBTRF( M, N, KL, KU, AFAC, LDAFAC, IWORK,
     $                               INFO )
*
*                       Check error code from CGBTRF.
*
                        IF( INFO.NE.IZERO )
     $                     CALL ALAERH( PATH, 'CGBTRF', INFO, IZERO,
     $                                  ' ', M, N, KL, KU, NB, IMAT,
     $                                  NFAIL, NERRS, NOUT )
                        TRFCON = .FALSE.
*
*+    TEST 1
*                       Reconstruct matrix from factors and compute
*                       residual.
*
                        CALL CGBT01( M, N, KL, KU, A, LDA, AFAC, LDAFAC,
     $                               IWORK, WORK, RESULT( 1 ) )
*
*                       Print information about the tests so far that
*                       did not pass the threshold.
*
                        IF( RESULT( 1 ).GE.THRESH ) THEN
                           IF( NFAIL.EQ.0 .AND. NERRS.EQ.0 )
     $                        CALL ALAHD( NOUT, PATH )
                           WRITE( NOUT, FMT = 9997 )M, N, KL, KU, NB,
     $                        IMAT, 1, RESULT( 1 )
                           NFAIL = NFAIL + 1
                        END IF
                        NRUN = NRUN + 1
*
*                       Skip the remaining tests if this is not the
*                       first block size or if M .ne. N.
*
                        IF( INB.GT.1 .OR. M.NE.N )
     $                     GO TO 110
*
                        ANORMO = CLANGB( 'O', N, KL, KU, A, LDA, RWORK )
                        ANORMI = CLANGB( 'I', N, KL, KU, A, LDA, RWORK )
*
                        IF( INFO.EQ.0 ) THEN
*
*                          Form the inverse of A so we can get a good
*                          estimate of CNDNUM = norm(A) * norm(inv(A)).
*
                           LDB = MAX( 1, N )
                           CALL CLASET( 'Full', N, N, CMPLX( ZERO ),
     $                                  CMPLX( ONE ), WORK, LDB )
                           SRNAMT = 'CGBTRS'
                           CALL CGBTRS( 'No transpose', N, KL, KU, N,
     $                                  AFAC, LDAFAC, IWORK, WORK, LDB,
     $                                  INFO )
*
*                          Compute the 1-norm condition number of A.
*
                           AINVNM = CLANGE( 'O', N, N, WORK, LDB,
     $                              RWORK )
                           IF( ANORMO.LE.ZERO .OR. AINVNM.LE.ZERO ) THEN
                              RCONDO = ONE
                           ELSE
                              RCONDO = ( ONE / ANORMO ) / AINVNM
                           END IF
*
*                          Compute the infinity-norm condition number of
*                          A.
*
                           AINVNM = CLANGE( 'I', N, N, WORK, LDB,
     $                              RWORK )
                           IF( ANORMI.LE.ZERO .OR. AINVNM.LE.ZERO ) THEN
                              RCONDI = ONE
                           ELSE
                              RCONDI = ( ONE / ANORMI ) / AINVNM
                           END IF
                        ELSE
*
*                          Do only the condition estimate if INFO.NE.0.
*
                           TRFCON = .TRUE.
                           RCONDO = ZERO
                           RCONDI = ZERO
                        END IF
*
*                       Skip the solve tests if the matrix is singular.
*
                        IF( TRFCON )
     $                     GO TO 90
*
                        DO 80 IRHS = 1, NNS
                           NRHS = NSVAL( IRHS )
                           XTYPE = 'N'
*
                           DO 70 ITRAN = 1, NTRAN
                              TRANS = TRANSS( ITRAN )
                              IF( ITRAN.EQ.1 ) THEN
                                 RCONDC = RCONDO
                                 NORM = 'O'
                              ELSE
                                 RCONDC = RCONDI
                                 NORM = 'I'
                              END IF
*
*+    TEST 2:
*                             Solve and compute residual for A * X = B.
*
                              SRNAMT = 'CLARHS'
                              CALL CLARHS( PATH, XTYPE, ' ', TRANS, N,
     $                                     N, KL, KU, NRHS, A, LDA,
     $                                     XACT, LDB, B, LDB, ISEED,
     $                                     INFO )
                              XTYPE = 'C'
                              CALL CLACPY( 'Full', N, NRHS, B, LDB, X,
     $                                     LDB )
*
                              SRNAMT = 'CGBTRS'
                              CALL CGBTRS( TRANS, N, KL, KU, NRHS, AFAC,
     $                                     LDAFAC, IWORK, X, LDB, INFO )
*
*                             Check error code from CGBTRS.
*
                              IF( INFO.NE.0 )
     $                           CALL ALAERH( PATH, 'CGBTRS', INFO, 0,
     $                                        TRANS, N, N, KL, KU, -1,
     $                                        IMAT, NFAIL, NERRS, NOUT )
*
                              CALL CLACPY( 'Full', N, NRHS, B, LDB,
     $                                     WORK, LDB )
                              CALL CGBT02( TRANS, M, N, KL, KU, NRHS, A,
     $                                     LDA, X, LDB, WORK, LDB,
     $                                     RESULT( 2 ) )
*
*+    TEST 3:
*                             Check solution from generated exact
*                             solution.
*
                              CALL CGET04( N, NRHS, X, LDB, XACT, LDB,
     $                                     RCONDC, RESULT( 3 ) )
*
*+    TESTS 4, 5, 6:
*                             Use iterative refinement to improve the
*                             solution.
*
                              SRNAMT = 'CGBRFS'
                              CALL CGBRFS( TRANS, N, KL, KU, NRHS, A,
     $                                     LDA, AFAC, LDAFAC, IWORK, B,
     $                                     LDB, X, LDB, RWORK,
     $                                     RWORK( NRHS+1 ), WORK,
     $                                     RWORK( 2*NRHS+1 ), INFO )
*
*                             Check error code from CGBRFS.
*
                              IF( INFO.NE.0 )
     $                           CALL ALAERH( PATH, 'CGBRFS', INFO, 0,
     $                                        TRANS, N, N, KL, KU, NRHS,
     $                                        IMAT, NFAIL, NERRS, NOUT )
*
                              CALL CGET04( N, NRHS, X, LDB, XACT, LDB,
     $                                     RCONDC, RESULT( 4 ) )
                              CALL CGBT05( TRANS, N, KL, KU, NRHS, A,
     $                                     LDA, B, LDB, X, LDB, XACT,
     $                                     LDB, RWORK, RWORK( NRHS+1 ),
     $                                     RESULT( 5 ) )
*
*                             Print information about the tests that did
*                             not pass the threshold.
*
                              DO 60 K = 2, 6
                                 IF( RESULT( K ).GE.THRESH ) THEN
                                    IF( NFAIL.EQ.0 .AND. NERRS.EQ.0 )
     $                                 CALL ALAHD( NOUT, PATH )
                                    WRITE( NOUT, FMT = 9996 )TRANS, N,
     $                                 KL, KU, NRHS, IMAT, K,
     $                                 RESULT( K )
                                    NFAIL = NFAIL + 1
                                 END IF
   60                         CONTINUE
                              NRUN = NRUN + 5
   70                      CONTINUE
   80                   CONTINUE
*
*+    TEST 7:
*                          Get an estimate of RCOND = 1/CNDNUM.
*
   90                   CONTINUE
                        DO 100 ITRAN = 1, 2
                           IF( ITRAN.EQ.1 ) THEN
                              ANORM = ANORMO
                              RCONDC = RCONDO
                              NORM = 'O'
                           ELSE
                              ANORM = ANORMI
                              RCONDC = RCONDI
                              NORM = 'I'
                           END IF
                           SRNAMT = 'CGBCON'
                           CALL CGBCON( NORM, N, KL, KU, AFAC, LDAFAC,
     $                                  IWORK, ANORM, RCOND, WORK,
     $                                  RWORK, INFO )
*
*                             Check error code from CGBCON.
*
                           IF( INFO.NE.0 )
     $                        CALL ALAERH( PATH, 'CGBCON', INFO, 0,
     $                                     NORM, N, N, KL, KU, -1, IMAT,
     $                                     NFAIL, NERRS, NOUT )
*
                           RESULT( 7 ) = SGET06( RCOND, RCONDC )
*
*                          Print information about the tests that did
*                          not pass the threshold.
*
                           IF( RESULT( 7 ).GE.THRESH ) THEN
                              IF( NFAIL.EQ.0 .AND. NERRS.EQ.0 )
     $                           CALL ALAHD( NOUT, PATH )
                              WRITE( NOUT, FMT = 9995 )NORM, N, KL, KU,
     $                           IMAT, 7, RESULT( 7 )
                              NFAIL = NFAIL + 1
                           END IF
                           NRUN = NRUN + 1
  100                   CONTINUE
  110                CONTINUE
  120             CONTINUE
  130          CONTINUE
  140       CONTINUE
  150    CONTINUE
  160 CONTINUE
*
*     Print a summary of the results.
*
      CALL ALASUM( PATH, NOUT, NFAIL, NRUN, NERRS )
*
 9999 FORMAT( ' *** In CCHKGB, LA=', I5, ' is too small for M=', I5,
     $      ', N=', I5, ', KL=', I4, ', KU=', I4,
     $      / ' ==> Increase LA to at least ', I5 )
 9998 FORMAT( ' *** In CCHKGB, LAFAC=', I5, ' is too small for M=', I5,
     $      ', N=', I5, ', KL=', I4, ', KU=', I4,
     $      / ' ==> Increase LAFAC to at least ', I5 )
 9997 FORMAT( ' M =', I5, ', N =', I5, ', KL=', I5, ', KU=', I5,
     $      ', NB =', I4, ', type ', I1, ', test(', I1, ')=', G12.5 )
 9996 FORMAT( ' TRANS=''', A1, ''', N=', I5, ', KL=', I5, ', KU=', I5,
     $      ', NRHS=', I3, ', type ', I1, ', test(', I1, ')=', G12.5 )
 9995 FORMAT( ' NORM =''', A1, ''', N=', I5, ', KL=', I5, ', KU=', I5,
     $      ',', 10X, ' type ', I1, ', test(', I1, ')=', G12.5 )
*
      RETURN
*
*     End of CCHKGB
*
      END


!===============================================================================
! One of Andy Nowacki's Fortran utility modules for dealing with seismic
! anisotropy and other problems.
!
! Andy Nowacki <andy.nowacki@bristol.ac.uk>
!
! See the file LICENCE for licence details.
!===============================================================================
program sph_tess
!===============================================================================
! Dump the points for a level-n tesselation of a sphere, starting with an
! icosahedron.
! This version will create a tesselation of higher order if it is not available
! as a pre-saved version on disk.

   use sphere_tesselate
   use spherical_geometry

   implicit none
   type(tesselation) :: t
   integer :: i, n
   logical :: output_triangles = .false., output_geog = .false., &
      cache_exists = .false.
   ! Path to directory containing pre-computed tesselations
   character(len=250) :: file

   call get_args

   ! Try to load the file from the global cache
   call st_load_cache(t, cache_exists)
   ! Otherwise, create the tesselation and save the cache file for later use
   if (.not.cache_exists) then
      write(0,'(a)') 'Generating tesselation and saving cache file...'
      t = st_icosahedron(pole=.true.)
      do i = 1, n
         call st_iterate_level(t)
      enddo
      call st_save_cache(t)
   endif
   ! Output
   if (.not.output_triangles) then
      call st_dump_points(t, geog=output_geog)
   else
      call st_dump_triangles(t, geog=output_geog)
   endif

contains

   subroutine usage
      write(0,'(a)') &
         'Usage: sph_tess (options) [n]', &
         'Dump points for a level n tesselation of a sphere, starting with an', &
         'icosahedron.  x, y, z values on the unit sphere are output to stdout.', &
         'n must be >= 0', &
         'Options:', &
         '   -g        : Write values as lon, lat instead of x, y, z', &
         '   -p [path] : Set directory path to precomputed tesselations', &
         '   -t        : Dump the triangles as multisegment lines'
      stop
   end subroutine usage

   subroutine get_args
      character(len=250) :: arg
      integer :: iarg, narg
      narg = command_argument_count()
      if(narg == 0) call usage
      ! Get options
      iarg = 1
      do while (iarg <= narg - 1)
         call get_command_argument(iarg, arg)
         select case(arg)
            case('-g');   output_geog = .true.;       iarg = iarg + 1
            case('-p');   call get_command_argument(iarg+1, st_cache_dir)
                                                      iarg = iarg + 2
            case('-t');   output_triangles = .true.;  iarg = iarg + 1
            case default; call usage
         end select
      enddo
      ! Get tesselation level
      call get_command_argument(narg, arg)
      read(arg,*) n
      t%level = n
      if (n < 0) call usage
   end subroutine get_args

end program sph_tess
!-------------------------------------------------------------------------------


    module   interpolation

    implicit none

    contains

!{ Subroutines:
!{ assign
!{ assign_direct
!{ assign_double_grid
!{ assign_single_grid
!{ assign_index
!{ assign_weights_2
!{ assign_weights_3
!{ assign_weights_4

!*******************************************************************
!*** Assignment choice *********************************************

    subroutine assign(r,L)

    use parbox, only: interlacing, nintp, rupr
    implicit none
    real (kind=8) :: r(3), L(3)

    r(:)=mod(r(:)+L(:),L(:)) ! make sure particle are in the box
    r(:)=min(rupr,r(:)/L(:)) ! normalize to interval [0,1)

    if (nintp.eq.0) then
       call assign_direct(r)  ! direct summation
    else if (interlacing) then
       call assign_double_grid(r)  ! double grid assignment
    else if (.not. interlacing) then
       call assign_single_grid(r)   ! single grid assignment
    endif

    end subroutine assign

!*******************************************************************
!*** Direct assignment *********************************************

    subroutine assign_direct(r)

    use grid, only : Nx,Ny,Nz,dcl
    use parbox, only : twopi
    implicit none
    integer :: ix,iy,iz,icy,icz,ikx,iky,ikz,Nnyqx,Nnyqy,Nnyqz
    real(kind=8) :: rkx,rky,rkz,product,sp,cp,rtpi(3),r(3)

      Nnyqx=Nx/2+1
      Nnyqy=Ny/2+1
      Nnyqz=Nz/2+1
      rtpi=r*twopi

      do iz=1,Nnyqz
         ikz=mod(iz+Nz/2-2,Nz)-Nz/2+1
         rkz=dble(ikz)*rtpi(3)
         icz=mod(Nz-iz+1,Nz)+1

         do iy=1,Nnyqy
            iky=mod(iy+Ny/2-2,Ny)-Ny/2+1
            rky=dble(iky)*rtpi(2)
            icy=mod(Ny-iy+1,Ny)+1

            do ix=1,Nnyqx
               ikx=mod(ix+Nx/2-2,Nx)-Nx/2+1
               rkx=dble(ikx)*rtpi(1)

               product=rkx+rky+rkz
               cp=dcos(product)
               sp=dsin(product)
               dcl(ix,iy ,iz ) = dcl(ix,iy ,iz ) + dcmplx(cp,sp)

               if (iz.ne.Nnyqz .and. iz.ne.1) then
                  product=rkx+rky-rkz
                  cp=dcos(product)
                  sp=dsin(product)
                  dcl(ix,iy ,icz) = dcl(ix,iy ,icz) + dcmplx(cp,sp)
               endif

               if (iy.ne.Nnyqy .and. iy.ne.1) then
                  product=rkx-rky+rkz
                  cp=dcos(product)
                  sp=dsin(product)
                  dcl(ix,icy,iz ) = dcl(ix,icy,iz ) + dcmplx(cp,sp)
               endif

               if (iz.ne.Nnyqz .and. iy.ne.Nnyqy  .and. iz.ne.1 .and. iy.ne.1) then
                  product=rkx-rky-rkz
                  cp=dcos(product)
                  sp=dsin(product)
                  dcl(ix,icy,icz) = dcl(ix,icy,icz) + dcmplx(cp,sp)
               endif

            enddo
         enddo
      enddo

    end subroutine assign_direct

! *************************************************************************************
! *** Assign density on a double grid for interlacing *********************************

    subroutine assign_double_grid(r)

    use grid, only : dcl
    use parbox, only : Nv, nintp
    implicit none
    integer :: ix,iy,iz,iv(3,4),jv(3,4)
    real(kind=8) :: r(3),w1(3,4),w2(3,4),vr(3),vt(3)
    complex(kind=8) :: cpx

     vr(:)=dble(Nv(:))*r(:)+1.d0
     vt(:)=vr(:)+0.5d0

     if (nintp.eq.4) then

       call assign_index(int(vr),iv)
       call assign_weights_4(vr,w1)
       call assign_index(int(vt),jv)
       call assign_weights_4(vt,w2)

     elseif (nintp.eq.3) then

       call assign_index(nint(vr),iv)
       call assign_weights_3(vr,w1)
       call assign_index(nint(vt),jv)
       call assign_weights_3(vt,w2)

     elseif (nintp.eq.2) then

       call assign_index(int(vr),iv)
       call assign_weights_2(vr,w1)
       call assign_index(int(vt),jv)
       call assign_weights_2(vt,w2)

     elseif (nintp.eq.1) then

       w1=1.d0
       w2=1.d0
       iv(:,1)=mod(nint(vr(:))-1+Nv(:),Nv(:))+1
       jv(:,1)=mod(nint(vt(:))-1+Nv(:),Nv(:))+1

     endif

     do ix=1,nintp
      do iy=1,nintp
       do iz=1,nintp

          cpx=dcmplx(w1(1,ix)*w1(2,iy)*w1(3,iz),0.d0)
          dcl(iv(1,ix),iv(2,iy),iv(3,iz))=dcl(iv(1,ix),iv(2,iy),iv(3,iz))+cpx

          cpx=dcmplx(0.d0,w2(1,ix)*w2(2,iy)*w2(3,iz))
          dcl(jv(1,ix),jv(2,iy),jv(3,iz))=dcl(jv(1,ix),jv(2,iy),jv(3,iz))+cpx

       enddo
      enddo
     enddo

    end subroutine assign_double_grid

! ************************************************************************************
! *** Assign density on a single grid *************************************************

    subroutine assign_single_grid(r)

    use grid, only : dtl
    use parbox, only : Nv, nintp
    implicit none
    integer :: ix,iy,iz,ivr(3),iv(3,4)
    real(kind=8) :: r(3),w(3,4),vr(3)

     w=0.d0
     vr=dble(Nv)*r+1.d0

     if (nintp.eq.4) then ! PCS

       ivr=int(vr)
       call assign_index(ivr,iv)
       call assign_weights_4(vr,w)

     elseif (nintp.eq.3) then ! TSC

       ivr=nint(vr)
       call assign_index(ivr,iv)
       call assign_weights_3(vr,w)

     elseif (nintp.eq.2) then ! CIC

       ivr=int(vr)
       call assign_index(ivr,iv)
       call assign_weights_2(vr,w)

     elseif (nintp.eq.1) then ! NGP

       w=1.d0
       ivr=nint(vr)
       iv(:,1)=mod(ivr(:)-1+Nv(:),Nv(:))+1

     endif

     do ix=1,nintp
      do iy=1,nintp
       do iz=1,nintp
       dtl(iv(1,ix),iv(2,iy),iv(3,iz))=dtl(iv(1,ix),iv(2,iy),iv(3,iz))+w(1,ix)*w(2,iy)*w(3,iz)
       enddo
      enddo
     enddo

    end subroutine assign_single_grid

! ************************************************************************************
! *** Indices and weights ************************************************************

    subroutine assign_index(iv,idx)
    use parbox, only: Nv,nintp,nintph
    implicit none
    integer :: j,iv(3),idx(3,4)
     do j=1,nintp
        idx(:,j)=mod(iv(:)-nintph+j+Nv(:),Nv(:))+1
     enddo
    end subroutine assign_index

    subroutine assign_weights_4(v,w)
    implicit none
    integer :: i
    real(kind=8) :: h,h2,v(3),w(3,4)
     do i=1,3
        h=v(i)-int(v(i))
        h2=h*h
        w(i,1)=(1.d0-h)**3/6.d0
        w(i,2)=4.d0/6.d0+(0.5d0*h-1.d0)*h2
        w(i,4)=h2*h/6.d0
        w(i,3)=1.d0-w(i,1)-w(i,2)-w(i,4)
     enddo
    end subroutine assign_weights_4

    subroutine assign_weights_3(v,w)
    implicit none
    integer :: i
    real(kind=8) :: h,h2,v(3),w(3,4)
     do i=1,3
        h=v(i)-nint(v(i))
        h2=h*h
        w(i,1)=0.5d0*(0.5d0-h)**2
        w(i,2)=0.75d0-h2
        w(i,3)=1.d0-w(i,1)-w(i,2)
     enddo
    end subroutine assign_weights_3

    subroutine assign_weights_2(v,w)
    implicit none
    integer :: i
    real(kind=8) :: h,v(3),w(3,4)
     do i=1,3
        h=v(i)-int(v(i))
        w(i,1)=1.d0-h
        w(i,2)=h
     enddo
    end subroutine assign_weights_2

! ************************************************************************************

  end module interpolation


c$Header: /data/petsun4/data1/src_solaris/parzen_4dfp/RCS/parzen.f,v 1.1 2007/07/04 02:10:26 avi Exp $
c$Log: parzen.f,v $
c Revision 1.1  2007/07/04  02:10:26  avi
c Initial revision
c
c	2003.02.10 - Designing Parzen 'filters' for 3D images.
c	Definition of arguments: nx,ny,nz - #voxels in the x,y,z directions.
c	voxdim, fwhm are in mm !.

      subroutine parzen(image,nx,ny,nz,voxdim,fwhm)
      real*4 image(nx,ny,nz),voxdim(3)
      real*4 M,pi,sx,sy,sz
      pointer (pa,a),(pb,b)
      real*4 a((nx/2+1)*ny*nz),b((nx/2+1)*ny*nz)
      character*256 rcsid
     &/'$Id: parzen.f,v 1.1 2007/07/04 02:10:26 avi Exp $'/
      data pi/3.141592653589793/

      l=lnblnk(rcsid)
      write(*,"(a)")rcsid(1:l)
      pa=malloc(4*(nx/2+1)*ny*nz)
      pb=malloc(4*(nx/2+1)*ny*nz)

      M=fwhm/0.722351745       ! correct

      i=1
      do 21 iz=1,nz
      do 21 iy=1,ny
      do 21 ix=1,nx,2
      a(i)=image(ix,iy,iz)
      b(i)=image(ix+1,iy,iz)
   21 i=i+1

      n1=nx/2
      n=n1*ny*nz
      call FFT(a,b,ny*nz,n1,1,-1)
      call REALT(a,b,ny*nz,n1,1,-1)
      call FFT(a(n+1),b(n+1),nz,ny,1,-1)
      call FFT(a(n+1),b(n+1),1,nz,ny,-1)
      call FFT(a,b,nz,ny,n1,-1)
      call FFT(a,b,1,nz,n1*ny,-1)

      i=1
      do 31 iz=1,nz
      if(iz.le.nz/2+1)then
        fz=float(iz-1)/(float(nz)*voxdim(3))
      else
        fz=float(nz-iz+1)/(float(nz)*voxdim(3))
      endif
      if(fz.ne.0.0)then
         sz=(sinc(pi*fz*M/2))**4
      else
         sz=1.0
      endif
      do 31 iy=1,ny
      if(iy.le.ny/2+1)then
        fy=float(iy-1)/(float(ny)*voxdim(2))
      else
        fy=float(ny-iy+1)/(float(ny)*voxdim(2))
      endif
      if(fy.ne.0.0)then
         sy=(sinc(pi*fy*M/2))**4
      else
         sy=1.0
      endif
      do 31 ix=1,n1
        fx=float(ix-1)/(float(nx)*voxdim(1))
      if(fx.ne.0.0)then
         sx=(sinc(pi*fx*M/2))**4
      else
         sx=1.0
      endif
      factor=sx*sy*sz
      a(i)=factor*a(i)
      b(i)=factor*b(i)
   31 i=i+1
      fx=0.5/voxdim(1)
      sx=(sinc(pi*fx*M/2))**4
      do 32 iz=1,nz
      if(iz.le.nz/2+1)then
        fz=float(iz-1)/(float(nz)*voxdim(3))
      else
        fz=float(nz-iz+1)/(float(nz)*voxdim(3))
      endif
      if(fz.ne.0.0)then
         sz=(sinc(pi*fz*M/2))**4
      else
         sz=1.0
      endif
      do 32 iy=1,ny
      if(iy.le.ny/2+1)then
        fy=float(iy-1)/(float(ny)*voxdim(2))
      else
        fy=float(ny-iy+1)/(float(ny)*voxdim(2))
      endif
      if(fy.ne.0.0)then
         sy=(sinc(pi*fy*M/2))**4
      else
         sy=1.0
      endif
      factor=sx*sy*sz
      a(i)=factor*a(i)
      b(i)=factor*b(i)
   32 i=i+1

      call FFT(a,b,1,nz,n1*ny,+1)
      call FFT(a,b,nz,ny,n1,+1)
      call FFT(a(n+1),b(n+1),1,nz,ny,+1)
      call FFT(a(n+1),b(n+1),nz,ny,1,+1)
      call REALT(a,b,ny*nz,n1,1,+1)
      call FFT(a,b,ny*nz,n1,1,+1)

      i=1
      do 29 iz=1,nz
      do 29 iy=1,ny
      do 29 ix=1,nx,2
      image(ix,iy,iz)=a(i)
      image(ix+1,iy,iz)=b(i)
   29 i=i+1

      call free(pa)
      call free(pb)
      return
      end

      real*4 function sinc(x)
      real*4 x
      if(x.ne.0.0) then
         sinc=sin(x)/x
      else
         sinc=1.0
      endif
C      write(*,"(2e16.6)") x, sinc
      return
      end


! Created by  on 02.05.20.

module unary
    use quantum
    implicit none
    real(8), parameter :: pi = 3.1415926535897932_8

contains
    function unary_function(r, rs, tilde_omega, n_max, l_max, bor_radius) result(y)
        implicit none
        real :: rs, bor_radius, tilde_omega, unary_res = 0, r, y
        integer :: i, j, l_max, n_max
        real, external :: radial_function
        unary_res = 0;
        do i = 0, n_max
            if (i==n_max) then
                j = l_max - 2
                do while(j>=0 .or. j>=1)
                    unary_res = unary_res + ((2 * j + 1) / pi) * radial_function(r, tilde_omega, i, j)
                    j = j - 2
                end do
                exit
            end if
            j = i - 2
            do while(j>=0 .or. j>=1)
                unary_res = unary_res + ((2 * j + 1) / pi) * radial_function(r, tilde_omega, i, j)
                j = j - 2
            end do
        end do
        unary_res = (2.0 * 3.0) * rs**3 * (unary_res**2)
    end function unary_function
end module unary


    ! Hubble measurement constraint, based on corresponding angular diamter distance value and error

    module HST
    use CosmologyTypes
    use Likelihood_Cosmology
    implicit none
    private

    type, extends(TCosmoCalcLikelihood) :: HSTLikelihood
        real(mcp) :: angconversion = 11425.8d0
        !angconversion converts inverse of the angular diameter distance at z = zeff to H0
        !for the fiducial cosmology (omega_k = 0, omega_lambda = 0.7, w = -1)
        !likelihood is in terms of inverse of the angular diameter distance, so includes the tiny cosmological
        !dependence of the measurement (primarily on w) correctly.
        real(mcp) :: zeff = 0.04d0
        real(mcp) :: H0, H0_err
    contains
    procedure :: LogLikeTheory => HST_LnLike
    end type HSTLikelihood

    public HSTLikelihood, HSTLikelihood_Add
    contains

    subroutine HSTLikelihood_Add(LikeList, Ini)
    class(TLikelihoodList) :: LikeList
    class(TSettingIni) :: ini
    Type(HSTLikelihood), pointer :: this

    if (Ini%Read_Logical('use_HST',.false.)) then
        allocate(this)
        this%LikelihoodType = 'Hubble'
        this%name= Ini%Read_String('Hubble_name')
        this%H0 = Ini%Read_Double('Hubble_H0')
        this%H0_err = Ini%Read_Double('Hubble_H0_err')
        call Ini%Read('Hubble_zeff',this%zeff)
        call Ini%Read('Hubble_angconversion',this%angconversion)
        this%needs_background_functions = .true.
        call LikeList%Add(this)
    end if

    end subroutine HSTLikelihood_Add

    real(mcp) function HST_LnLike(this, CMB)
    Class(HSTLikelihood) :: this
    Class(CMBParams) CMB
    real(mcp) :: theoryval

    theoryval = this%angconversion/this%Calculator%AngularDiameterDistance(this%zeff)
    HST_LnLike = (theoryval - this%H0)**2/(2*this%H0_err**2)

    end function  HST_LnLike

    end module HST


c
c program stereocam
c
      include 'VICMAIN_FOR'
      subroutine main44

      real*8 focal,camera1(9),camera2(9),x,y,z,coords(4)
      real*8 mat1(3,3),mat2(3,3),mat3(3,3),cam1(9),cam2(9)
      integer*4 count,def,inunit(2)
      logical xvptst

      raddeg=180.0d0/3.141592653589d0

c open files
c      call xvunit(inunit(1),'INP',1,status,' ')
c      call xvopen(inunit(1),status,'IO_ACT','AS'
c     +            ,'OPEN_ACT','AS',' ')
c      call xvget(inunit(1),status,'NL',nl,'NS',np,
c     +  'FORMAT',oformat,' ')

c      call xvunit(inunit(2),'INP',2,status,' ')
c      call xvopen(inunit(2),status,'IO_ACT','AS'
c     +            ,'OPEN_ACT','AS',' ')

c Get parameters:
c 1=focal length in pixels
c 2=camera x location
c 3=camera y location
c 4=camera z location
c 5=first rotation degrees
c 6=second rotation degrees
c 7=third rotation degrees
c 8=sample of optical axis
c 9=line of optical axis
      call xvparmd('CAMERA1',camera1,count,def,9)
      call xvparmd('CAMERA2',camera2,count,def,9)
      call xvparm('NL',nl,count,def,1)
      call xvparm('NS',ns,count,def,1)
      call xvparmd('COORDS',coords,count,def,4)
      camera1(9)=nl-camera1(9)+1
      camera2(9)=nl-camera2(9)+1

c set up rotations
      call rotate_about_y(camera1(5),mat3)
      call rotate_about_x(camera1(6),mat2)
      call rotate_about_z(camera1(7),mat1)
      call multiply_matrices(mat3,mat2,mat1,cam1)
c      call multiply_matrices(mat1,mat2,mat3,cam1)
      call rotate_about_y(camera2(5),mat3)
      call rotate_about_x(camera2(6),mat2)
      call rotate_about_z(camera2(7),mat1)
      call multiply_matrices(mat3,mat2,mat1,cam2)
c      call multiply_matrices(mat1,mat2,mat3,cam2)
c      write(*,*)(cam1(i),i=1,9)
c      write(*,*)(cam2(i),i=1,9)

c convert image coords to xyz.
c      write(*,*)(camera1(i),i=1,9)
c      write(*,*)(camera2(i),i=1,9)
      call xvector(cam1,cam2,camera1(1),camera2(1),camera1(2),
     +   camera2(2),camera1(8),camera1(9),camera2(8),camera2(9),
     +   coords(1),nl-coords(2)+1,coords(3),nl-coords(4)+1,
     +   x,y,z,error,ind)

      if(ind.ne.0) then
        write(*,*)'no solution'
      else
        write(*,*)'s1=',coords(1),' l1=',coords(2),' s2=',coords(3),
     +    ' l2=',coords(4)
        write(*,*)'xyz=',x,y,z ! correct camera space
c        write(*,*)'xyz=',z,-x,y ! for air bag test coordinates only
        write(*,*)'error=',error
      endif

      return
      end

      subroutine rotate_about_x(angle,matrix)
      real*8 raddeg,angle,matrix(3,3) ! in row order
      raddeg=180.0d0/3.141592653589d0
      matrix(1,1)=1.d0
      matrix(2,1)=0.d0
      matrix(3,1)=0.d0
      matrix(1,2)=0.d0
      matrix(2,2)=dcos(angle/raddeg)
      matrix(3,2)=-dsin(angle/raddeg)
      matrix(1,3)=0.d0
      matrix(2,3)=dsin(angle/raddeg)
      matrix(3,3)=dcos(angle/raddeg)
      return
      end
      subroutine rotate_about_y(angle,matrix)
      real*8 raddeg,angle,matrix(3,3) ! in row order
      raddeg=180.0d0/3.141592653589d0
      matrix(1,1)=dcos(angle/raddeg)
      matrix(2,1)=0.d0
      matrix(3,1)=dsin(angle/raddeg)
      matrix(1,2)=0.d0
      matrix(2,2)=1.d0
      matrix(3,2)=0.d0
      matrix(1,3)=-dsin(angle/raddeg)
      matrix(2,3)=0.d0
      matrix(3,3)=dcos(angle/raddeg)
      return
      end
      subroutine rotate_about_z(angle,matrix)
      real*8 raddeg,angle,matrix(3,3) ! in row order
      raddeg=180.0d0/3.141592653589d0
      matrix(1,1)=dcos(angle/raddeg)
      matrix(2,1)=-dsin(angle/raddeg)
      matrix(3,1)=0.d0
      matrix(1,2)=dsin(angle/raddeg)
      matrix(2,2)=dcos(angle/raddeg)
      matrix(3,2)=0.d0
      matrix(1,3)=0.d0
      matrix(2,3)=0.d0
      matrix(3,3)=1.d0
      return
      end

      subroutine multiply_matrices(m1,m2,m3,result)
c m1,m2,m3 in row order, result returned in column order.
c order of multiplies is m1*(m2*m3)  right to left
      real*8 m1(3,3),m2(3,3),m3(3,3),m(3,3),result(9)

      m(1,1)=m2(1,1)*m3(1,1)+m2(2,1)*m3(1,2)+m2(3,1)*m3(1,3)
      m(2,1)=m2(1,1)*m3(2,1)+m2(2,1)*m3(2,2)+m2(3,1)*m3(2,3)
      m(3,1)=m2(1,1)*m3(3,1)+m2(2,1)*m3(3,2)+m2(3,1)*m3(3,3)
      m(1,2)=m2(1,2)*m3(1,1)+m2(2,2)*m3(1,2)+m2(3,2)*m3(1,3)
      m(2,2)=m2(1,2)*m3(2,1)+m2(2,2)*m3(2,2)+m2(3,2)*m3(2,3)
      m(3,2)=m2(1,2)*m3(3,1)+m2(2,2)*m3(3,2)+m2(3,2)*m3(3,3)
      m(1,3)=m2(1,3)*m3(1,1)+m2(2,3)*m3(1,2)+m2(3,3)*m3(1,3)
      m(2,3)=m2(1,3)*m3(2,1)+m2(2,3)*m3(2,2)+m2(3,3)*m3(2,3)
      m(3,3)=m2(1,3)*m3(3,1)+m2(2,3)*m3(3,2)+m2(3,3)*m3(3,3)

      result(1)=m1(1,1)*m(1,1)+m1(2,1)*m(1,2)+m1(3,1)*m(1,3)
      result(4)=m1(1,1)*m(2,1)+m1(2,1)*m(2,2)+m1(3,1)*m(2,3)
      result(7)=m1(1,1)*m(3,1)+m1(2,1)*m(3,2)+m1(3,1)*m(3,3)
      result(2)=m1(1,2)*m(1,1)+m1(2,2)*m(1,2)+m1(3,2)*m(1,3)
      result(5)=m1(1,2)*m(2,1)+m1(2,2)*m(2,2)+m1(3,2)*m(2,3)
      result(8)=m1(1,2)*m(3,1)+m1(2,2)*m(3,2)+m1(3,2)*m(3,3)
      result(3)=m1(1,3)*m(1,1)+m1(2,3)*m(1,2)+m1(3,3)*m(1,3)
      result(6)=m1(1,3)*m(2,1)+m1(2,3)*m(2,2)+m1(3,3)*m(2,3)
      result(9)=m1(1,3)*m(3,1)+m1(2,3)*m(3,2)+m1(3,3)*m(3,3)

c      result(1)=m1(1,1)*m(1,1)+m1(2,1)*m(1,2)+m1(3,1)*m(1,3)
c      result(2)=m1(1,1)*m(2,1)+m1(2,1)*m(2,2)+m1(3,1)*m(2,3)
c      result(3)=m1(1,1)*m(3,1)+m1(2,1)*m(3,2)+m1(3,1)*m(3,3)
c      result(4)=m1(1,2)*m(1,1)+m1(2,2)*m(1,2)+m1(3,2)*m(1,3)
c      result(5)=m1(1,2)*m(2,1)+m1(2,2)*m(2,2)+m1(3,2)*m(2,3)
c      result(6)=m1(1,2)*m(3,1)+m1(2,2)*m(3,2)+m1(3,2)*m(3,3)
c      result(7)=m1(1,3)*m(1,1)+m1(2,3)*m(1,2)+m1(3,3)*m(1,3)
c      result(8)=m1(1,3)*m(2,1)+m1(2,3)*m(2,2)+m1(3,3)*m(2,3)
c      result(9)=m1(1,3)*m(3,1)+m1(2,3)*m(3,2)+m1(3,3)*m(3,3)

      return
      end

       subroutine xvector(mat1,mat2,focal1,focal2,cam1,cam2,
     +     x1p,y1p,x2p,y2p,x1,y1,x2,y2,x,y,z,error,ind)
c Convert from image coordinates to xyz coordinates given two
c images forming a stereo pair.
c mat1=rotation matrix for camera 1
c mat2=rotation matrix for camera 2
c focal1=camera1 focal length
c focal2=camera2 focal length
c cam1=x,y,z object space position of camera 1
c cam2=x,y,z object space position of camera 2
c x1p,y1p= x & y image plane coord of optical axis, camera 1
c x2p,y2p= x & y image plane coord of optical axis, camera 2
c x1,y1= x & y image plane coord of common point of interest, camera 1
c x2,y2= x & y image plane coord of common point of interest, camera 2
c x,y,z= xyz object space coord of object (returned)
c ind=0 OK, ind=1 no solution (returned)
c Reference: Manual of Photogrammetry, page 64.
       implicit real*8 (a-z)
       real*8 mat1(9),mat2(9),cam1(3),cam2(3)
       real*8 a(9),b(3),c(9)
       real*4 error
       integer*4 ind

c compute direction cosines u,v,w for ray1 and ray2
       ind=0
       dx=x1-x1p
       dy=y1-y1p
       u1=mat1(1)*dx+mat1(4)*dy+mat1(7)*(-focal1)
       v1=mat1(2)*dx+mat1(5)*dy+mat1(8)*(-focal1)
       w1=mat1(3)*dx+mat1(6)*dy+mat1(9)*(-focal1)
c       write(*,*)'uvw1',u1,v1,w1
       d=dsqrt(u1*u1+v1*v1+w1*w1)
       u1=u1/d
       v1=v1/d
       w1=w1/d
       dx=x2-x2p
       dy=y2-y2p
       u2=mat2(1)*dx+mat2(4)*dy+mat2(7)*(-focal2)
       v2=mat2(2)*dx+mat2(5)*dy+mat2(8)*(-focal2)
       w2=mat2(3)*dx+mat2(6)*dy+mat2(9)*(-focal2)
c       write(*,*)'uvw2',u2,v2,w2
       d=dsqrt(u2*u2+v2*v2+w2*w2)
       u2=u2/d
       v2=v2/d
       w2=w2/d

c solve for x,y,z point on ray1 nearest to ray2
       as=v1*w2-w1*v2
       bs=u2*w1-u1*w2
       cs=u1*v2-v1*u2
       as1=bs*w1-v1*cs
       bs1=u1*cs-as*w1
       cs1=as*v1-u1*bs
       as2=bs*w2-v2*cs
       bs2=u2*cs-as*w2
       cs2=as*v2-u2*bs
       a(1)=as
       a(2)=as1
       a(3)=as2
       a(4)=bs
       a(5)=bs1
       a(6)=bs2
       a(7)=cs
       a(8)=cs1
       a(9)=cs2
       do 10 i=1,9
          c(i)=a(i)
10     continue
       b(1)=as*cam1(1)+bs*cam1(2)+cs*cam1(3)
       b(2)=as1*cam1(1)+bs1*cam1(2)+cs1*cam1(3)
       b(3)=as2*cam2(1)+bs2*cam2(2)+cs2*cam2(3)
       call dsimq(a,b,3,ind)
       x=b(1)
       y=b(2)
       z=b(3)
       if(ind.gt.0) return

c solve for xx,yy,zz point on ray2 nearest to ray1
       b(1)=as*cam2(1)+bs*cam2(2)+cs*cam2(3)
       b(2)=as1*cam1(1)+bs1*cam1(2)+cs1*cam1(3)
       b(3)=as2*cam2(1)+bs2*cam2(2)+cs2*cam2(3)
       call dsimq(c,b,3,ind)
       if(ind.gt.0) return
       xx=b(1)
       yy=b(2)
       zz=b(3)

c point inbetween is the closest approach point to both vectors
       error=dsqrt((z-zz)**2+(y-yy)**2+(x-xx)**2)
       x=(x+xx)/2.d0
       y=(y+yy)/2.d0
       z=(z+zz)/2.d0
       return
       end

      SUBROUTINE DSIMQ(A,B,N,KS)
C        USAGE
C           CALL DSIMQ(A,B,N,KS)
C        DESCRIPTION OF PARAMETERS
C           A - MATRIX OF COEFFICIENTS STORED COLUMNWISE.  THESE ARE
C               DESTROYED IN THE COMPUTATION.  THE SIZE OF MATRIX A IS
C               N BY N.
C           B - VECTOR OF ORIGINAL CONSTANTS (LENGTH N). THESE ARE
C               REPLACED BY FINAL SOLUTION VALUES, VECTOR X.
C           N - NUMBER OF EQUATIONS AND VARIABLES. N MUST BE .GT. ONE.
C           KS - OUTPUT DIGIT
C                0 FOR A NORMAL SOLUTION
C                1 FOR A SINGULAR SET OF EQUATIONS
      real*8 A(1),B(1),biga,save,tol
C
C        FORWARD SOLUTION
C
      TOL=0.d0
      KS=0
      JJ=-N
      DO 65 J=1,N
      JY=J+1
      JJ=JJ+N+1
      BIGA=0.d0
      IT=JJ-J
      DO 30 I=J,N
C
C        SEARCH FOR MAXIMUM COEFFICIENT IN COLUMN
C
      IJ=IT+I
      IF(dabs(BIGA)-dabs(A(IJ))) 20,30,30
   20 BIGA=A(IJ)
      IMAX=I
   30 CONTINUE
C
C        TEST FOR PIVOT LESS THAN TOLERANCE (SINGULAR MATRIX)
C
      IF(dabs(BIGA)-TOL) 35,35,40
   35 KS=1
      RETURN
C
C        INTERCHANGE ROWS IF NECESSARY
C
   40 I1=J+N*(J-2)
      IT=IMAX-J
      DO 50 K=J,N
      I1=I1+N
      I2=I1+IT
      SAVE=A(I1)
      A(I1)=A(I2)
      A(I2)=SAVE
C
C        DIVIDE EQUATION BY LEADING COEFFICIENT
C
   50 A(I1)=A(I1)/BIGA
      SAVE=B(IMAX)
      B(IMAX)=B(J)
      B(J)=SAVE/BIGA
C
C        ELIMINATE NEXT VARIABLE
C
      IF(J-N) 55,70,55
   55 IQS=N*(J-1)
      DO 65 IX=JY,N
      IXJ=IQS+IX
      IT=J-IX
      DO 60 JX=JY,N
      IXJX=N*(JX-1)+IX
      JJX=IXJX+IT
   60 A(IXJX)=A(IXJX)-(A(IXJ)*A(JJX))
   65 B(IX)=B(IX)-(B(J)*A(IXJ))
C
C        BACK SOLUTION
C
   70 NY=N-1
      IT=N*N
      DO 80 J=1,NY
      IA=IT-J
      IB=N-J
      IC=N
      DO 80 K=1,J
      B(IB)=B(IB)-A(IA)*B(IC)
      IA=IA-N
   80 IC=IC-1
      RETURN
      END


      SUBROUTINE UMAT(STRESS,STATEV,DDSDDE,SSE,SPD,SCD,
     1 RPL,DDSDDT,DRPLDE,DRPLDT,
     2 STRAN,DSTRAN,TIME,DTIME,TEMP,DTEMP,PREDEF,DPRED,CMNAME,
     3 NDI,NSHR,NTENS,NSTATV,PROPS,NPROPS,COORDS,DROT,PNEWDT,
     4 CELENT,DFGRD0,DFGRD1,NOEL,NPT,LAYER,KSPT,KSTEP,KINC)

      INCLUDE 'ABA_PARAM.INC'

      DIMENSION STRESS(NTENS),STATEV(NSTATV),
     1 DDSDDE(NTENS,NTENS),DDSDDT(NTENS),DRPLDE(NTENS),
     2 STRAN(NTENS),DSTRAN(NTENS),TIME(2),PREDEF(1),DPRED(1),
     3 PROPS(NPROPS),COORDS(3),DROT(3,3),DFGRD0(3,3),DFGRD1(3,3),
     4 JSTEP(4)

      parameter(ZERO=0.D0, ONE=1.D0, TWO=2.D0, TOL=1.D-4)
      double precision det, mu, lam, tp1, tp2, tp3, tp4,
     1 theta, theta_pre
      integer i, j, cnt
      logical flag
      double precision Fe(3,3), Be(6), Ce(6)
      double precision Je, trce, trme, dr1, dr2, kg, local_res, local_K
C
C Note theta is the growth factor at the current time step (increment)
C Don't want to update it unless this is the last iteration of the current time step (increment)
C According to abaqus, statev is updated only at the end of a time step
C
      theta_pre = statev(1)
      theta = statev(1)
      cnt = 0
      flag = .false.
      mu = props(1)
      lam = props(2)
      mecr = props(3) ! growth criterion
      tmax = props(4) ! theta_max
      gmax = props(5) ! positive growth exponent
      amax = props(6) ! adaption speed (alpha = 1/tau) for positive growth
C      tmin = props(7) ! theta_min
C      gmin = props(8) ! negative growth exponent
C      amin = props(9) ! adaption speed for negative growth
      local_res = 1.D0

      call get_elastic(DFGRD1,theta,mu,lam,Fe,Be,Ce,Je,trce,trme)
C Local Newton iteration to calculate theta, currently only positive growth is allowed
      do while (kstep .ge. 3 .and. local_res > tol
     1  .and. dabs(tmax - theta) > tol .and. trme - mecr > tol)
          kg = amax*((tmax - theta_pre)/(tmax - 1.D0))**gmax
          local_res = theta - theta_pre - kg*(trme - mecr)*dtime
          dr1 = -(9.D0*lam + 2.D0*mu*trce)/theta ! partial derivative of trme w.r.t. theta
          dr2 = -gmax*kg/(tmax - theta) ! partial derivative of kg w.r.t. theta
          local_K = 1.D0 - (kg*dr1 + (trme - mecr)*dr2)*dtime
          theta = theta - local_res/local_K
          flag = .true.
          call get_elastic(DFGRD1,theta,mu,lam,Fe,Be,Ce,Je,trce,trme)
          cnt = cnt + 1
          if (theta - tmax > tol) then
              write(*,*) 'Maximum value of theta exceeded!'
          else if (cnt .eq. 500) then
              write(*,*) 'Maximum number of theta iteration exceeded!'
          end if
      end do

C
C Total Jacobian
C
      det = Je*theta**3
C
C CALCULATE THE STRESS
C
      tp1 = (lam*LOG(Je)-mu)/det
      tp2 = mu/det
      DO i = 1,NDI
          STRESS(i) = tp1 + tp2*Be(i)
      END DO
      DO i = NDI+1, NDI+NSHR
          STRESS(i) = tp2*Be(i)
      END DO

C Calculate the tangent with kinematic terms required by Abaqus
C
      tp3 = lam/det
      DDSDDE(1, 1) = tp3 + tp2*two*Be(1)
      DDSDDE(2, 2) = tp3 + tp2*two*Be(2)
      DDSDDE(3, 3) = tp3 + tp2*two*Be(3)
      DDSDDE(1, 2) = tp3
      DDSDDE(1, 3) = tp3
      DDSDDE(2, 3) = tp3
      DDSDDE(1, 4) = tp2*Be(4)
      DDSDDE(2, 4) = tp2*Be(4)
      DDSDDE(3, 4) = 0.D0
      DDSDDE(4, 4) = tp2/two*(Be(1)+Be(2))
      IF(NSHR.EQ.3) THEN
          DDSDDE(1, 5) = tp2*Be(5)
          DDSDDE(2, 5) = 0.D0
          DDSDDE(3, 5) = tp2*Be(5)
          DDSDDE(1, 6) = 0.D0
          DDSDDE(2, 6) = tp2*Be(6)
          DDSDDE(3, 6) = tp2*Be(6)
          DDSDDE(5, 5) = tp2/two*(Be(1)+Be(3))
          DDSDDE(6, 6) = tp2/two*(Be(2)+Be(3))
          DDSDDE(4, 5) = tp2/two*Be(6)
          DDSDDE(4, 6) = tp2/two*Be(5)
          DDSDDE(5, 6) = tp2/two*Be(4)
      END IF
C
C Add growth term to tangent
C
      if (flag) then
          tp4 = -kg*dtime/(det*local_K*theta)
          DDSDDE(1, 1) = DDSDDE(1, 1) + tp4*(3.D0*lam+2.D0*mu*Be(1))**2
          DDSDDE(2, 2) = DDSDDE(2, 2) + tp4*(3.D0*lam+2.D0*mu*Be(2))**2
          DDSDDE(3, 3) = DDSDDE(3, 3) + tp4*(3.D0*lam+2.D0*mu*Be(3))**2
          DDSDDE(1, 2) = DDSDDE(1, 2)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(1))*(3.D0*lam+2.D0*mu*Be(2))
          DDSDDE(1, 3) = DDSDDE(1, 3)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(1))*(3.D0*lam+2.D0*mu*Be(3))
          DDSDDE(2, 3) = DDSDDE(2, 3)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(2))*(3.D0*lam+2.D0*mu*Be(3))
          DDSDDE(1, 4) = DDSDDE(1, 4)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(1))*(2.D0*mu*Be(4))
          DDSDDE(2, 4) = DDSDDE(2, 4)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(2))*(2.D0*mu*Be(4))
          DDSDDE(3, 4) = DDSDDE(3, 4)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(3))*(2.D0*mu*Be(4))
          DDSDDE(4, 4) = DDSDDE(4, 4) + tp4*(2.D0*mu*Be(4))**2
          IF(NSHR.EQ.3) THEN
              DDSDDE(1, 5) = DDSDDE(1, 5)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(1))*(2.D0*mu*Be(5))
              DDSDDE(2, 5) = DDSDDE(2, 5)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(2))*(2.D0*mu*Be(5))
              DDSDDE(3, 5) = DDSDDE(3, 5)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(3))*(2.D0*mu*Be(5))
              DDSDDE(1, 6) = DDSDDE(1, 6)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(1))*(2.D0*mu*Be(6))
              DDSDDE(2, 6) = DDSDDE(2, 6)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(2))*(2.D0*mu*Be(6))
              DDSDDE(3, 6) = DDSDDE(3, 6)
     1 + tp4*(3.D0*lam+2.D0*mu*Be(3))*(2.D0*mu*Be(6))
              DDSDDE(5, 5) = DDSDDE(5, 5) + tp4*(2.D0*mu*Be(5))**2
              DDSDDE(6, 6) = DDSDDE(6, 6) + tp4*(2.D0*mu*Be(6))**2
              DDSDDE(4, 5) = DDSDDE(4, 5)
     1 + tp4*(2.D0*mu*Be(4))*(2.D0*mu*Be(5))
              DDSDDE(4, 6) = DDSDDE(4, 6)
     1 + tp4*(2.D0*mu*Be(4))*(2.D0*mu*Be(6))
              DDSDDE(5, 6) = DDSDDE(5, 6)
     1 + tp4*(2.D0*mu*Be(5))*(2.D0*mu*Be(6))
          END IF
      end if

      DO i =1, NTENS
          DO j = 1, i-1
              DDSDDE(i, j) = DDSDDE(j, i)
          END DO
      END DO

      STATEV(1) = theta
      STATEV(2) = trme
      RETURN
      END

      subroutine get_elastic(F,theta,mu,lam,Fe,Be,Ce,Je,trce,trme)
      double precision F(3,3), Fe(3,3), Be(6), Ce(6)
      double precision theta, mu, lam, trce, trme, Je

      Fe = F/theta

      Je = Fe(1, 1)*Fe(2, 2)*Fe(3, 3)
     1 -Fe(1, 2)*Fe(2, 1)*Fe(3, 3)
      Je = Je + Fe(1, 2)*Fe(2, 3)*Fe(3, 1)
     1 +Fe(1, 3)*Fe(3, 2)*Fe(2, 1)
     2 -Fe(1, 3)*Fe(3,1)*Fe(2, 2)
     3 -Fe(2, 3)*Fe(3, 2)*Fe(1, 1)

      Be(1)=Fe(1, 1)**2+Fe(1, 2)**2+Fe(1, 3)**2
      Be(2)=Fe(2, 1)**2+Fe(2, 2)**2+Fe(2, 3)**2
      Be(3)=Fe(3, 3)**2+Fe(3, 1)**2+Fe(3, 2)**2
      Be(4)=Fe(1, 1)*Fe(2, 1)+Fe(1, 2)*Fe(2, 2)
     1 +Fe(1, 3)*Fe(2, 3)
      Be(5)=Fe(1, 1)*Fe(3, 1)+Fe(1, 2)*Fe(3, 2)
     1 +Fe(1, 3)*Fe(3, 3)
      Be(6)=Fe(2, 1)*Fe(3, 1)+Fe(2, 2)*Fe(3, 2)
     1 +Fe(2, 3)*Fe(3, 3)

      Ce(1)=Fe(1, 1)**2+Fe(2, 1)**2+Fe(3, 1)**2
      Ce(2)=Fe(1, 2)**2+Fe(2, 2)**2+Fe(3, 2)**2
      Ce(3)=Fe(1, 3)**2+Fe(2, 3)**2+Fe(3, 3)**2
      Ce(4)=Fe(1, 1)*Fe(1, 2)+Fe(2, 1)*Fe(2, 2)
     1 +Fe(3, 1)*Fe(3, 2)
      Ce(5)=Fe(1, 1)*Fe(1, 3)+Fe(2, 1)*Fe(2, 3)
     1 +Fe(3, 1)*Fe(3, 3)
      Ce(6)=Fe(1, 2)*Fe(1, 3)+Fe(2, 2)*Fe(2, 3)
     1 +Fe(3, 2)*Fe(3, 3)

      trce = Ce(1) + Ce(2) + Ce(3)
      trme = (lam*LOG(Je)-mu)*3.D0 + mu*trce
      return
      end

      SUBROUTINE SDVINI(STATEV,COORDS,NSTATV,NCRDS,NOEL,NPT,
     1 LAYER,KSPT)
C
      INCLUDE 'ABA_PARAM.INC'
C
      DIMENSION STATEV(NSTATV),COORDS(NCRDS)

      STATEV(1) = 1.0D0 ! theta
      STATEV(2) = 0.0D0 ! trace of Me

      RETURN
      END

      SUBROUTINE  DISP(U,KSTEP,KINC,TIME,NODE,NOEL,JDOF,COORDS)
C
      INCLUDE 'ABA_PARAM.INC'
C
      DIMENSION U(3),TIME(2),COORDS(3)
C
      parameter(pi = 3.1415926d0, phi = 0.7853982d0, rin0 = 1.5d-3,
     1 rout0=2.25d-3, tol=1.d-6)
      double precision r0, theta0, rout, rin, r, theta
C phi = 45 degrees, rin0 = 1.5 mm, rout0 = 2.25 mm
C Assuming dr/r0 = const, dtheta/theta = [-1,0], dz = 0
C (pi-phi)/pi*(rout0**2-rin0**2) = rout**2-rin**2
C (rout-rout0)/rout0 = (rin - rin0)/rin0
      if (kstep .eq. 1) then
          r0 = sqrt(coords(1)**2 + coords(2)**2)
          theta0 = asin(coords(2)/r0)
          if (coords(1) < 0.D0) then
              theta0 = pi - theta0
          end if
          rout = rout0*sqrt((pi-phi)/pi)
          rin = rin0*sqrt((pi-phi)/pi)
          r = r0 + r0*(rin-rin0)/rin0 ! new r
          theta = theta0 - (theta0-pi)/(phi-pi)*phi ! new theta
          if (JDOF .eq. 1) then
              U(1) = r*cos(theta) - coords(1)
          else if (JDOF .eq. 2) then
              U(1) = r*sin(theta) - coords(2)
          else if (JDOF .eq. 3) then
              U(1) = 0.D0
          end if
      end if
      RETURN
      END


*> \brief \b SQRT12
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*  Definition:
*  ===========
*
*       REAL             FUNCTION SQRT12( M, N, A, LDA, S, WORK, LWORK )
*
*       .. Scalar Arguments ..
*       INTEGER            LDA, LWORK, M, N
*       ..
*       .. Array Arguments ..
*       REAL               A( LDA, * ), S( * ), WORK( LWORK )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> SQRT12 computes the singular values `svlues' of the upper trapezoid
*> of A(1:M,1:N) and returns the ratio
*>
*>      || s - svlues||/(||svlues||*eps*max(M,N))
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] M
*> \verbatim
*>          M is INTEGER
*>          The number of rows of the matrix A.
*> \endverbatim
*>
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The number of columns of the matrix A.
*> \endverbatim
*>
*> \param[in] A
*> \verbatim
*>          A is REAL array, dimension (LDA,N)
*>          The M-by-N matrix A. Only the upper trapezoid is referenced.
*> \endverbatim
*>
*> \param[in] LDA
*> \verbatim
*>          LDA is INTEGER
*>          The leading dimension of the array A.
*> \endverbatim
*>
*> \param[in] S
*> \verbatim
*>          S is REAL array, dimension (min(M,N))
*>          The singular values of the matrix A.
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is REAL array, dimension (LWORK)
*> \endverbatim
*>
*> \param[in] LWORK
*> \verbatim
*>          LWORK is INTEGER
*>          The length of the array WORK. LWORK >= max(M*N + 4*min(M,N) +
*>          max(M,N), M*N+2*MIN( M, N )+4*N).
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \date November 2011
*
*> \ingroup single_lin
*
*  =====================================================================
      REAL             FUNCTION SQRT12( M, N, A, LDA, S, WORK, LWORK )
*
*  -- LAPACK test routine (version 3.4.0) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     November 2011
*
*     .. Scalar Arguments ..
      INTEGER            LDA, LWORK, M, N
*     ..
*     .. Array Arguments ..
      REAL               A( LDA, * ), S( * ), WORK( LWORK )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      REAL               ZERO, ONE
      PARAMETER          ( ZERO = 0.0E0, ONE = 1.0E0 )
*     ..
*     .. Local Scalars ..
      INTEGER            I, INFO, ISCL, J, MN
      REAL               ANRM, BIGNUM, NRMSVL, SMLNUM
*     ..
*     .. External Functions ..
      REAL               SASUM, SLAMCH, SLANGE, SNRM2
      EXTERNAL           SASUM, SLAMCH, SLANGE, SNRM2
*     ..
*     .. External Subroutines ..
      EXTERNAL           SAXPY, SBDSQR, SGEBD2, SLABAD, SLASCL, SLASET,
     $                   XERBLA
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN, REAL
*     ..
*     .. Local Arrays ..
      REAL               DUMMY( 1 )
*     ..
*     .. Executable Statements ..
*
      SQRT12 = ZERO
*
*     Test that enough workspace is supplied
*
      IF( LWORK.LT.MAX( M*N+4*MIN( M, N )+MAX( M, N ),
     $                  M*N+2*MIN( M, N )+4*N) ) THEN
         CALL XERBLA( 'SQRT12', 7 )
         RETURN
      END IF
*
*     Quick return if possible
*
      MN = MIN( M, N )
      IF( MN.LE.ZERO )
     $   RETURN
*
      NRMSVL = SNRM2( MN, S, 1 )
*
*     Copy upper triangle of A into work
*
      CALL SLASET( 'Full', M, N, ZERO, ZERO, WORK, M )
      DO 20 J = 1, N
         DO 10 I = 1, MIN( J, M )
            WORK( ( J-1 )*M+I ) = A( I, J )
   10    CONTINUE
   20 CONTINUE
*
*     Get machine parameters
*
      SMLNUM = SLAMCH( 'S' ) / SLAMCH( 'P' )
      BIGNUM = ONE / SMLNUM
      CALL SLABAD( SMLNUM, BIGNUM )
*
*     Scale work if max entry outside range [SMLNUM,BIGNUM]
*
      ANRM = SLANGE( 'M', M, N, WORK, M, DUMMY )
      ISCL = 0
      IF( ANRM.GT.ZERO .AND. ANRM.LT.SMLNUM ) THEN
*
*        Scale matrix norm up to SMLNUM
*
         CALL SLASCL( 'G', 0, 0, ANRM, SMLNUM, M, N, WORK, M, INFO )
         ISCL = 1
      ELSE IF( ANRM.GT.BIGNUM ) THEN
*
*        Scale matrix norm down to BIGNUM
*
         CALL SLASCL( 'G', 0, 0, ANRM, BIGNUM, M, N, WORK, M, INFO )
         ISCL = 1
      END IF
*
      IF( ANRM.NE.ZERO ) THEN
*
*        Compute SVD of work
*
         CALL SGEBD2( M, N, WORK, M, WORK( M*N+1 ), WORK( M*N+MN+1 ),
     $                WORK( M*N+2*MN+1 ), WORK( M*N+3*MN+1 ),
     $                WORK( M*N+4*MN+1 ), INFO )
         CALL SBDSQR( 'Upper', MN, 0, 0, 0, WORK( M*N+1 ),
     $                WORK( M*N+MN+1 ), DUMMY, MN, DUMMY, 1, DUMMY, MN,
     $                WORK( M*N+2*MN+1 ), INFO )
*
         IF( ISCL.EQ.1 ) THEN
            IF( ANRM.GT.BIGNUM ) THEN
               CALL SLASCL( 'G', 0, 0, BIGNUM, ANRM, MN, 1,
     $                      WORK( M*N+1 ), MN, INFO )
            END IF
            IF( ANRM.LT.SMLNUM ) THEN
               CALL SLASCL( 'G', 0, 0, SMLNUM, ANRM, MN, 1,
     $                      WORK( M*N+1 ), MN, INFO )
            END IF
         END IF
*
      ELSE
*
         DO 30 I = 1, MN
            WORK( M*N+I ) = ZERO
   30    CONTINUE
      END IF
*
*     Compare s and singular values of work
*
      CALL SAXPY( MN, -ONE, S, 1, WORK( M*N+1 ), 1 )
      SQRT12 = SASUM( MN, WORK( M*N+1 ), 1 ) /
     $         ( SLAMCH( 'Epsilon' )*REAL( MAX( M, N ) ) )
      IF( NRMSVL.NE.ZERO )
     $   SQRT12 = SQRT12 / NRMSVL
*
      RETURN
*
*     End of SQRT12
*
      END


      SUBROUTINE DSIFA(A,LDA,N,KPVT,INFO)
      INTEGER LDA,N,KPVT(1),INFO
      DOUBLE PRECISION A(LDA,1)
C
C     DSIFA FACTORS A DOUBLE PRECISION SYMMETRIC MATRIX BY ELIMINATION
C     WITH SYMMETRIC PIVOTING.
C
C     TO SOLVE  A*X = B , FOLLOW DSIFA BY DSISL.
C     TO COMPUTE  INVERSE(A)*C , FOLLOW DSIFA BY DSISL.
C     TO COMPUTE  DETERMINANT(A) , FOLLOW DSIFA BY DSIDI.
C     TO COMPUTE  INERTIA(A) , FOLLOW DSIFA BY DSIDI.
C     TO COMPUTE  INVERSE(A) , FOLLOW DSIFA BY DSIDI.
C
C     ON ENTRY
C
C        A       DOUBLE PRECISION(LDA,N)
C                THE SYMMETRIC MATRIX TO BE FACTORED.
C                ONLY THE DIAGONAL AND UPPER TRIANGLE ARE USED.
C
C        LDA     INTEGER
C                THE LEADING DIMENSION OF THE ARRAY  A .
C
C        N       INTEGER
C                THE ORDER OF THE MATRIX  A .
C
C     ON RETURN
C
C        A       A BLOCK DIAGONAL MATRIX AND THE MULTIPLIERS WHICH
C                WERE USED TO OBTAIN IT.
C                THE FACTORIZATION CAN BE WRITTEN  A = U*D*TRANS(U)
C                WHERE  U  IS A PRODUCT OF PERMUTATION AND UNIT
C                UPPER TRIANGULAR MATRICES , TRANS(U) IS THE
C                TRANSPOSE OF  U , AND  D  IS BLOCK DIAGONAL
C                WITH 1 BY 1 AND 2 BY 2 BLOCKS.
C
C        KPVT    INTEGER(N)
C                AN INTEGER VECTOR OF PIVOT INDICES.
C
C        INFO    INTEGER
C                = 0  NORMAL VALUE.
C                = K  IF THE K-TH PIVOT BLOCK IS SINGULAR. THIS IS
C                     NOT AN ERROR CONDITION FOR THIS SUBROUTINE,
C                     BUT IT DOES INDICATE THAT DSISL OR DSIDI MAY
C                     DIVIDE BY ZERO IF CALLED.
C
C     LINPACK. THIS VERSION DATED 08/14/78 .
C     JAMES BUNCH, UNIV. CALIF. SAN DIEGO, ARGONNE NAT. LAB.
C
C     SUBROUTINES AND FUNCTIONS
C
C     BLAS DAXPY,DSWAP,IDAMAX
C     FORTRAN DABS,DMAX1,DSQRT
C
C     INTERNAL VARIABLES
C
      DOUBLE PRECISION AK,AKM1,BK,BKM1,DENOM,MULK,MULKM1,T
      DOUBLE PRECISION ABSAKK,ALPHA,COLMAX,ROWMAX
      INTEGER IMAX,IMAXP1,J,JJ,JMAX,K,KM1,KM2,KSTEP,IDAMAX
      LOGICAL SWAP
C
C
C     INITIALIZE
C
C     ALPHA IS USED IN CHOOSING PIVOT BLOCK SIZE.
      ALPHA = (1.0D0 + DSQRT(17.0D0))/8.0D0
C
      INFO = 0
C
C     MAIN LOOP ON K, WHICH GOES FROM N TO 1.
C
      K = N
   10 CONTINUE
C
C        LEAVE THE LOOP IF K=0 OR K=1.
C
C     ...EXIT
         IF (K .EQ. 0) GO TO 200
         IF (K .GT. 1) GO TO 20
            KPVT(1) = 1
            IF (A(1,1) .EQ. 0.0D0) INFO = 1
C     ......EXIT
            GO TO 200
   20    CONTINUE
C
C        THIS SECTION OF CODE DETERMINES THE KIND OF
C        ELIMINATION TO BE PERFORMED.  WHEN IT IS COMPLETED,
C        KSTEP WILL BE SET TO THE SIZE OF THE PIVOT BLOCK, AND
C        SWAP WILL BE SET TO .TRUE. IF AN INTERCHANGE IS
C        REQUIRED.
C
         KM1 = K - 1
         ABSAKK = DABS(A(K,K))
C
C        DETERMINE THE LARGEST OFF-DIAGONAL ELEMENT IN
C        COLUMN K.
C
         IMAX = IDAMAX(K-1,A(1,K),1)
         COLMAX = DABS(A(IMAX,K))
         IF (ABSAKK .LT. ALPHA*COLMAX) GO TO 30
            KSTEP = 1
            SWAP = .FALSE.
         GO TO 90
   30    CONTINUE
C
C           DETERMINE THE LARGEST OFF-DIAGONAL ELEMENT IN
C           ROW IMAX.
C
            ROWMAX = 0.0D0
            IMAXP1 = IMAX + 1
            DO 40 J = IMAXP1, K
               ROWMAX = DMAX1(ROWMAX,DABS(A(IMAX,J)))
   40       CONTINUE
            IF (IMAX .EQ. 1) GO TO 50
               JMAX = IDAMAX(IMAX-1,A(1,IMAX),1)
               ROWMAX = DMAX1(ROWMAX,DABS(A(JMAX,IMAX)))
   50       CONTINUE
            IF (DABS(A(IMAX,IMAX)) .LT. ALPHA*ROWMAX) GO TO 60
               KSTEP = 1
               SWAP = .TRUE.
            GO TO 80
   60       CONTINUE
            IF (ABSAKK .LT. ALPHA*COLMAX*(COLMAX/ROWMAX)) GO TO 70
               KSTEP = 1
               SWAP = .FALSE.
            GO TO 80
   70       CONTINUE
               KSTEP = 2
               SWAP = IMAX .NE. KM1
   80       CONTINUE
   90    CONTINUE
         IF (DMAX1(ABSAKK,COLMAX) .NE. 0.0D0) GO TO 100
C
C           COLUMN K IS ZERO.  SET INFO AND ITERATE THE LOOP.
C
            KPVT(K) = K
            INFO = K
         GO TO 190
  100    CONTINUE
         IF (KSTEP .EQ. 2) GO TO 140
C
C           1 X 1 PIVOT BLOCK.
C
            IF (.NOT.SWAP) GO TO 120
C
C              PERFORM AN INTERCHANGE.
C
               CALL DSWAP(IMAX,A(1,IMAX),1,A(1,K),1)
               DO 110 JJ = IMAX, K
                  J = K + IMAX - JJ
                  T = A(J,K)
                  A(J,K) = A(IMAX,J)
                  A(IMAX,J) = T
  110          CONTINUE
  120       CONTINUE
C
C           PERFORM THE ELIMINATION.
C
            DO 130 JJ = 1, KM1
               J = K - JJ
               MULK = -A(J,K)/A(K,K)
               T = MULK
               CALL DAXPY(J,T,A(1,K),1,A(1,J),1)
               A(J,K) = MULK
  130       CONTINUE
C
C           SET THE PIVOT ARRAY.
C
            KPVT(K) = K
            IF (SWAP) KPVT(K) = IMAX
         GO TO 190
  140    CONTINUE
C
C           2 X 2 PIVOT BLOCK.
C
            IF (.NOT.SWAP) GO TO 160
C
C              PERFORM AN INTERCHANGE.
C
               CALL DSWAP(IMAX,A(1,IMAX),1,A(1,K-1),1)
               DO 150 JJ = IMAX, KM1
                  J = KM1 + IMAX - JJ
                  T = A(J,K-1)
                  A(J,K-1) = A(IMAX,J)
                  A(IMAX,J) = T
  150          CONTINUE
               T = A(K-1,K)
               A(K-1,K) = A(IMAX,K)
               A(IMAX,K) = T
  160       CONTINUE
C
C           PERFORM THE ELIMINATION.
C
            KM2 = K - 2
            IF (KM2 .EQ. 0) GO TO 180
               AK = A(K,K)/A(K-1,K)
               AKM1 = A(K-1,K-1)/A(K-1,K)
               DENOM = 1.0D0 - AK*AKM1
               DO 170 JJ = 1, KM2
                  J = KM1 - JJ
                  BK = A(J,K)/A(K-1,K)
                  BKM1 = A(J,K-1)/A(K-1,K)
                  MULK = (AKM1*BK - BKM1)/DENOM
                  MULKM1 = (AK*BKM1 - BK)/DENOM
                  T = MULK
                  CALL DAXPY(J,T,A(1,K),1,A(1,J),1)
                  T = MULKM1
                  CALL DAXPY(J,T,A(1,K-1),1,A(1,J),1)
                  A(J,K) = MULK
                  A(J,K-1) = MULKM1
  170          CONTINUE
  180       CONTINUE
C
C           SET THE PIVOT ARRAY.
C
            KPVT(K) = 1 - K
            IF (SWAP) KPVT(K) = -IMAX
            KPVT(K-1) = KPVT(K)
  190    CONTINUE
         K = K - KSTEP
      GO TO 10
  200 CONTINUE
      RETURN
      END


      SUBROUTINE svdinv(m, a, wcut, file)
      USE stel_kinds
      IMPLICIT NONE
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      INTEGER     :: m
      REAL(rprec) :: wcut
      CHARACTER(LEN=*) :: file
      REAL(rprec), DIMENSION(m,m) :: a
C-----------------------------------------------
C   L o c a l   P a r a m e t e r s
C-----------------------------------------------
      REAL(rprec) :: zero = 0
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      INTEGER     :: nonzero, nlast, istat, i, j, iunit
      REAL(rprec), ALLOCATABLE :: u(:,:), w(:), v(:,:)
      REAL(rprec) :: wmax, wcuta
C-----------------------------------------------

c       Inverts Square Matrix A using SVD method
c       Allows supression of weights, storage and recall of answers
c       Uses svd routines from numerical recipes
c       Prashant Valanju (Sept 1998) pvalanju@mail.utexas.edu

c       A(M,M) - Matrix on input, Inverse on output
c       M      - Size of A
c       Wcut   - Cutoff parameter
c                If Wcut = 0, writes answers into "file", all weights are kept
c                If Wcut < 0, reads previous answers from "file",
c                              and uses -Wcut for weights to keep
c                If 0 < Wcut < 1 : Cuts off weights with w(i)/wmax < wcut
c                      Use this to cut off small weights, does calculation
c                If Wcut > 1, keeps INT(Wcut) weights, does calculation
c                      Use this to cut off fixed number of weights
c
c       Note: M is same as N, Mp, and Np in this routine

c     ALLOCATE local arrays
      ALLOCATE (u(m,m), w(m), v(m,m), stat=istat)
      IF (istat .eq. 0) THEN                       !have enough memory

         iunit = 41
         wcuta = ABS(wcut)
c...............................
c     If Wcut < 0 use precalculated weights to get answer fast
c        use wcuta = ABS(wcut) for everything
c     If any error happens while reading "file", do full svd calculation
         IF (wcut .lt. zero) THEN         !Read file from previous calculation
            CALL safe_open(iunit, istat, file, 'old', 'formatted')
            IF (istat .ne. 0) GOTO 98
            READ (iunit, *, err=98)
            READ (iunit, *, err=98)
            READ (iunit, *, err=98) m, nonzero
c                           !Read exactly the same way they were written
            DO i = 1, nonzero
               READ (iunit, *, err=98) w(i)      !read i-th weight
               DO j = 1, m
                  READ (iunit, *, err=98) v(j,i), u(j,i)
               END DO
            END DO
            CLOSE(unit=iunit)

c        Decide how many weights to keep
            nlast = wcuta                        !remember Wcut is < 0
            nlast = MIN(nonzero,nlast)           !Do not use nonzero weights

            GOTO 99           !bypass svdcmp and GOTO matrix inversion
         ENDIF               !End of precalculated U,w,V branch Wcut < 0
   98    CONTINUE
c.......................................
c     If Wcut .ge. 0, do full svd calculation and save V,w,U
c     Initialize all to zero to wipe out effects of old call
         DO j = 1, m
            w(j) = zero                           !Zero all weights
            u(:m,j) = a(:m,j)  !Because U will be changed by svdcmp
            v(:m,j) = zero
         END DO

         CALL svdcmp (u, m, m, m, m, w, v)       !Do SVD decomposition

c       Sort weights and matrices with DECREASING weights
c       Permute weight w(i) AND column vectors U(*,i), V(*,i) at the same time
         CALL sortsvd (m, m, m, m, w, u, v)

c       Find the number of nonzero weights (already decreasing ordered)
         DO nonzero = m, 1, -1
c                                   !Found first nonzero weight, get out
            IF (w(nonzero) .ne. 0) EXIT
         END DO
         IF (nonzero .le. 0) GOTO 999

c       Write all weights and U, V into SVD file (if Wcut=0)
         IF (wcut .eq. zero) THEN    !no nonzero weights, you must be kidding
            CALL safe_open(iunit, istat, file, 'unknown', 'formatted')
            WRITE (iunit, *) 'Max w = ', w(1), ', Min w = ', w(nonzero)
            WRITE (iunit, *) 'Ratio = ', w(1)/w(nonzero)
            WRITE (iunit, *) m, nonzero
c                          !WRITE exactly the same way they were written
            DO i = 1, nonzero
               WRITE (iunit, *) w(i)             !write i-th weight
               DO j = 1, m
                  WRITE (iunit, *) v(j,i), u(j,i)
               END DO
            END DO
            CLOSE(unit=iunit)
         END IF
c.......................................

c        Following gets done in both cases (using saved or recalculating)
c        Decide how many weights to keep
   99    CONTINUE
         IF (wcuta .gt. 1) THEN
            nlast = wcuta
         ELSE                                    !cutoff small weights
            wmax = w(1)                          !weights are already ordered
            nlast = 0
            DO i = 1, nonzero
               IF (w(i) .gt. wmax*wcuta) THEN
                  nlast = nlast + 1              !accept this weight
               ELSE
                  GOTO 96
               END IF
            END DO
         END IF

c       Find Inverse of A = V [diag(1/wi)] Utranspose, return in A
c       First do [diag(1/wi)] Utranspose, store answer in U again
   96    CONTINUE
         IF (nlast.le.0 .or. nlast.gt.nonzero) nlast = nonzero
         DO i = 1, nlast
c                                         !divide ith row of Utr by w(i)
            u(:m,i) = u(:m,i)/w(i)
         END DO
c        Zero the infinite 1/weights
         DO i = nlast + 1, m
            u(:m,i) = zero                !multiply ith row of Utr by zero
         END DO
c       Next multiply by V matrix to get A inverse, put it in A
         DO i = 1, m                      !multiply ith row vector of V by
            DO j = 1, m                         !jth column vector of wU
                                                !vector multiply V and wU
               a(i,j) = SUM(v(i,:m)*u(j,:m))    !Put inverse back in A
            END DO
         END DO
c................................................
      END IF
  999 CONTINUE

      DEALLOCATE (u, w, v, stat=istat)

      END SUBROUTINE svdinv


      FUNCTION SAHA(THETA,CHI,U1,U2,PE)
C     SAHA-EGGERT EQUATION
      SAHA=U2*10.**(9.0805126-THETA*CHI)/(U1*PE*THETA**2.5)
c      print*,'saha en saha =          ',saha
c      print*,theta,chi,u1,u2,pe
      RETURN
      END

      FUNCTION SAHAdouble(THETA,CHI,U1,U2,PE)
      real*8 THETA,PE
      real*4 CHI,U1,U2

      SAHAdouble=U2*10.d0**(9.0805126d0-THETA*CHI)/(U1*PE*THETA**2.5d0)

      RETURN
      END

      FUNCTION SAHAdoubleinv(THETA,CHI,U1,U2,PE)
      real*8 THETA,PE
      real*4 CHI,U1,U2

      SAHAdoubleinv=8.3078262e-10*10.d0**(THETA*CHI)*THETA**2.5d0*PE*U1/U2

      RETURN
      END


use cosmo

implicit none

real(dp) x1,x2,y1,y2

x1 = -4.0839520530704121 * deg2rad
y1 = 36.130845150889883* deg2rad

x2 = -4.0858035726343953* deg2rad
y2 = 36.130983323439665* deg2rad

print*, haversine(x1,y1,x2,y2)

x2 = -4.0955060401875754* deg2rad
y2 = 36.130122356975214 * deg2rad

print*, haversine(x1,y1,x2,y2)

x1 = -4.23_dp* deg2rad
y1 = 35.79_dp* deg2rad

x2 =  -4.2308994570560667 * deg2rad
y2 =  35.795425066464134 * deg2rad

print*, haversine(x1,y1,x2,y2)

x2 =  -4.2281751602129303  * deg2rad
y2 =  35.810390085363039* deg2rad

print*, haversine(x1,y1,x2,y2)
x2 =  -4.2302049720819550 * deg2rad
y2 =  35.801124105899596* deg2rad

print*, haversine(x1,y1,x2,y2)

x2 = -4.2307104265931397  * deg2rad
y2 =  35.783852846582519 * deg2rad
print*, haversine(x1,y1,x2,y2)

end


      SUBROUTINE axepld(coorn,trans,x,y,area2,flag)
!***********************************************************************
!
!****this routine compute the element local axes system
!****for the 3 node element
!****(local x-axis is aligned with a side 1-2
!***********************************************************************
      IMPLICIT NONE

      REAL (kind=8),INTENT(IN) :: coorn(3,3)
      REAL (kind=8),INTENT(OUT) :: trans(3,3),x(3),y(3),area2
      LOGICAL, INTENT(IN) :: flag

!
      INTEGER (kind=4) i,j
      REAL (kind=8) xno1,xno3,coorb(2,3)

      INTERFACE
        INCLUDE 'vecpro.h'
        INCLUDE 'vecuni.h'
      END INTERFACE
!*** evaluate the local axes

      trans(1:3,1) = coorn(1:3,2) - coorn(1:3,1)    !parallel to side 1
      trans(1:3,2) = coorn(1:3,3) - coorn(1:3,1)    !parallel to side 3

!*** evaluate the cross product  T3 = T1 x T2

!      CALL vecpro(trans(1:3,1),trans(1:3,2),trans(1:3,3))
      trans(1,3) = trans(2,1)*trans(3,2) - trans(3,1)*trans(2,2)
      trans(2,3) = trans(3,1)*trans(1,2) - trans(1,1)*trans(3,2)
      trans(3,3) = trans(1,1)*trans(2,2) - trans(2,1)*trans(1,2)

!      CALL vecuni(3,trans(1:3,1),xno1)
      xno1 = SQRT(DOT_PRODUCT(trans(:,1),trans(:,1)))
      trans(:,1) = trans(:,1)/xno1
!      CALL vecuni(3,trans(1:3,3),xno3)
      xno3 = SQRT(DOT_PRODUCT(trans(:,3),trans(:,3)))
      trans(:,3) = trans(:,3)/xno3

      IF (xno3 == 0.0D0) THEN
         WRITE (*,*) 'ERROR: AREA2 == 0 '
         write(*,"(3e15.5)")coorn
         WRITE (55,*,ERR=9999) 'ERROR: AREA2 == 0 '
         write(55,"(3e15.5)",ERR=9999) coorn
         CALL runen3('AXEPLD: ELEMENT DISTORTION OCCURRED')
      END IF

!      CALL vecpro(trans(1:3,3),trans(1:3,1),trans(1:3,2))
      trans(1,2) = trans(2,3)*trans(3,1) - trans(3,3)*trans(2,1)
      trans(2,2) = trans(3,3)*trans(1,1) - trans(1,3)*trans(3,1)
      trans(3,2) = trans(1,3)*trans(2,1) - trans(2,3)*trans(1,1)

      area2 = xno3

      IF(flag)THEN

       !*** find the local coordinates

!        coorb = MATMUL(TRANSPOSE(trans(1:3,1:2)),coorn)
        DO i=1,2
          DO j=1,3
            coorb(i,j) = DOT_PRODUCT(trans(:,i),coorn(:,j))
          END DO
        END DO

        !side proyections
        x(1) = coorb(1,1) - coorb(1,2)
        y(1) = coorb(2,1) - coorb(2,2)
        x(2) = coorb(1,2) - coorb(1,3)
        y(2) = coorb(2,2) - coorb(2,3)
        x(3) = coorb(1,3) - coorb(1,1)      ! -x(1) -x(2)
        y(3) = coorb(2,3) - coorb(2,1)      ! -y(1) -y(2)

      END IF

      RETURN
 9999 CALL runen2('')
      END SUBROUTINE axepld


      MODULE FACTORSTUFF	!This protocol evades the need for multiple parameters, or COMMON, or one shapeless main line...
Concocted by R.N.McLean, MMXV.
c       INTEGER*4 I4LIMIT
c       PARAMETER (I4LIMIT = 2147483647)
       INTEGER*8 TOOBIG		!Some bounds.
       PARAMETER (TOOBIG = 2**47)	!Computer arithmetic is not with real numbers.
       INTEGER LOTS			!Nor is computer storage infinite.
       PARAMETER (LOTS = 10000)	!So there can't be all that many of these.
       INTEGER*8 KNOWNSUM(LOTS)	!If multiple references are expected, it is worthwhile calculating these.
       CONTAINS			!Assistants.
        INTEGER*8 FUNCTION SUMF(N)	!Sum of the proper divisors of N.
         INTEGER*8 N			!The number in question.
         INTEGER*8 F,F2		!Candidate factor, and its square.
         INTEGER*8 S,INC,BOOST		!Assistants.
          IF (N.LE.LOTS) THEN		!If we're within reach,
            SUMF = KNOWNSUM(N)			!The result is to hand.
           ELSE			!Otherwise, some on-the-spot effort ensues.
Could use SUMF in place of S, but some compilers have been confused by such usage.
            S = 1			!1 is always a factor of N, but N is deemed not proper.
            F = 1			!Prepare a crude search for factors.
            INC = 1			!One by plodding one.
            IF (MOD(N,2) .EQ. 1) INC = 2!Ah, but an odd number cannot have an even number as a divisor.
    1       F = F + INC			!So half the time we can doubleplod.
            F2 = F*F				!Up to F2 < N rather than F < SQRT(N) and worries over inexact arithmetic.
            IF (F2 .LT. N) THEN			!F2 = N handled below.
              IF (MOD(N,F) .EQ. 0) THEN		!Does F divide N?
                BOOST = F + N/F			!Yes. The divisor and its counterpart.
                IF (S .GT. TOOBIG - BOOST) GO TO 666	!Would their augmentation cause an overflow?
                S = S + BOOST			!No, so count in the two divisors just discovered.
              END IF				!So much for a divisor discovered.
              GO TO 1				!Try for another.
            END IF			!So much for N = p*q style factors.
            IF (F2 .EQ. N) THEN	!Special case: N may be a perfect square, not necessarily of a prime number.
              IF (S .GT. TOOBIG - F) GO TO 666	!It is. And it too might cause overflow.
              S = S + F			!But if not, count F once only.
            END IF			!All done.
            SUMF = S			!This is the result.
          END IF			!Whichever way obtained,
         RETURN			!Done.
Cannot calculate the sum, because it exceeds the INTEGER*8 limit.
  666     SUMF = -666		!An expression of dismay that the caller will notice.
        END FUNCTION SUMF	!Alternatively, find the prime factors, and combine them...
         SUBROUTINE PREPARESUMF	!Initialise the KNOWNSUM array.
Convert the Sieve of Eratoshenes to have each slot contain the sum of the proper divisors of its slot number.
Changes to instead count the number of factors, or prime factors, etc. would be simple enough.
         INTEGER*8 F		!A factor for numbers such as 2F, 3F, 4F, 5F, ...
          KNOWNSUM(1) = 0		!Proper divisors of N do not include N.
          KNOWNSUM(2:LOTS) = 1		!So, although 1 divides all N without remainder, 1 is excluded for itself.
          DO F = 2,LOTS/2		!Step through all the possible divisors of numbers not exceeding LOTS.
            FORALL(I = F + F:LOTS:F) KNOWNSUM(I) = KNOWNSUM(I) + F	!And augment each corresponding slot.
          END DO			!Different divisors can hit the same slot. For instance, 6 by 2 and also by 3.
        END SUBROUTINE PREPARESUMF	!Could alternatively generate all products of prime numbers.
         SUBROUTINE CLASSIFY(N)	!Traipse along the SumF trail.
         INTEGER*8 N		!The starter.
         INTEGER ROPE		!The size of my memory is not so great..
         PARAMETER(ROPE = 16)	!Indeed, this is strictly limited.
         INTEGER*8 TRAIL(ROPE)	!But the numbers can be large.
         INTEGER*8 SF		!The working sum of proper divisors.
         INTEGER I,L		!Indices, merely.
         CHARACTER*28 THIS	!A perfect scratchpad for remarks.
          L = 1		!Every journey starts with its first step.
          TRAIL(1) = N		!Which is this.
          SF = N		!Syncopation.
   10     SF = SUMF(SF)		!Step onwards.
          IF (SF .LT. 0) THEN		!Trouble?
            WRITE (THIS,11) L,"overflows!"	!Yes. Too big a number.
   11       FORMAT ("After ",I0,", ",A)		!Describe the situation.
            CALL REPORT(ADJUSTR(THIS))		!And give the report.
          ELSE IF (SF .EQ. 0) THEN		!Otherwise, a finish?
            WRITE (THIS,11) L,"terminates!"	!Yay!
            CALL REPORT(ADJUSTR(THIS))		!This sequence is finished.
          ELSE IF (ANY(TRAIL(1:L) .EQ. SF)) THEN	!Otherwise, is there an echo somewhere?
            IF (L .EQ. 1) THEN				!Yes!
              CALL REPORT("Perfect!")			!Are we at the start?
            ELSE IF (L .EQ. 2) THEN			!Or perhaps not far along.
              CALL REPORT("Amicable:")			!These are held special.
            ELSE					!Otherwise, we've wandered further along.
              I = MINLOC(ABS(TRAIL(1:L) - SF),DIM=1)	!Damnit, re-scan the array to finger the first matching element.
              IF (I .EQ. 1) THEN		!If all the way back to the start,
                WRITE (THIS,12) L		!Then there are this many elements in the sociable ring.
   12           FORMAT ("Sociable ",I0,":")	!Computers are good at counting.
                CALL REPORT(ADJUSTR(THIS))	!So, perform an added service.
              ELSE IF (I .EQ. L) THEN		!Perhaps we've hit a perfect number!
                CALL REPORT("Aspiring:")	!A cycle of length one.
              ELSE				!But otherwise,
                WRITE (THIS,13) L - I + 1,SF	!A longer cycle. Amicable, or sociable.
   13           FORMAT ("Cyclic end ",I0,", to ",I0,":")	!Name the flashback value too.
                CALL REPORT(ADJUSTR(THIS))	!Thus.
              END IF				!So much for cycles.
            END IF			!So much for finding an echo.
          ELSE				!Otherwise, nothing special has happened.
            IF (L .GE. ROPE) THEN		!So, how long is a piece of string?
              WRITE (THIS,11) L,"non-terminating?"	!Not long enough!
              CALL REPORT(ADJUSTR(THIS))		!So we give up.
             ELSE				!But if there is more scope,
              L = L + 1			!Advance one more step.
              TRAIL(L) = SF			!Save the latest result.
              GO TO 10				!And try for the next.
            END IF			!So much for continuing.
          END IF		!So much for the classification.
         RETURN		!Finished.
         CONTAINS		!Not quite.
          SUBROUTINE REPORT(WHAT)	!There is this service routine.
           CHARACTER*(*) WHAT		!Whatever the length of the text, the FORMAT's A28 shows 28 characters, right-aligned.
            WRITE (6,1) WHAT,TRAIL(1:L)!Mysteriously, a fresh line after every twelve elements.
    1       FORMAT (A28,1X,12(I0:","))	!And obviously, the : signifies "do not print what follows unless there is another number to go.
          END SUBROUTINE REPORT	!That was easy.
        END SUBROUTINE CLASSIFY	!Enough.
       END MODULE FACTORSTUFF	!Enough assistants.
       PROGRAM CLASSIFYTHEM	!Report on the nature of the sequence N, Sumf(N), Sumf(Sumf(N)), etc.
       USE FACTORSTUFF		!This should help.
       INTEGER*8 I,N		!Steppers.
       INTEGER*8 THIS(14)	!A testing collection.
       DATA THIS/11,12,28,496,220,1184,12496,1264460,790,909,     !Old-style continuation character in column six.
     1  562,1064,1488,15355717786080/	!Monster value far exceeds the INTEGER*4 limit
         CALL PREPARESUMF		!Prepare for 1:LOTS, even though this test run will use only a few.
         DO I = 1,10			!As specified, the first ten integers.
          CALL CLASSIFY(I)
        END DO
         DO I = 1,SIZE(THIS)		!Now for the specified list.
          CALL CLASSIFY(THIS(I))
        END DO
       END			!Done.


-- NATIVE DATATYPES

-- Bitstring
data Bits : Type
| 0 : (x : Bits) -> Bits
| 1 : (x : Bits) -> Bits
| z : Bits

-- Natural Number
data Nat  : Type
| succ    : (x : Nat) -> Nat
| zero    : Nat

-- List of Bitstrings
data List : Type
| cons    : (x : Bits, xs : List) -> List
| nil     : List

-- LAMBDA-ENCODED DATATYPES

-- Bitstring
let S-Bits (P : Type, 0 : (bs : S-Bits) -> P, 1 : (bs : S-Bits) -> P, z : P) -> P
let S-z    (P : Type, 0 : (bs : S-Bits) -> P, 1 : (bs : S-Bits) -> P, z : P) => z
let S-0    (bs : S-Bits) => (P : Type, 0 : (x : S-Bits) -> P, 1 : (x : S-Bits) -> P, z : P) => 0(bs)
let S-1    (bs : S-Bits) => (P : Type, 0 : (x : S-Bits) -> P, 1 : (x : S-Bits) -> P, z : P) => 1(bs)

-- Natural Number
let S-Nat  (P : Type, s : (x : S-Nat) -> P, z : P) -> P
let S-zero (P : Type, s : (x : S-Nat) -> P, z : P) => z
let S-succ (n : S-Nat) => (P : Type, s : (x : S-Nat) -> P, z : P) => s(n)

-- List of Bitstrings
let S-List (P : Type, c : (x : S-Bits, xs : S-List) -> P, n : P) -> P
let S-nil  (P : Type, c : (x : S-Bits, xs : S-List) -> P, n : P) => n
let S-cons (x : S-Bits, xs : S-List) => (P : Type, c : (x : S-Bits, xs : S-List) -> P, n : P) => c(x, xs)

-- VALUES

-- Some natural numbers
let n0 S-zero
let n1 S-succ(S-zero)
let n2 S-succ(S-succ(S-zero))
let n3 S-succ(S-succ(S-succ(S-zero)))
let n4 S-succ(S-succ(S-succ(S-succ(S-zero))))
let n5 S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))
let n6 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))
let n7 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))))
let n8 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))))
let n9 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))))))
let n10 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))))))
let n11 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))))))))
let n12 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))))))))
let n13 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))))))))))
let n14 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))))))))))
let n15 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))))))))))))
let n16 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))))))))))))
let n17 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))))))))))))))
let n18 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))))))))))))))
let n19 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))))))))))))))))
let n20 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))))))))))))))))
let n21 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero)))))))))))))))))))))
let n22 S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-succ(S-zero))))))))))))))))))))))

-- A string of 32 bits, all zeroes
let u32-zero S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-0(S-z))))))))))))))))))))))))))))))))

-- A list with 100 strings of 32 bits, all zeroes
let list-with-100-zeros
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
  S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero, S-cons(u32-zero,
    S-nil))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))

-- CONVERSIONS

-- Converts a lambda-encoded bitstring to a native bitstring
let to-Bits(bs : S-Bits) => bs((x : S-Bits) -> Bits,
  (bs : S-Bits, f : (bs : S-Bits) -> Bits) => Bits.0(f(bs)),
  (bs : S-Bits, f : (bs : S-Bits) -> Bits) => Bits.1(f(bs)),
  (f : (bs : S-Bits) -> Bits) => Bits.z,
  to-Bits)

-- Converts a lambda-encoded list to a native list
let to-List(xs : S-List) => xs(List,
  (x : S-Bool, xs : S-List) => List.cons(to-Bits(x), to-List(xs)),
  List.nil)

-- FUNCTIONS

-- Increments a bitstring by 1
let inc(bs : S-Bits) =>
  (P : Type, 0 : (bs : S-Bits) -> P, 1 : (bs : S-Bits) -> P, z : P) =>
    bs(S-Bits, 1, (bs : S-Bits) => 0(inc(bs)), z)

-- Maps a function over a list
let map(f : (x : S-Bool) -> S-Bool, xs : S-List) =>
  (P : Type, c : (x : S-Bool, xs : S-List) -> P, n : P) =>
    xs(S-List, (x : S-Bool, xs : List) => copy f as g, h in c(g(x), map(h, xs)), n)

-- Applies a function 2^n times to a value
let apply-pow2n-times(n : S-Nat) =>
  n((f : (x : S-List) -> S-List, x : S-List) -> S-List,
    (n : S-List, f : (x : S-List) -> S-List) => copy f as g, h in apply-pow2n-times(n, (x : S-List) => g(h(x))),
    (f : (x : S-List) -> S-List, x : S-List) => f(x))

-- Applies a function 2^n times to a value
let apply-pow2n-times-bits(n : S-Nat) =>
  n((f : (x : S-Bits) -> S-Bits, x : S-Bits) -> S-Bits,
    (n : S-Bits, f : (x : S-Bits) -> S-Bits) => copy f as g, h in apply-pow2n-times-bits(n, (x : S-Bits) => g(h(x))),
    (f : (x : S-Bits) -> S-Bits, x : S-Bits) => f(x))

-- Doubles the size of a list
let double-size(xs : S-List) =>
  xs(S-List,
    (x : S-Bits, xs : S-List) => copy x as y, z in S-cons(y, S-cons(z, double-size(xs))),
    S-nil)

-- Computes the length of a list
let length(xs : S-List) =>
  let length-aux(xs : S-List) =>
    xs((x : S-Bits) -> S-Bits,
      (x : S-Bits, xs : S-List, r : S-Bits) => length-aux(xs, inc(r)),
      (r : S-Bits) => r)
  length-aux(xs, u32-zero)

-- Flips every bit of a bitstring. Not fusible, in order to force it
-- pattern-matches as many times as expected without being optimized away
let flip-unfusible(bs : S-Bits) => bs((x : S-Bits) -> Bits,
  (bs : S-Bits, f : (bs : S-Bits) -> S-Bits) => S-1(f(bs)),
  (bs : S-Bits, f : (bs : S-Bits) -> S-Bits) => S-0(f(bs)),
  (f : (bs : S-Bits) -> S-Bits) => S-z,
  flip-unfusible)