MODULE voronoi_mesh_interpolation

  USE ISO_FORTRAN_ENV
  IMPLICIT NONE
  PRIVATE
  PUBLIC voronoi_mesh

CONTAINS

  SUBROUTINE voronoi_mesh(data, values, points, predictions)
    ! Given "data" (defining voronoi mesh interpolation points),
    ! the associated "values" for each point in data,
    ! and "points" that are being predicted,
    ! return "predictions" for each point using a voronoi mesh.
    REAL(KIND=REAL64), DIMENSION(:,:), INTENT(IN) :: data
    REAL(KIND=REAL64), DIMENSION(SIZE(data,2)), INTENT(IN) :: values
    REAL(KIND=REAL64), DIMENSION(:,:), INTENT(IN) :: points
    REAL(KIND=REAL64), DIMENSION(SIZE(points,2)), INTENT(OUT) :: predictions
    ! Local variables
    INTEGER :: pt_idx, d_idx
    REAL(KIND=REAL64), DIMENSION(SIZE(data,2)) :: weights
    REAL(KIND=REAL64), DIMENSION(SIZE(data,1)) :: direction

    ! For each point, the process of making a prediction is independent
    ! but it would utilize a lot of memory to store the weights
    DO pt_idx = 1, SIZE(points,2)
       DO d_idx = 1, SIZE(data,2)
          ! Calculate the direction vector (data -> point)
          direction = points(:,pt_idx) - data(:,d_idx)
          ! First, set the weight to be the euclidean distance to the point
          weights(d_idx) = 2 * dist_to_boundary(data, d_idx, direction)
          ! Now, calculate the relative 'weight' that goes from 0 to 1
          weights(d_idx) = SQRT(SUM( direction**2 )) / weights(d_idx)
          ! Make the lienar weight go (1 -> 0) with increasing distance
          weights(d_idx) = MAX(1 - weights(d_idx), 0.0_REAL64)
       END DO
       ! Make the weights convex
       weights = weights / SUM(weights)
       ! Produce the interpolated value
       predictions(pt_idx) = SUM( values * weights )
    END DO

  END SUBROUTINE voronoi_mesh

  FUNCTION dist_to_boundary(data, point_idx, vector) RESULT(distance)
    ! Given a data and an index of a point in data, find the distance to
    ! the edge of the voronoi cell for "point_idx" along direction "vector".
    REAL(KIND=REAL64), DIMENSION(:,:), INTENT(IN) :: data
    INTEGER, INTENT(IN) :: point_idx
    REAL(KIND=REAL64), INTENT(IN), DIMENSION(SIZE(data,1)) :: vector
    REAL(KIND=REAL64) :: distance
    ! Local variables
    REAL(KIND=REAL64), DIMENSION(SIZE(data,1)) :: candidate
    REAL(KIND=REAL64) :: step_size, mult
    INTEGER :: nearest
    ! Set the initial multiple to be that which makes the length of the vector 1
    mult = SQRT(SUM(vector**2))**(-1)
    ! Set the initial step size to 0 (it will be incremented
    step_size = mult
    ! Search along vector for the boundary
    DO WHILE ((0.000001_REAL64 < step_size) .AND. (mult < 1000000_REAL64))
       ! Find the nearest neighbor when travling "mult" along vector
       candidate = data(:,point_idx) + vector*mult
       nearest = naive_nearest(data, candidate)
       IF (nearest .EQ. point_idx) THEN
          ! If the nearest hasn't changed, then increase step size
          step_size = step_size * 2
       ELSE
          ! Otherwise (exited voronoi cell), revert the step and shorten step size
          mult = mult - step_size
          step_size = step_size / 2
       END IF
       mult = mult + step_size
    END DO
    ! Now that we've found the vector to the boundary, compute it's length
    distance = SQRT(SUM( (vector*mult)**2 ))
  END FUNCTION dist_to_boundary

  FUNCTION naive_nearest(data, point) RESULT(nearest)
    ! Find the nearest neighbor with the naive O(n^2) computation.
    REAL(KIND=REAL64), DIMENSION(:,:), INTENT(IN) :: data
    REAL(KIND=REAL64), DIMENSION(SIZE(data,1)), INTENT(IN) :: point
    INTEGER :: nearest
    ! Local variables
    INTEGER :: idx
    REAL(KIND=REAL64), DIMENSION(SIZE(data,2)) :: distances
    ! Find the distance from the current point to all other points
    DO CONCURRENT (idx = 1:SIZE(data,2))
       distances(idx) = SUM( (data(:,idx) - point)**2 )
    END DO
    ! Set the neighbor as the minimum index of those distances
    nearest = MINLOC( distances, 1 )
  END FUNCTION naive_nearest

  !==============================================
  !     Optimizations to be implemented soon
  !==============================================

  SUBROUTINE nearest(data, sorted_by_dim, point, neighbor)
    ! Given "data" and "sort_by_dim" of equal size such that the values
    ! in "sorted_by_dim" are the indices of the sorted "data" along
    ! each dimension, lookup the nearest datum to "point"
    !
    REAL(KIND=REAL64), DIMENSION(:,:), INTENT(IN) :: data
    INTEGER, DIMENSION(SIZE(data)), INTENT(IN) :: sorted_by_dim
    REAL(KIND=REAL64), DIMENSION(SIZE(data,2)), INTENT(IN) :: point
    INTEGER, INTENT(OUT) :: neighbor
    !

  END SUBROUTINE nearest

  SUBROUTINE lookup_index(val, array, index)
    ! Given a value and a sorted array, look up the index of the point
    ! that is immediately less than or equal to that point.
    REAL(KIND=REAL64), INTENT(IN) :: val
    REAL(KIND=REAL64), DIMENSION(:), INTENT(IN) :: array
    INTEGER, INTENT(OUT) :: index
    ! Local variables
    INTEGER :: below, above
    below = 1
    above = SIZE(array)
    find_index : DO WHILE ((above - below) .GE. 1)
       index = (below+above) / 2
       ! If the middle index was truncated to the lower value,
       IF (index .EQ. below) THEN
          above = below
       ELSE IF (array(index) .LT. val) THEN
          below = index
       ELSE IF (array(index) .GT. val) THEN
          above = index
       END IF
    END DO find_index
    index = below
  END SUBROUTINE lookup_index

  SUBROUTINE sort_by_dimension(data, sorted_by_dim)
    ! Given "data" (dimensions along columns), make the values of an
    ! equally sized integer array "sorted_by_dim" the indices in data of
    ! the points in sorted order. Then, data(sorted_by_dim) = sorted(data).
    !
    REAL(KIND=REAL64), DIMENSION(:,:), INTENT(IN) :: data
    INTEGER, DIMENSION(SIZE(data,1), SIZE(data,2)), INTENT(OUT) :: sorted_by_dim
    INTEGER :: dim
    ! Sort along each dimension
    sorting_dims : DO dim = 1, SIZE(data,2)
       CALL QSORTC( data(:,dim), sorted_by_dim(:,dim) )
    END DO sorting_dims
  END SUBROUTINE sort_by_dimension

  SUBROUTINE QSORTC(TO_SORT, SORTED_INDICES)
    ! This is a QuickSort routine adapted from Orderpack 2.0.
    !
    ! Also, this implementation incorporates ideas from "A Practical Introduction
    ! to Data Structures and Algorithm Analysis", by Clifford Shaffer.
    !
    ! It sorts real numbers into ascending numerical order
    ! and keeps an index of the value's original array position.
    !
    ! Author: Will Thacker, Winthrop University, July 2013.
    !
    ! QSORTC sorts the real array A and keeps the index of the value's
    ! original array position along with the value (integer array IDX).
    !
    ! On input:
    !
    ! TO_SORT(:) is the array to be sorted.
    !
    ! On output:
    !
    ! SORTED_INDICES(:) is a list of indices such that
    !   TO_SORT(SORTED_INDICES(i)) = the i-th point in ascending sorted order.
    !
    ! This has been modified in 2016 by Thomas Lux (tchlux@vt.edu) to
    ! not have an effect on the input array and rather to only return
    ! the indices of input elements sorted.
    !
    REAL(KIND=REAL64), DIMENSION(:), INTENT(IN):: TO_SORT
    INTEGER, DIMENSION(SIZE(TO_SORT)), INTENT(OUT):: SORTED_INDICES
    ! Local variables
    REAL(KIND=REAL64), DIMENSION(SIZE(TO_SORT)) :: LOCAL_COPY
    INTEGER:: I   ! Loop iteration variable.
    ! Initialize the copy array of values
    LOCAL_COPY = TO_SORT
    ! Initialize the array of original positions.
    DO CONCURRENT (I=1:SIZE(SORTED_INDICES))
       SORTED_INDICES(I)=I
    END DO
    CALL QSORTC_HELPER(LOCAL_COPY, SORTED_INDICES, 1, SIZE(TO_SORT))
    RETURN
  CONTAINS
    RECURSIVE SUBROUTINE QSORTC_HELPER(A, IDX, ISTART, ISTOP)
      ! This internal recursive subroutine performs the recursive quicksort
      ! algorithm.  It is needed because the boundaries of the part of the
      ! array being sorted change and the initial call to the sort routine
      ! does not need to specify boundaries since, generally, the user will
      ! want to sort the entire array passed.
      !
      ! On input:
      !
      ! A(:) contains a subpart to be sorted.
      !
      ! IDX(i) contains the initial position of the value A(i) before sorting.
      !
      ! ISTART is the starting position of the subarray to be sorted.
      !
      ! ISTOP is the ending position of the subarray to be sorted.
      !
      ! On output:
      !
      ! A(ISTART:ISTOP) will be sorted.
      !
      ! IDX(i) contains the original position for the value at A(i).
      !
      !
      REAL(KIND=REAL64), DIMENSION(:), INTENT (IN OUT):: A
      INTEGER, DIMENSION(SIZE(A)), INTENT(IN OUT):: IDX
      INTEGER, INTENT(IN):: ISTART, ISTOP
      !  Local variables
      INTEGER:: ILEFT ! A position on the left to be swapped with value at IRIGHT.
      INTEGER:: IMID ! The middle position used to select the pivot.
      INTEGER:: IRIGHT ! A position on the right to be swapped with value at ILEFT.
      INTEGER:: ITEMP  ! Used for swapping within IDX.
      REAL(KIND=REAL64):: ATEMP ! Used for swapping.
      REAL(KIND=REAL64):: PIVOT ! Holds the temporary pivot.
      ! INSMAX is used to stop recursively dividing the array and to instead
      ! use a sort that is more efficient for small arrays than quicksort.
      !
      ! The best cutoff point is system dependent.
      INTEGER, PARAMETER:: INSMAX=24
      ! Check to see if we have enough values to make quicksort useful.
      ! Otherwise let the insertion sort handle it.
      IF ((ISTOP - ISTART) .LT. INSMAX) THEN
         CALL INSERTION(A, IDX, ISTART, ISTOP)
      ELSE
         ! Use the median of the first, middle and last items for the pivot
         ! and place the median (pivot) at the end of the list.
         ! Putting it at the end of the list allows for a guard value to keep
         ! the loop from falling off the right end of the array (no need to
         ! check for at the end of the subarray EACH time through the loop).
         IMID = (ISTART + ISTOP)/2
         IF (A(ISTOP) .LT. A(ISTART)) THEN
            ATEMP = A(ISTART)
            A(ISTART) = A(ISTOP)
            A(ISTOP) = ATEMP
            ITEMP = IDX(ISTART)
            IDX(ISTART) = IDX(ISTOP)
            IDX(ISTOP) = ITEMP
         END IF
         IF (A(IMID) .LT. A(ISTOP)) THEN
            ATEMP = A(ISTOP)
            A(ISTOP) = A(IMID)
            A(IMID) = ATEMP
            ITEMP = IDX(ISTOP)
            IDX(ISTOP) = IDX(IMID)
            IDX(IMID) = ITEMP
            IF (A(ISTOP) .LT. A(ISTART)) THEN
               ATEMP = A(ISTOP)
               A(ISTOP) = A(ISTART)
               A(ISTART) = ATEMP
               ITEMP = IDX(ISTOP)
               IDX(ISTOP) = IDX(ISTART)
               IDX(ISTART) = ITEMP
            END IF
         END IF
         ! Now, the first position has a value that is less or equal to the
         ! partition. So, we know it belongs in the left side of the partition
         ! and we can skip it. Also, the pivot is at the end.  So, there is
         ! no need to compare the pivot with itself.
         PIVOT = A(ISTOP)
         ILEFT = ISTART + 1
         IRIGHT = ISTOP - 1
         DO WHILE (ILEFT < IRIGHT)
            ! Find a value in the left side that is bigger than the pivot value.
            ! Pivot is at the right end so ILEFT will not fall off the end
            ! of the subarray.
            DO WHILE (A(ILEFT) .LT. PIVOT)
               ILEFT = ILEFT + 1
            END DO
            DO WHILE (IRIGHT .NE. ILEFT)
               IF (A(IRIGHT) .LT. PIVOT) EXIT
               IRIGHT = IRIGHT - 1
            END DO
            ! Now we have a value bigger than pivot value on the left side that can be
            ! swapped with a value smaller than the pivot on the right side.
            !
            ! This gives us all values less than pivot on the left side of the
            ! array and all values greater than the pivot on the right side.
            ATEMP = A(IRIGHT)
            A(IRIGHT) = A(ILEFT)
            A(ILEFT) = ATEMP
            ITEMP = IDX(IRIGHT)
            IDX(IRIGHT) = IDX(ILEFT)
            IDX(ILEFT) = ITEMP
         END DO
         !
         ! The last swap was in error (since the while condition is not checked
         ! until after the swap is done) so we swap again to fix it.
         ! This is done (once) rather than having an if (done many times) in the
         ! loop to prevent the swapping.
         ATEMP = A(IRIGHT)
         A(IRIGHT) = A(ILEFT)
         A(ILEFT) = ATEMP
         ITEMP = IDX(IRIGHT)
         IDX(IRIGHT) = IDX(ILEFT)
         IDX(ILEFT) = ITEMP
         ! Put the pivot value in its correct spot (between the 2 partitions)
         ! When the WHILE condition finishes, ILEFT is greater than IRIGHT.
         ! So, ILEFT has the position of the first value in the right side.
         ! This is where we can put the pivot (and where it will finally rest,
         ! so no need to look at it again).  Also, place the first value of
         ! the right side (being displaced by the pivot) at the end of the
         ! subarray (since it is bigger than the pivot).
         ATEMP = A(ISTOP)
         A(ISTOP) = A(ILEFT)
         A(ILEFT) = ATEMP
         ITEMP = IDX(ISTOP)
         IDX(ISTOP) = IDX(ILEFT)
         IDX(ILEFT) = ITEMP
         CALL QSORTC_HELPER(A, IDX, ISTART, ILEFT-1)
         CALL QSORTC_HELPER(A, IDX, ILEFT+1, ISTOP)
      END IF
    END SUBROUTINE QSORTC_HELPER

    SUBROUTINE INSERTION(A, IDX, ISTART, ISTOP)
      ! This subroutine performs an insertion sort used for sorting
      ! small subarrays efficiently.
      !
      ! This subroutine sorts a subarray of A (between positions ISTART
      ! and ISTOP) keeping a record of the original position (array IDX).

      ! On input:
      !
      ! A(:) contains a subpart to be sorted.
      !
      ! IDX(i) contains the initial position of the value A(i) before sorting.
      !
      ! ISTART is the starting position of the subarray to be sorted.
      !
      ! ISTOP is the ending position of the subarray to be sorted.
      !
      ! On output:
      !
      ! A(ISTART:ISTOP) will be sorted.
      !
      ! IDX(i) contains the original position for the value at A(i).
      !
      REAL(KIND=REAL64), DIMENSION(:), INTENT (IN OUT):: A
      INTEGER, DIMENSION(SIZE(A)), INTENT(IN OUT)::IDX
      INTEGER, INTENT(IN):: ISTART, ISTOP
      ! Local variables.
      REAL(KIND=REAL64):: AMIN  ! Temporary minimum.
      REAL(KIND=REAL64):: ATEMP  ! The value to be inserted.
      INTEGER:: I    ! Index variable.
      INTEGER:: IABOVE ! Index to find insertion point.
      INTEGER:: IMIN ! Temporary minimum position.
      INTEGER:: ITEMP ! Temporary for swapping.
      IF (ISTOP .EQ. ISTART) THEN
         RETURN
      END IF
      ! Find the smallest and put it at the top as a "guard" so there is
      ! no need for the DO WHILE to check if it is going past the top.
      AMIN = A(ISTART)
      IMIN = ISTART
      DO I=ISTOP,ISTART+1,-1
         IF (A(I) .LT. AMIN) THEN
            AMIN = A(I)
            IMIN = I
         END IF
      END DO
      A(IMIN) = A(ISTART)
      A(ISTART) = AMIN
      ITEMP = IDX(ISTART)
      IDX(ISTART) = IDX(IMIN)
      IDX(IMIN) = ITEMP
      ! Insertion sort the rest of the array.
      DO I=ISTART+2,ISTOP
         ATEMP = A(I)
         ITEMP = IDX(I)
         IABOVE = I - 1
         IF (ATEMP .LT. A(IABOVE)) THEN
            A(I) = A(IABOVE)
            IDX(I) = IDX(IABOVE)
            IABOVE = IABOVE - 1
            ! Stop moving items down when the position for "insertion" is found.
            !
            ! Do not have to check for "falling off" the beginning of the
            ! array since the smallest value is a guard value in the first position.
            DO WHILE (ATEMP .LT. A(IABOVE))
               A(IABOVE+1) = A(IABOVE)
               IDX(IABOVE+1) = IDX(IABOVE)
               IABOVE = IABOVE - 1
            END DO
         END IF
         A(IABOVE+1) = ATEMP
         IDX(IABOVE+1) = ITEMP
      END DO
    END SUBROUTINE INSERTION
  END SUBROUTINE QSORTC

END MODULE voronoi_mesh_interpolation


      SUBROUTINE ZGESVX( FACT, TRANS, N, NRHS, A, LDA, AF, LDAF, IPIV,
     $                   EQUED, R, C, B, LDB, X, LDX, RCOND, FERR, BERR,
     $                   WORK, RWORK, INFO )
*
*  -- LAPACK driver routine (version 3.2) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      CHARACTER          EQUED, FACT, TRANS
      INTEGER            INFO, LDA, LDAF, LDB, LDX, N, NRHS
      DOUBLE PRECISION   RCOND
*     ..
*     .. Array Arguments ..
      INTEGER            IPIV( * )
      DOUBLE PRECISION   BERR( * ), C( * ), FERR( * ), R( * ),
     $                   RWORK( * )
      COMPLEX*16         A( LDA, * ), AF( LDAF, * ), B( LDB, * ),
     $                   WORK( * ), X( LDX, * )
*     ..
*
*  Purpose
*  =======
*
*  ZGESVX uses the LU factorization to compute the solution to a complex
*  system of linear equations
*     A * X = B,
*  where A is an N-by-N matrix and X and B are N-by-NRHS matrices.
*
*  Error bounds on the solution and a condition estimate are also
*  provided.
*
*  Description
*  ===========
*
*  The following steps are performed:
*
*  1. If FACT = 'E', real scaling factors are computed to equilibrate
*     the system:
*        TRANS = 'N':  diag(R)*A*diag(C)     *inv(diag(C))*X = diag(R)*B
*        TRANS = 'T': (diag(R)*A*diag(C))**T *inv(diag(R))*X = diag(C)*B
*        TRANS = 'C': (diag(R)*A*diag(C))**H *inv(diag(R))*X = diag(C)*B
*     Whether or not the system will be equilibrated depends on the
*     scaling of the matrix A, but if equilibration is used, A is
*     overwritten by diag(R)*A*diag(C) and B by diag(R)*B (if TRANS='N')
*     or diag(C)*B (if TRANS = 'T' or 'C').
*
*  2. If FACT = 'N' or 'E', the LU decomposition is used to factor the
*     matrix A (after equilibration if FACT = 'E') as
*        A = P * L * U,
*     where P is a permutation matrix, L is a unit lower triangular
*     matrix, and U is upper triangular.
*
*  3. If some U(i,i)=0, so that U is exactly singular, then the routine
*     returns with INFO = i. Otherwise, the factored form of A is used
*     to estimate the condition number of the matrix A.  If the
*     reciprocal of the condition number is less than machine precision,
*     INFO = N+1 is returned as a warning, but the routine still goes on
*     to solve for X and compute error bounds as described below.
*
*  4. The system of equations is solved for X using the factored form
*     of A.
*
*  5. Iterative refinement is applied to improve the computed solution
*     matrix and calculate error bounds and backward error estimates
*     for it.
*
*  6. If equilibration was used, the matrix X is premultiplied by
*     diag(C) (if TRANS = 'N') or diag(R) (if TRANS = 'T' or 'C') so
*     that it solves the original system before equilibration.
*
*  Arguments
*  =========
*
*  FACT    (input) CHARACTER*1
*          Specifies whether or not the factored form of the matrix A is
*          supplied on entry, and if not, whether the matrix A should be
*          equilibrated before it is factored.
*          = 'F':  On entry, AF and IPIV contain the factored form of A.
*                  If EQUED is not 'N', the matrix A has been
*                  equilibrated with scaling factors given by R and C.
*                  A, AF, and IPIV are not modified.
*          = 'N':  The matrix A will be copied to AF and factored.
*          = 'E':  The matrix A will be equilibrated if necessary, then
*                  copied to AF and factored.
*
*  TRANS   (input) CHARACTER*1
*          Specifies the form of the system of equations:
*          = 'N':  A * X = B     (No transpose)
*          = 'T':  A**T * X = B  (Transpose)
*          = 'C':  A**H * X = B  (Conjugate transpose)
*
*  N       (input) INTEGER
*          The number of linear equations, i.e., the order of the
*          matrix A.  N >= 0.
*
*  NRHS    (input) INTEGER
*          The number of right hand sides, i.e., the number of columns
*          of the matrices B and X.  NRHS >= 0.
*
*  A       (input/output) COMPLEX*16 array, dimension (LDA,N)
*          On entry, the N-by-N matrix A.  If FACT = 'F' and EQUED is
*          not 'N', then A must have been equilibrated by the scaling
*          factors in R and/or C.  A is not modified if FACT = 'F' or
*          'N', or if FACT = 'E' and EQUED = 'N' on exit.
*
*          On exit, if EQUED .ne. 'N', A is scaled as follows:
*          EQUED = 'R':  A := diag(R) * A
*          EQUED = 'C':  A := A * diag(C)
*          EQUED = 'B':  A := diag(R) * A * diag(C).
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,N).
*
*  AF      (input or output) COMPLEX*16 array, dimension (LDAF,N)
*          If FACT = 'F', then AF is an input argument and on entry
*          contains the factors L and U from the factorization
*          A = P*L*U as computed by ZGETRF.  If EQUED .ne. 'N', then
*          AF is the factored form of the equilibrated matrix A.
*
*          If FACT = 'N', then AF is an output argument and on exit
*          returns the factors L and U from the factorization A = P*L*U
*          of the original matrix A.
*
*          If FACT = 'E', then AF is an output argument and on exit
*          returns the factors L and U from the factorization A = P*L*U
*          of the equilibrated matrix A (see the description of A for
*          the form of the equilibrated matrix).
*
*  LDAF    (input) INTEGER
*          The leading dimension of the array AF.  LDAF >= max(1,N).
*
*  IPIV    (input or output) INTEGER array, dimension (N)
*          If FACT = 'F', then IPIV is an input argument and on entry
*          contains the pivot indices from the factorization A = P*L*U
*          as computed by ZGETRF; row i of the matrix was interchanged
*          with row IPIV(i).
*
*          If FACT = 'N', then IPIV is an output argument and on exit
*          contains the pivot indices from the factorization A = P*L*U
*          of the original matrix A.
*
*          If FACT = 'E', then IPIV is an output argument and on exit
*          contains the pivot indices from the factorization A = P*L*U
*          of the equilibrated matrix A.
*
*  EQUED   (input or output) CHARACTER*1
*          Specifies the form of equilibration that was done.
*          = 'N':  No equilibration (always true if FACT = 'N').
*          = 'R':  Row equilibration, i.e., A has been premultiplied by
*                  diag(R).
*          = 'C':  Column equilibration, i.e., A has been postmultiplied
*                  by diag(C).
*          = 'B':  Both row and column equilibration, i.e., A has been
*                  replaced by diag(R) * A * diag(C).
*          EQUED is an input argument if FACT = 'F'; otherwise, it is an
*          output argument.
*
*  R       (input or output) DOUBLE PRECISION array, dimension (N)
*          The row scale factors for A.  If EQUED = 'R' or 'B', A is
*          multiplied on the left by diag(R); if EQUED = 'N' or 'C', R
*          is not accessed.  R is an input argument if FACT = 'F';
*          otherwise, R is an output argument.  If FACT = 'F' and
*          EQUED = 'R' or 'B', each element of R must be positive.
*
*  C       (input or output) DOUBLE PRECISION array, dimension (N)
*          The column scale factors for A.  If EQUED = 'C' or 'B', A is
*          multiplied on the right by diag(C); if EQUED = 'N' or 'R', C
*          is not accessed.  C is an input argument if FACT = 'F';
*          otherwise, C is an output argument.  If FACT = 'F' and
*          EQUED = 'C' or 'B', each element of C must be positive.
*
*  B       (input/output) COMPLEX*16 array, dimension (LDB,NRHS)
*          On entry, the N-by-NRHS right hand side matrix B.
*          On exit,
*          if EQUED = 'N', B is not modified;
*          if TRANS = 'N' and EQUED = 'R' or 'B', B is overwritten by
*          diag(R)*B;
*          if TRANS = 'T' or 'C' and EQUED = 'C' or 'B', B is
*          overwritten by diag(C)*B.
*
*  LDB     (input) INTEGER
*          The leading dimension of the array B.  LDB >= max(1,N).
*
*  X       (output) COMPLEX*16 array, dimension (LDX,NRHS)
*          If INFO = 0 or INFO = N+1, the N-by-NRHS solution matrix X
*          to the original system of equations.  Note that A and B are
*          modified on exit if EQUED .ne. 'N', and the solution to the
*          equilibrated system is inv(diag(C))*X if TRANS = 'N' and
*          EQUED = 'C' or 'B', or inv(diag(R))*X if TRANS = 'T' or 'C'
*          and EQUED = 'R' or 'B'.
*
*  LDX     (input) INTEGER
*          The leading dimension of the array X.  LDX >= max(1,N).
*
*  RCOND   (output) DOUBLE PRECISION
*          The estimate of the reciprocal condition number of the matrix
*          A after equilibration (if done).  If RCOND is less than the
*          machine precision (in particular, if RCOND = 0), the matrix
*          is singular to working precision.  This condition is
*          indicated by a return code of INFO > 0.
*
*  FERR    (output) DOUBLE PRECISION array, dimension (NRHS)
*          The estimated forward error bound for each solution vector
*          X(j) (the j-th column of the solution matrix X).
*          If XTRUE is the true solution corresponding to X(j), FERR(j)
*          is an estimated upper bound for the magnitude of the largest
*          element in (X(j) - XTRUE) divided by the magnitude of the
*          largest element in X(j).  The estimate is as reliable as
*          the estimate for RCOND, and is almost always a slight
*          overestimate of the true error.
*
*  BERR    (output) DOUBLE PRECISION array, dimension (NRHS)
*          The componentwise relative backward error of each solution
*          vector X(j) (i.e., the smallest relative change in
*          any element of A or B that makes X(j) an exact solution).
*
*  WORK    (workspace) COMPLEX*16 array, dimension (2*N)
*
*  RWORK   (workspace/output) DOUBLE PRECISION array, dimension (2*N)
*          On exit, RWORK(1) contains the reciprocal pivot growth
*          factor norm(A)/norm(U). The "max absolute element" norm is
*          used. If RWORK(1) is much less than 1, then the stability
*          of the LU factorization of the (equilibrated) matrix A
*          could be poor. This also means that the solution X, condition
*          estimator RCOND, and forward error bound FERR could be
*          unreliable. If factorization fails with 0<INFO<=N, then
*          RWORK(1) contains the reciprocal pivot growth factor for the
*          leading INFO columns of A.
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0:  if INFO = -i, the i-th argument had an illegal value
*          > 0:  if INFO = i, and i is
*                <= N:  U(i,i) is exactly zero.  The factorization has
*                       been completed, but the factor U is exactly
*                       singular, so the solution and error bounds
*                       could not be computed. RCOND = 0 is returned.
*                = N+1: U is nonsingular, but RCOND is less than machine
*                       precision, meaning that the matrix is singular
*                       to working precision.  Nevertheless, the
*                       solution and error bounds are computed because
*                       there are a number of situations where the
*                       computed solution can be more accurate than the
*                       value of RCOND would suggest.
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            COLEQU, EQUIL, NOFACT, NOTRAN, ROWEQU
      CHARACTER          NORM
      INTEGER            I, INFEQU, J
      DOUBLE PRECISION   AMAX, ANORM, BIGNUM, COLCND, RCMAX, RCMIN,
     $                   ROWCND, RPVGRW, SMLNUM
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      DOUBLE PRECISION   DLAMCH, ZLANGE, ZLANTR
      EXTERNAL           LSAME, DLAMCH, ZLANGE, ZLANTR
*     ..
*     .. External Subroutines ..
      EXTERNAL           XERBLA, ZGECON, ZGEEQU, ZGERFS, ZGETRF, ZGETRS,
     $                   ZLACPY, ZLAQGE
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
*     ..
*     .. Executable Statements ..
*
      INFO = 0
      NOFACT = LSAME( FACT, 'N' )
      EQUIL = LSAME( FACT, 'E' )
      NOTRAN = LSAME( TRANS, 'N' )
      IF( NOFACT .OR. EQUIL ) THEN
         EQUED = 'N'
         ROWEQU = .FALSE.
         COLEQU = .FALSE.
      ELSE
         ROWEQU = LSAME( EQUED, 'R' ) .OR. LSAME( EQUED, 'B' )
         COLEQU = LSAME( EQUED, 'C' ) .OR. LSAME( EQUED, 'B' )
         SMLNUM = DLAMCH( 'Safe minimum' )
         BIGNUM = ONE / SMLNUM
      END IF
*
*     Test the input parameters.
*
      IF( .NOT.NOFACT .AND. .NOT.EQUIL .AND. .NOT.LSAME( FACT, 'F' ) )
     $     THEN
         INFO = -1
      ELSE IF( .NOT.NOTRAN .AND. .NOT.LSAME( TRANS, 'T' ) .AND. .NOT.
     $         LSAME( TRANS, 'C' ) ) THEN
         INFO = -2
      ELSE IF( N.LT.0 ) THEN
         INFO = -3
      ELSE IF( NRHS.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF( LDAF.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LSAME( FACT, 'F' ) .AND. .NOT.
     $         ( ROWEQU .OR. COLEQU .OR. LSAME( EQUED, 'N' ) ) ) THEN
         INFO = -10
      ELSE
         IF( ROWEQU ) THEN
            RCMIN = BIGNUM
            RCMAX = ZERO
            DO 10 J = 1, N
               RCMIN = MIN( RCMIN, R( J ) )
               RCMAX = MAX( RCMAX, R( J ) )
   10       CONTINUE
            IF( RCMIN.LE.ZERO ) THEN
               INFO = -11
            ELSE IF( N.GT.0 ) THEN
               ROWCND = MAX( RCMIN, SMLNUM ) / MIN( RCMAX, BIGNUM )
            ELSE
               ROWCND = ONE
            END IF
         END IF
         IF( COLEQU .AND. INFO.EQ.0 ) THEN
            RCMIN = BIGNUM
            RCMAX = ZERO
            DO 20 J = 1, N
               RCMIN = MIN( RCMIN, C( J ) )
               RCMAX = MAX( RCMAX, C( J ) )
   20       CONTINUE
            IF( RCMIN.LE.ZERO ) THEN
               INFO = -12
            ELSE IF( N.GT.0 ) THEN
               COLCND = MAX( RCMIN, SMLNUM ) / MIN( RCMAX, BIGNUM )
            ELSE
               COLCND = ONE
            END IF
         END IF
         IF( INFO.EQ.0 ) THEN
            IF( LDB.LT.MAX( 1, N ) ) THEN
               INFO = -14
            ELSE IF( LDX.LT.MAX( 1, N ) ) THEN
               INFO = -16
            END IF
         END IF
      END IF
*
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'ZGESVX', -INFO )
         RETURN
      END IF
*
      IF( EQUIL ) THEN
*
*        Compute row and column scalings to equilibrate the matrix A.
*
         CALL ZGEEQU( N, N, A, LDA, R, C, ROWCND, COLCND, AMAX, INFEQU )
         IF( INFEQU.EQ.0 ) THEN
*
*           Equilibrate the matrix.
*
            CALL ZLAQGE( N, N, A, LDA, R, C, ROWCND, COLCND, AMAX,
     $                   EQUED )
            ROWEQU = LSAME( EQUED, 'R' ) .OR. LSAME( EQUED, 'B' )
            COLEQU = LSAME( EQUED, 'C' ) .OR. LSAME( EQUED, 'B' )
         END IF
      END IF
*
*     Scale the right hand side.
*
      IF( NOTRAN ) THEN
         IF( ROWEQU ) THEN
            DO 40 J = 1, NRHS
               DO 30 I = 1, N
                  B( I, J ) = R( I )*B( I, J )
   30          CONTINUE
   40       CONTINUE
         END IF
      ELSE IF( COLEQU ) THEN
         DO 60 J = 1, NRHS
            DO 50 I = 1, N
               B( I, J ) = C( I )*B( I, J )
   50       CONTINUE
   60    CONTINUE
      END IF
*
      IF( NOFACT .OR. EQUIL ) THEN
*
*        Compute the LU factorization of A.
*
         CALL ZLACPY( 'Full', N, N, A, LDA, AF, LDAF )
         CALL ZGETRF( N, N, AF, LDAF, IPIV, INFO )
*
*        Return if INFO is non-zero.
*
         IF( INFO.GT.0 ) THEN
*
*           Compute the reciprocal pivot growth factor of the
*           leading rank-deficient INFO columns of A.
*
            RPVGRW = ZLANTR( 'M', 'U', 'N', INFO, INFO, AF, LDAF,
     $               RWORK )
            IF( RPVGRW.EQ.ZERO ) THEN
               RPVGRW = ONE
            ELSE
               RPVGRW = ZLANGE( 'M', N, INFO, A, LDA, RWORK ) /
     $                  RPVGRW
            END IF
            RWORK( 1 ) = RPVGRW
            RCOND = ZERO
            RETURN
         END IF
      END IF
*
*     Compute the norm of the matrix A and the
*     reciprocal pivot growth factor RPVGRW.
*
      IF( NOTRAN ) THEN
         NORM = '1'
      ELSE
         NORM = 'I'
      END IF
      ANORM = ZLANGE( NORM, N, N, A, LDA, RWORK )
      RPVGRW = ZLANTR( 'M', 'U', 'N', N, N, AF, LDAF, RWORK )
      IF( RPVGRW.EQ.ZERO ) THEN
         RPVGRW = ONE
      ELSE
         RPVGRW = ZLANGE( 'M', N, N, A, LDA, RWORK ) / RPVGRW
      END IF
*
*     Compute the reciprocal of the condition number of A.
*
      CALL ZGECON( NORM, N, AF, LDAF, ANORM, RCOND, WORK, RWORK, INFO )
*
*     Compute the solution matrix X.
*
      CALL ZLACPY( 'Full', N, NRHS, B, LDB, X, LDX )
      CALL ZGETRS( TRANS, N, NRHS, AF, LDAF, IPIV, X, LDX, INFO )
*
*     Use iterative refinement to improve the computed solution and
*     compute error bounds and backward error estimates for it.
*
      CALL ZGERFS( TRANS, N, NRHS, A, LDA, AF, LDAF, IPIV, B, LDB, X,
     $             LDX, FERR, BERR, WORK, RWORK, INFO )
*
*     Transform the solution matrix X to a solution of the original
*     system.
*
      IF( NOTRAN ) THEN
         IF( COLEQU ) THEN
            DO 80 J = 1, NRHS
               DO 70 I = 1, N
                  X( I, J ) = C( I )*X( I, J )
   70          CONTINUE
   80       CONTINUE
            DO 90 J = 1, NRHS
               FERR( J ) = FERR( J ) / COLCND
   90       CONTINUE
         END IF
      ELSE IF( ROWEQU ) THEN
         DO 110 J = 1, NRHS
            DO 100 I = 1, N
               X( I, J ) = R( I )*X( I, J )
  100       CONTINUE
  110    CONTINUE
         DO 120 J = 1, NRHS
            FERR( J ) = FERR( J ) / ROWCND
  120    CONTINUE
      END IF
*
*     Set INFO = N+1 if the matrix is singular to working precision.
*
      IF( RCOND.LT.DLAMCH( 'Epsilon' ) )
     $   INFO = N + 1
*
      RWORK( 1 ) = RPVGRW
      RETURN
*
*     End of ZGESVX
*
      END


PROGRAM array_unit4
  IMPLICIT NONE
  REAL, DIMENSION(1:3) :: a
  REAL, DIMENSION(1:3) :: b
  REAL, DIMENSION(1:1) :: c

  a = (/4.0, 5.0, 9.0 /)
  b = (/6.0, 7.0, 3.0 /)

 c = DOT_PRODUCT(a,b)

  WRITE(6,*) c

END PROGRAM array_unit4


!********************************************************************************
!>
!  Unit tests for STL module.

    program main

    use,intrinsic :: iso_fortran_env, only: wp => real64
    use stl_module

    implicit none

    type(stl_file) :: model
    integer :: istat !! status return code
    integer :: i !! counter
    integer :: j !! counter
    real(wp),dimension(:),allocatable :: x,y,z !! curve coordinates

    real(wp),parameter :: deg2rad = acos(-1.0_wp) / 180.0_wp !! degrees to radians

    write(*,*) 'Running tests...'

    ! add some spheres:
    call model%add_sphere([0.0_wp, 0.0_wp, 0.0_wp], 1.0_wp, 20, 40)
    call model%add_sphere([0.0_wp, 2.0_wp, 0.0_wp], 0.3_wp, 20, 40)
    call model%add_sphere([0.0_wp, 0.0_wp, 2.0_wp], 0.1_wp, 20, 40)
    call model%write_binary_stl_file('spheres.stl',istat)
    if (istat/=0) error stop 'error creating spheres'
    call model%destroy()

    ! spheres with different resolutions:
    do i = 1, 20
        call model%add_sphere([real(i,wp), 0.0_wp, 0.0_wp], 0.5_wp, i, i*2)
    end do
    call model%write_binary_stl_file('spheres2.stl',istat)
    if (istat/=0) error stop 'error creating spheres'
    call model%destroy()

    ! cylinders:
    call model%add_cylinder([0.0_wp,0.0_wp,0.0_wp],&
                            [0.0_wp,0.0_wp,1.0_wp],&
                             0.5_wp,10,initial_cap=.true.,final_cap=.true.)
    call model%add_cylinder([1.0_wp,0.0_wp,0.0_wp],&
                             [0.0_wp,0.0_wp,2.0_wp],&
                              0.4_wp,50,initial_cap=.true.,final_cap=.true.,&
                             initial_normal=[0.0_wp,0.0_wp,-1.0_wp])
     call model%add_cylinder([0.0_wp,3.0_wp,0.0_wp],&
                            [3.0_wp,3.0_wp,3.0_wp],&
                             0.2_wp,10,initial_cap=.true.,final_cap=.true.)
    call model%write_binary_stl_file('cylinders.stl',istat)
    if (istat/=0) error stop 'error creating cylinders'
    call model%destroy()

    ! curves:
    call model%add_curve(x = [ 0.0_wp, 1.0_wp, 2.0_wp, 5.0_wp ], &
                         y = [ 0.0_wp, 1.0_wp, 3.0_wp, 5.0_wp ], &
                         z = [ 0.0_wp, 1.0_wp, 4.0_wp, 5.0_wp ], &
                         radius = 0.1_wp,&
                         num_points = 10, &
                         initial_cap=.true.,final_cap=.true.)
    call model%write_binary_stl_file('curve.stl',istat)
    if (istat/=0) error stop 'error creating curve'
    call model%destroy()

    ! curves:
    allocate(x(360))
    allocate(y(360))
    allocate(z(360)); z = 0.0_wp
    do j = 1, 5
        do i = 1, 360
            x(i) = j*2.0_wp * cos(i*deg2rad) * sin(i/2.0_wp*deg2rad)
            y(i) = j*2.0_wp * sin(i*deg2rad) * sin(i/2.0_wp*deg2rad)
        end do
        call model%add_curve(x = x, &
                             y = y, &
                             z = z, &
                             radius = 0.1_wp,&
                             num_points = 10, &
                             initial_cap=.true.,final_cap=.true.)
    end do
    call model%add_axes(origin = [0.0_wp,0.0_wp,0.0_wp],&
                        vx = [4.0_wp,0.0_wp,0.0_wp],&
                        vy = [0.0_wp,1.0_wp,0.0_wp],&
                        vz = [0.0_wp,0.0_wp,1.0_wp],&
                        radius = 0.1_wp,&
                        num_points = 10,&
                        arrowhead_radius_factor = 2.0_wp,&
                        arrowhead_length_factor = 0.2_wp)
    call model%add_sphere([0.0_wp, 0.0_wp, 0.0_wp], 0.1_wp, 20, 40)
    call model%write_binary_stl_file('curve2.stl',istat)
    if (istat/=0) error stop 'error creating curves'
    call model%destroy()

    ! cone:
    call model%add_cone([0.0_wp,0.0_wp,0.0_wp],&
                        [0.0_wp,0.0_wp,1.0_wp],&
                        radius = 0.5_wp,&
                        num_points= 20, &
                        initial_cap = .true. )
    call model%add_cone([0.0_wp,0.0_wp,2.0_wp],&
                        [0.0_wp,0.0_wp,1.0_wp],&
                        radius = 0.5_wp,&
                        num_points= 20, &
                        initial_cap = .true. )
    call model%write_binary_stl_file('cones.stl',istat)
    if (istat/=0) error stop 'error creating cones'
    call model%destroy()

    ! read test:
    call model%read('cones.stl',istat)
    if (istat/=0) error stop 'error reading binary file'
    call model%write_binary_stl_file('cones_resaved.stl',istat)
    if (istat/=0) error stop 'error writing binary file'
    call model%destroy()

    ! read test 2:
    ! this file is from: https://sbnarchive.psi.edu/pds4/non_mission/gaskell.phobos.shape-model/data/phobos_ver64q.tab
    call model%read_tab_file('data/phobos_ver64q.tab',istat)
    if (istat/=0) error stop 'error reading binary file'
    call model%write_binary_stl_file('./data/phobos_quad64q.stl',istat)
    if (istat/=0) error stop 'error writing binary file'
    call model%destroy()

    write(*,*) 'Done.'

    end program main
!********************************************************************************


Program main

  use mo_kind, only: dp
  use ModLine, only: line

  implicit none

  integer, parameter :: nmax = 14
  integer  :: n
  real(dp) :: x(nmax), y(nmax), t(2), dt(2), s

  n = 14
  x = (/ 3, 4, 6, 6, 6, 7, 8, 9, 11, 11, 12, 12, 14, 16 /)
  y = (/ 24.82, 23.26, 14.77, 19.06, 14.79, 17.66, 11.83, 14.82, 5.16, 11.12, 8.04, 2.72, 0.74, 1.21 /)

  call line(n,x,y,t,dt,s)

end Program main


!----------------------------------------------------------------------
!----------------------------------------------------------------------
!   um3: Computing a 2D unstable manifold in 3D (a phase portrait)
!----------------------------------------------------------------------
!----------------------------------------------------------------------

 SUBROUTINE FUNC(NDIM,U,ICP,PAR,IJAC,F,DFDU,DFDP)
!---------- ----

   IMPLICIT NONE
   INTEGER, INTENT(IN) :: NDIM, IJAC, ICP(*)
   DOUBLE PRECISION, INTENT(IN) :: U(NDIM), PAR(*)
   DOUBLE PRECISION, INTENT(OUT) :: F(NDIM)
   DOUBLE PRECISION, INTENT(INOUT) :: DFDU(NDIM,*), DFDP(NDIM,*)

   DOUBLE PRECISION eps, Period, x, y, z

   eps    = PAR(2)
   Period = PAR(11)

   x = U(1)
   y = U(2)
   z = U(3)

   F(1) =  Period * ( eps*x - y**3 + z**3)
   F(2) =  Period * (     y + x**3 )
   F(3) =  Period * (    -z + x**2 )

 END SUBROUTINE FUNC
!----------------------------------------------------------------------

 SUBROUTINE STPNT(NDIM,U,PAR,T)
!---------- -----

   IMPLICIT NONE
   INTEGER, INTENT(IN) :: NDIM
   DOUBLE PRECISION, INTENT(INOUT) :: U(NDIM), PAR(*)
   DOUBLE PRECISION, INTENT(IN) :: T

   DOUBLE PRECISION, PARAMETER :: &
        theta = 0.25, &
        eps   = 0.5, &
        r0    = 0.03, &
        r1    = r0, &
        Period= 1.d-4
   DOUBLE PRECISION PI

   PI = 4*ATAN(1.D0)

   PAR(1:4) = (/theta,eps,r0,r1/)
   PAR(11) = Period

   U(1) = r0 * COS( 2.0D0 * PI * theta)
   U(2) = r0 * SIN( 2.0D0 * PI * theta)
   U(3) = 0.d0

 END SUBROUTINE STPNT
!----------------------------------------------------------------------

 SUBROUTINE BCND(NDIM,PAR,ICP,NBC,U0,U1,FB,IJAC,DBC)
!---------- ----

   IMPLICIT NONE
   INTEGER, INTENT(IN) :: NDIM, ICP(*), NBC, IJAC
   DOUBLE PRECISION, INTENT(IN) :: PAR(*), U0(NDIM), U1(NDIM)
   DOUBLE PRECISION, INTENT(OUT) :: FB(NBC)
   DOUBLE PRECISION, INTENT(INOUT) :: DBC(NBC,*)

   DOUBLE PRECISION x0,y0,z0 , x1,y1,z1 , PI,theta,r0,r1

   x0 = U0(1)
   y0 = U0(2)
   z0 = U0(3)

   x1 = U1(1)
   y1 = U1(2)
   z1 = U1(3)

   PI = 4*ATAN(1.D0)

   theta = PAR(1)
   r0  = PAR(3)
   r1  = PAR(4)

   FB(1) = x0 - r0 * COS( 2.0 * PI * theta)
   FB(2) = y0 - r0 * SIN( 2.0 * PI * theta)
   FB(3) = z0
   FB(4) = SQRT(x1**2 + y1**2 + z1**2) - r1

 END SUBROUTINE BCND

!----------------------------------------------------------------------
 SUBROUTINE ICND
 END SUBROUTINE ICND

 SUBROUTINE FOPT
 END SUBROUTINE FOPT

 SUBROUTINE PVLS
 END SUBROUTINE PVLS
!----------------------------------------------------------------------


      subroutine profil(iplane)
c********************************************************************
c   This subroutine reads in slope input data from screen or a      *
c   file and computes dimensionless elevations, horizontal          *
c   distances and slopes (assumed linear between points)            *
c                                                                   *
c********************************************************************
c
      integer i, iplane, k, km
      include 'pmxnsl.inc'
      include 'pmxpln.inc'
      include 'pmxslp.inc'
c
c     mxnsl             : maximum number of soil layers
c
c*********************************************************************
c                                                                    *
c   Common Blocks                                                    *
c                                                                    *
c*********************************************************************
c
      include 'cdist.inc'
c
c*********************************************************************
c                                                                    *
c dist variables updated                                             *
c    y(101,mxplan) :                                                 *
c                                                                    *
c*********************************************************************
c
      include 'cslope.inc'
c
      save
      real xstar(mxslp), yu(mxslp), yl(mxslp), slen(mxplan), c(mxslp),
     1    sstar(mxslp)
c
      slen(iplane) = xinput(nslpts(iplane),iplane)
      y(nslpts(iplane),iplane) = 0.0
      do 10 k = 1, nslpts(iplane) - 1
        km = nslpts(iplane) - k
        y(km,iplane) = y(km+1,iplane) + (xinput(km+1,iplane)-
     1      xinput(km,iplane)) * (slpinp(km,iplane)+
     1      slpinp(km+1,iplane)) / 2.0
   10 continue
      avgslp(iplane) = y(1,iplane) / slen(iplane)
c
c     correction by dcf to prevent model bombing - do not allow avgslp=0
c
c     Change of AVGSLP value by Baffaut 1996 - dcf 3/97
c     if (avgslp(iplane).le.0.0) avgslp(iplane) = 0.00001
      if (avgslp(iplane).le.0.0) avgslp(iplane) = 0.000001
      do 20 k = 1, nslpts(iplane)
        sstar(k) = slpinp(k,iplane) / avgslp(iplane)
        xstar(k) = xinput(k,iplane) / slen(iplane)
   20 continue
      do 30 k = 2, nslpts(iplane)
        a(k,iplane) = (sstar(k)-sstar(k-1)) / (xstar(k)-xstar(k-1))
        b(k,iplane) = sstar(k-1) - a(k,iplane) * xstar(k-1)
   30 continue
      yl(1) = 1.0
      xl(1,iplane) = 0.0
      do 40 k = 2, nslpts(iplane)
        yu(k) = yl(k-1)
        xu(k,iplane) = xl(k-1,iplane)
        c(k) = yu(k) + a(k,iplane) * xstar(k-1) ** 2 / 2.0 +
     1      b(k,iplane) * xstar(k-1)
        yl(k) = -a(k,iplane) * xstar(k) ** 2 / 2.0 - b(k,iplane) *
     1      xstar(k) + c(k)
        xl(k,iplane) = xstar(k)
   40 continue
      k = 2
      y(1,iplane) = 1.0
      do 60 i = 2, 101
        xinput(i,iplane) = float(i-1) * 0.01
   50   if (xinput(i,iplane).gt.xstar(k)) then
          k = k + 1
          go to 50
        end if
        y(i,iplane) = -a(k,iplane) * xinput(i,iplane) ** 2 / 2.0 -
     1      b(k,iplane) * xinput(i,iplane) + c(k)
   60 continue
      return
      end


      program inegfloat02
      real a, b
      read *, a, b
      if (a+b.eq.0.0) then
         print *, 'a+b = 0'
         print *, a+b
      endif
      if (a.gt.0.0.and.b.gt.0.0) then
         print *, 'a>0 and b>0'
         print *, a+b
         if (a+b.gt.0.0) then
            print *, 'test redondant a+b>0'
            print *, a+b
         endif
      endif
      if (a.eq.b) then
         print *, 'a-b = 0'
         print *, a-b
      endif
      if (a.eq.-b) then
         print *, 'a+b = 0'
         print *, a+b
      endif
      end


program solution
    integer :: result1
    logical :: work1, work2

    result1 = 0

    ! do n = 111122,111122 ! -> true
    ! do n = 123444,123444 ! -> false
    ! do n = 112233,112233 ! -> true
    !  do n = 112222,112222 ! -> true
    !  do n = 122222,122222 ! -> false
    !  do n = 222223,222223 ! -> false
    do n = 153517,630395
      if (work2(n,99,.false.,0)) then
        result1 = result1 + 1
      end if
    end do
    print *, 'result: ',result1

end program solution
!part
recursive function work2(num, previous,seenPair, seenCount) result (result)

  integer, intent(in) :: num, previous, seenCount
  logical , intent(in) :: seenPair
  logical ::result, digitsDescreasing,seenPairStillValid
  integer::digit, newSeenCount

  seenPairStillValid = seenPair

  digit = mod(num,10)

  digitsDescreasing = (digit <= previous)

  if (digit == previous)then
    newSeenCount = seenCount + 1
  else
    if (seenCount == 2) then
      seenPairStillValid = .true.
    end if
    newSeenCount = 1
  end if

  if (num >= 10) then
    result = digitsDescreasing .and. work2(int(num / 10), digit, seenPairStillValid,newSeenCount )
  else
    result = digitsDescreasing .and. (seenPairStillValid .or.newSeenCount == 2)
  end if
end function work2

! part 1
recursive function work1(num, previous, seenPair) result (result)

  integer, intent(in) :: num, previous
  logical , intent(in) :: seenPair
  logical ::result, digitsDescreasing,seenPairStillValid
  integer::digit

  digit = mod(num,10)

  digitsDescreasing = (digit <= previous)
  seenPairStillValid = seenPair .or. digit == previous

  if (num >= 10) then
    result = digitsDescreasing .and. work1(int(num / 10), digit, seenPairStillValid )
  else
    result = digitsDescreasing .and. seenPairStillValid
  end if
end function work1


c      FORTRAN implementation of gluInvertMatrix by Eric Mascot
c      http://www.mesa3d.org/
c
c      Inverts a 4 by 4 matrix
c
c      m - matrix to invert
c        array of size 16
c
c      invOut - inverted matrix
c        array of size 16
c
c      fail - status of result
c        0 - Normal exit
c        1 - Singular matrix
c
c============================================================
c
c      program example
c        implicit none
c        complex, dimension(16) :: a,b
c        integer :: status
c
c        complex, parameter :: ci = dcmplx(0.d0,1.d0)
c
c        a = (/ complex :: &
c          ci, 0, 0, 0, &
c           0,-1, 0, 0, &
c           0, 0, 1, 0, &
c           0, 0, 0,-1 /)
c
c        call invert4x4(a,b,status)
c
c        if (status.eq.0) then
c          print *, b
c        else
c          print *, "Inversion failed"
c        end if
c
c      end program example

      subroutine invert4x4(m, invOut, fail)
      implicit none

      complex(kind=8), dimension(16), intent(in) :: m
      complex(kind=8), dimension(16), intent(out) :: invOut
      integer, intent(out) :: fail

      integer :: i
      complex(kind=8) :: det
      complex(kind=8), dimension(16) :: inv

      inv(1) = m(6)  * m(11) * m(16) -
     +         m(6)  * m(12) * m(15) -
     +         m(10) * m(7)  * m(16) +
     +         m(10) * m(8)  * m(15) +
     +         m(14) * m(7)  * m(12) -
     +         m(14) * m(8)  * m(11)

      inv(5) = -m(5)  * m(11) * m(16) +
     +          m(5)  * m(12) * m(15) +
     +          m(9)  * m(7)  * m(16) -
     +          m(9)  * m(8)  * m(15) -
     +          m(13) * m(7)  * m(12) +
     +          m(13) * m(8)  * m(11)

      inv(9) = m(5)  * m(10) * m(16) -
     +         m(5)  * m(12) * m(14) -
     +         m(9)  * m(6)  * m(16) +
     +         m(9)  * m(8)  * m(14) +
     +         m(13) * m(6)  * m(12) -
     +         m(13) * m(8)  * m(10)

      inv(13) = -m(5)  * m(10) * m(15) +
     +           m(5)  * m(11) * m(14) +
     +           m(9)  * m(6)  * m(15) -
     +           m(9)  * m(7)  * m(14) -
     +           m(13) * m(6)  * m(11) +
     +           m(13) * m(7)  * m(10)

      inv(2) = -m(2)  * m(11) * m(16) +
     +          m(2)  * m(12) * m(15) +
     +          m(10) * m(3)  * m(16) -
     +          m(10) * m(4)  * m(15) -
     +          m(14) * m(3)  * m(12) +
     +          m(14) * m(4)  * m(11)

      inv(6) = m(1)  * m(11) * m(16) -
     +         m(1)  * m(12) * m(15) -
     +         m(9)  * m(3)  * m(16) +
     +         m(9)  * m(4)  * m(15) +
     +         m(13) * m(3)  * m(12) -
     +         m(13) * m(4)  * m(11)

      inv(10) = -m(1) * m(10) * m(16) +
     +          m(1)  * m(12) * m(14) +
     +          m(9)  * m(2)  * m(16) -
     +          m(9)  * m(4)  * m(14) -
     +          m(13) * m(2)  * m(12) +
     +          m(13) * m(4)  * m(10)

      inv(14) = m(1)  * m(10) * m(15) -
     +          m(1)  * m(11) * m(14) -
     +          m(9)  * m(2)  * m(15) +
     +          m(9)  * m(3)  * m(14) +
     +          m(13) * m(2)  * m(11) -
     +          m(13) * m(3)  * m(10)

      inv(3) = m(2)  * m(7) * m(16) -
     +         m(2)  * m(8) * m(15) -
     +         m(6)  * m(3) * m(16) +
     +         m(6)  * m(4) * m(15) +
     +         m(14) * m(3) * m(8)  -
     +         m(14) * m(4) * m(7)

      inv(7) = -m(1)  * m(7) * m(16) +
     +          m(1)  * m(8) * m(15) +
     +          m(5)  * m(3) * m(16) -
     +          m(5)  * m(4) * m(15) -
     +          m(13) * m(3) * m(8)  +
     +          m(13) * m(4) * m(7)

      inv(11) = m(1)  * m(6) * m(16) -
     +          m(1)  * m(8) * m(14) -
     +          m(5)  * m(2) * m(16) +
     +          m(5)  * m(4) * m(14) +
     +          m(13) * m(2) * m(8)  -
     +          m(13) * m(4) * m(6)

      inv(15) = -m(1)  * m(6) * m(15) +
     +           m(1)  * m(7) * m(14) +
     +           m(5)  * m(2) * m(15) -
     +           m(5)  * m(3) * m(14) -
     +           m(13) * m(2) * m(7)  +
     +           m(13) * m(3) * m(6)

      inv(4) = -m(2)  * m(7) * m(12) +
     +          m(2)  * m(8) * m(11) +
     +          m(6)  * m(3) * m(12) -
     +          m(6)  * m(4) * m(11) -
     +          m(10) * m(3) * m(8)  +
     +          m(10) * m(4) * m(7)

      inv(8) = m(1) * m(7) * m(12) -
     +         m(1) * m(8) * m(11) -
     +         m(5) * m(3) * m(12) +
     +         m(5) * m(4) * m(11) +
     +         m(9) * m(3) * m(8)  -
     +         m(9) * m(4) * m(7)

      inv(12) = -m(1) * m(6) * m(12) +
     +           m(1) * m(8) * m(10) +
     +           m(5) * m(2) * m(12) -
     +           m(5) * m(4) * m(10) -
     +           m(9) * m(2) * m(8)  +
     +           m(9) * m(4) * m(6)

      inv(16) = m(1) * m(6) * m(11) -
     +          m(1) * m(7) * m(10) -
     +          m(5) * m(2) * m(11) +
     +          m(5) * m(3) * m(10) +
     +          m(9) * m(2) * m(7)  -
     +          m(9) * m(3) * m(6)

      det = m(1) * inv(1) +
     +      m(2) * inv(5) +
     +      m(3) * inv(9) +
     +      m(4) * inv(13)

      if (det.eq.0) then
          fail = 1
          return
      end if

      det = 1 / det

      do 10 i = 1,16
          invOut(i) = inv(i) * det
10    continue

      fail = 0
      end subroutine invert4x4


!-------------------------------------------------------------------------------
!> module vector
!!
!! @par Description
!!          module for 3D vector on the sphere
!!
!! @author NICAM developers
!<
!-------------------------------------------------------------------------------
module mod_vector
  !-----------------------------------------------------------------------------
  !
  !++ Used modules
  !
  use mod_precision
  use mod_misc
  !-----------------------------------------------------------------------------
  implicit none
  private
  !-----------------------------------------------------------------------------
  !
  !++ Public procedures
  !
  public :: VECTR_xyz2latlon
  public :: VECTR_latlon2xyz
  public :: VECTR_cross
  public :: VECTR_dot
  public :: VECTR_abs
  public :: VECTR_angle
  public :: VECTR_intersec
  public :: VECTR_anticlockwise
  public :: VECTR_triangle
  public :: VECTR_triangle_plane
  public :: VECTR_rotation
  public :: VECTR_distance

  !-----------------------------------------------------------------------------
  !
  !++ Public parameters & variables
  !
  integer, public, parameter :: I_Xaxis = 1
  integer, public, parameter :: I_Yaxis = 2
  integer, public, parameter :: I_Zaxis = 3

  !-----------------------------------------------------------------------------
  !
  !++ Private procedures
  !
  !-----------------------------------------------------------------------------
  !
  !++ Private parameters & variables
  !
  !-----------------------------------------------------------------------------
contains
  !-----------------------------------------------------------------------------
  subroutine VECTR_xyz2latlon( &
       x,   &
       y,   &
       z,   &
       lat, &
       lon  )
!ESC!    use mod_const, only: &
!ESC!       EPS => CONST_EPS
    implicit none

    real(RP), intent(in)  :: x
    real(RP), intent(in)  :: y
    real(RP), intent(in)  :: z
    real(RP), intent(out) :: lat
    real(RP), intent(out) :: lon

    real(RP) :: length, length_h
    !---------------------------------------------------------------------------

    length = sqrt( x*x + y*y + z*z )

    if ( length < CONST_EPS ) then ! 3D vector length is
       lat = 0.0_RP
       lon = 0.0_RP
       return
    endif

    if    ( z / length >= 1.0_RP ) then ! vector is parallele to z axis.
       lat = asin(1.0_RP)
       lon = 0.0_RP
       return
    elseif( z / length <= -1.0_RP ) then ! vector is parallele to z axis.
       lat = asin(-1.0_RP)
       lon = 0.0_RP
       return
    else
       lat = asin( z / length )
    endif

    length_h = sqrt( x*x + y*y )

    if ( length_h < CONST_EPS ) then
       lon = 0.0_RP
       return
    endif

    if    ( x / length_h >= 1.0_RP ) then
       lon = acos(1.0_RP)
    elseif( x / length_h <= -1.0_RP ) then
       lon = acos(-1.0_RP)
    else
       lon = acos( x / length_h )
    endif

    if( y < 0.0_RP ) lon = -lon

    return
  end subroutine VECTR_xyz2latlon

  !-----------------------------------------------------------------------------
  subroutine VECTR_latlon2xyz( &
       lat,   &
       lon,   &
       x,     &
       y,     &
       z,     &
       radius )
    implicit none

    real(RP), intent(in)  :: lat
    real(RP), intent(in)  :: lon
    real(RP), intent(out) :: x
    real(RP), intent(out) :: y
    real(RP), intent(out) :: z
    real(RP), intent(in)  :: radius
    !---------------------------------------------------------------------------

    x = radius * cos(lat) * cos(lon)
    y = radius * cos(lat) * sin(lon)
    z = radius * sin(lat)

    return
  end subroutine VECTR_latlon2xyz

  !-----------------------------------------------------------------------------
  !> exterior product of vector a->b and c->d
  subroutine VECTR_cross( nv, a, b, c, d )
    implicit none

    real(RP), intent(out) :: nv(3)                  ! normal vector
    real(RP), intent(in)  :: a(3), b(3), c(3), d(3) ! x,y,z(cartesian)
    !---------------------------------------------------------------------------

    nv(1) = ( b(2)-a(2) ) * ( d(3)-c(3) ) &
          - ( b(3)-a(3) ) * ( d(2)-c(2) )
    nv(2) = ( b(3)-a(3) ) * ( d(1)-c(1) ) &
          - ( b(1)-a(1) ) * ( d(3)-c(3) )
    nv(3) = ( b(1)-a(1) ) * ( d(2)-c(2) ) &
          - ( b(2)-a(2) ) * ( d(1)-c(1) )

    return
  end subroutine VECTR_cross

  !-----------------------------------------------------------------------------
  !> interior product of vector a->b and c->d
  subroutine VECTR_dot( l, a, b, c, d )
    implicit none

    real(RP), intent(out) :: l
    real(RP), intent(in)  :: a(3), b(3), c(3), d(3) ! x,y,z(cartesian)
    !---------------------------------------------------------------------------
    ! if a=c=zero-vector and b=d, result is abs|a|^2

    l = ( b(1)-a(1) ) * ( d(1)-c(1) ) &
      + ( b(2)-a(2) ) * ( d(2)-c(2) ) &
      + ( b(3)-a(3) ) * ( d(3)-c(3) )

    return
  end subroutine VECTR_dot

  !-----------------------------------------------------------------------------
  !> length of vector o->a
  subroutine VECTR_abs( l, a )
    implicit none

    real(RP), intent(out) :: l
    real(RP), intent(in)  :: a(3) ! x,y,z(cartesian)
    !---------------------------------------------------------------------------

    l = a(1)*a(1) + a(2)*a(2) + a(3)*a(3)
    l = sqrt(l)

    return
  end subroutine VECTR_abs

  !---------------------------------------------------------------------
  !> calc angle between two vector(b->a,b->c)
  subroutine VECTR_angle( angle, a, b, c )
    implicit none

    real(RP), intent(out) :: angle
    real(RP), intent(in)  :: a(3), b(3), c(3)

    real(RP) :: nv(3), nvlenS, nvlenC
    !---------------------------------------------------------------------

    call VECTR_dot  ( nvlenC, b, a, b, c )
    call VECTR_cross( nv(:),  b, a, b, c )
    call VECTR_abs  ( nvlenS, nv(:) )
    angle = atan2( nvlenS, nvlenC )

    return
  end subroutine VECTR_angle

  !-----------------------------------------------------------------------------
  !> judge intersection of two vector
  subroutine VECTR_intersec( ifcross, p, a, b, c, d )
!ESC!    use mod_const, only: &
!ESC!       EPS => CONST_EPS
    implicit none

    logical,  intent(out) :: ifcross
    ! .true. : line a->b and c->d intersect
    ! .false.: line a->b and c->d do not intersect and p = (0,0)
    real(RP), intent(out) :: p(3) ! intersection point
    real(RP), intent(in)  :: a(3), b(3), c(3), d(3)

    real(RP), parameter :: o(3) = 0.0_RP

    real(RP) :: oaob(3), ocod(3), cdab(3)
    real(RP) :: ip, length
    real(RP) :: angle_aop, angle_pob, angle_aob
    real(RP) :: angle_cop, angle_pod, angle_cod
    !---------------------------------------------------------------------

    call VECTR_cross( oaob, o, a, o, b )
    call VECTR_cross( ocod, o, c, o, d )
    call VECTR_cross( cdab, o, ocod, o, oaob )

    call VECTR_abs  ( length, cdab )
    call VECTR_dot  ( ip, o, cdab, o, a )

    p(:) = cdab(:) / sign(length,ip)
!    if( IO_L ) write(IO_FID_LOG,*), 'p:', p(:)

    call VECTR_angle( angle_aop, a, o, p )
    call VECTR_angle( angle_pob, p, o, b )
    call VECTR_angle( angle_aob, a, o, b )
!    if( IO_L ) write(IO_FID_LOG,*), 'angle a-p-b:', angle_aop, angle_pob, angle_aob

    call VECTR_angle( angle_cop, c, o, p )
    call VECTR_angle( angle_pod, p, o, d )
    call VECTR_angle( angle_cod, c, o, d )
!    if( IO_L ) write(IO_FID_LOG,*), 'angle c-p-d:', angle_cop, angle_pod, angle_cod

!    if( IO_L ) write(IO_FID_LOG,*), 'judge:', angle_aob-(angle_aop+angle_pob), angle_cod-(angle_cop+angle_pod)

    ! --- judge intersection
    if (       abs(angle_aob-(angle_aop+angle_pob)) < CONST_EPS &
         .AND. abs(angle_cod-(angle_cop+angle_pod)) < CONST_EPS &
         .AND. abs(angle_aop) > CONST_EPS                       &
         .AND. abs(angle_pob) > CONST_EPS                       &
         .AND. abs(angle_cop) > CONST_EPS                       &
         .AND. abs(angle_pod) > CONST_EPS                       ) then
       ifcross = .true.
    else
       ifcross = .false.
       p(:) = 0.0_RP
    endif

    return
  end subroutine VECTR_intersec

  !---------------------------------------------------------------------
  !> bubble sort anticlockwise by angle
  subroutine VECTR_anticlockwise( vertex, nvert )
!ESC!    use mod_const, only: &
!ESC!       EPS => CONST_EPS
    implicit none

    integer,  intent(in)    :: nvert
    real(RP), intent(inout) :: vertex(nvert,3)

    real(RP), parameter :: o(3) = 0.0_RP

    real(RP) :: v1(3), v2(3), v3(3)
    real(RP) :: xp(3), ip
    real(RP) :: angle1, angle2

    integer  :: i, j
    !---------------------------------------------------------------------

    do j = 2  , nvert-1
    do i = j+1, nvert
       v1(:) = vertex(1,:)
       v2(:) = vertex(j,:)
       v3(:) = vertex(i,:)

       call VECTR_cross( xp(:), v1(:), v2(:), v1(:), v3(:) )
       call VECTR_dot  ( ip, o(:), v1(:), o(:), xp(:) )

       if ( ip < -CONST_EPS ) then ! right hand : exchange
!          if( IO_L ) write(IO_FID_LOG,*) 'exchange by ip', i, '<->',j
          vertex(i,:) = v2(:)
          vertex(j,:) = v3(:)
       endif

    enddo
    enddo

    v1(:) = vertex(1,:)
    v2(:) = vertex(2,:)
    v3(:) = vertex(3,:)
    ! if 1->2->3 is on the line
    call VECTR_cross( xp(:), v1(:), v2(:), v1(:), v3(:) )
    call VECTR_dot  ( ip, o(:), v1(:), o(:), xp(:) )
    call VECTR_angle( angle1, v1(:), o, v2(:) )
    call VECTR_angle( angle2, v1(:), o, v3(:) )
!    if( IO_L ) write(IO_FID_LOG,*) ip, angle1, angle2, abs(angle1)-abs(angle2)

    if (       abs(ip)                 < CONST_EPS  &      ! on the same line
         .AND. abs(angle2)-abs(angle1) < 0.0_RP     ) then ! which is far?
!       if( IO_L ) write(IO_FID_LOG,*) 'exchange by angle', 2, '<->', 3
       vertex(2,:) = v3(:)
       vertex(3,:) = v2(:)
    endif

    v2(:) = vertex(nvert  ,:)
    v3(:) = vertex(nvert-1,:)
    ! if 1->nvert->nvert-1 is on the line
    call VECTR_cross( xp(:), v1(:), v2(:), v1(:), v3(:) )
    call VECTR_dot  ( ip, o(:), v1(:), o(:), xp(:) )
    call VECTR_angle( angle1, v1(:), o, v2(:) )
    call VECTR_angle( angle2, v1(:), o, v3(:) )
!    if( IO_L ) write(IO_FID_LOG,*) ip, angle1, angle2, abs(angle1)-abs(angle2)

    if (       abs(ip)                 < CONST_EPS  &      ! on the same line
         .AND. abs(angle2)-abs(angle1) < 0.0_RP     ) then ! which is far?
!       if( IO_L ) write(IO_FID_LOG,*) 'exchange by angle', nvert, '<->', nvert-1
       vertex(nvert,  :) = v3(:)
       vertex(nvert-1,:) = v2(:)
    endif

    return
  end subroutine VECTR_anticlockwise

  !-----------------------------------------------------------------------------
  !> calc triangle area
  !> @return area
  function VECTR_triangle( &
       a, b, c,      &
       polygon_type, &
       radius      ) &
       result(area)
!ESC!    use mod_const, only: &
!ESC!       PI  => CONST_PI, &
!ESC!       EPS => CONST_EPS
    implicit none

    real(RP),         intent(in) :: a(3), b(3), c(3)
    character(len=*), intent(in) :: polygon_type
    real(RP),         intent(in) :: radius
    real(RP)                     :: area

    real(RP), parameter :: o(3) = 0.0_RP

    ! ON_PLANE
    real(RP) :: abc(3)
    real(RP) :: prd, r

    ! ON_SPHERE (Using three angles)
!     real(RP) :: angle(3)
!     real(RP) :: oaob(3), oaoc(3)
!     real(RP) :: oboc(3), oboa(3)
!     real(RP) :: ocoa(3), ocob(3)
!     real(RP) :: abab, acac
!     real(RP) :: bcbc, baba
!     real(RP) :: caca, cbcb

    ! ON_SPHERE (Using three sides: lHuillier formula)
    real(RP) :: len(3)
    real(RP) :: s, x
    !---------------------------------------------------------------------------

    area = 0.0_RP

    if ( polygon_type == 'ON_PLANE' ) then ! Note : On a plane, area = | ourter product of two vectors |.

       call VECTR_cross( abc(:), a(:), b(:), a(:), c(:) )
       call VECTR_abs( prd, abc(:) )
       call VECTR_abs( r  , a(:)   )

       prd = 0.5_RP * prd !! triangle area
       if ( r < CONST_EPS ) then
          print *, 'zero length?', a(:)
       else
          r = 1.0_RP / r   !! 1 / length
       endif

       area = prd * r*r * radius*radius

!     elseif( polygon_type == 'ON_SPHERE' ) then ! On a unit sphere, area = sum of angles - pi.
!
!        ! angle 1
!        call VECTR_cross( oaob(:), o(:), a(:), o(:), b(:) )
!        call VECTR_cross( oaoc(:), o(:), a(:), o(:), c(:) )
!        call VECTR_abs( abab, oaob(:) )
!        call VECTR_abs( acac, oaoc(:) )
!
!        if ( abab < CONST_EPS .OR. acac < CONST_EPS ) then
!           !write(*,'(A,3(ES20.10))') 'zero length abab or acac:', abab, acac
!           return
!        endif
!
!        call VECTR_angle( angle(1), oaob(:), o(:), oaoc(:) )
!
!        ! angle 2
!        call VECTR_cross( oboc(:), o(:), b(:), o(:), c(:) )
!        oboa(:) = -oaob(:)
!        call VECTR_abs( bcbc, oboc(:) )
!        baba = abab
!
!        if ( bcbc < CONST_EPS .OR. baba < CONST_EPS ) then
!           !write(*,'(A,3(ES20.10))') 'zero length bcbc or baba:', bcbc, baba
!           return
!        endif
!
!        call VECTR_angle( angle(2), oboc(:), o(:), oboa(:) )
!
!        ! angle 3
!        ocoa(:) = -oaoc(:)
!        ocob(:) = -oboc(:)
!        caca = acac
!        cbcb = bcbc
!
!        if ( caca < CONST_EPS .OR. cbcb < CONST_EPS ) then
!           !write(*,'(A,3(ES20.10))') 'zero length caca or cbcb:', caca, cbcb
!           return
!        endif
!
!        call VECTR_angle( angle(3), ocoa(:), o(:), ocob(:) )
!
!        ! calc area
!        area = ( angle(1)+angle(2)+angle(3) - CONST_PI ) * radius*radius
!
    elseif( polygon_type == 'ON_SPHERE' ) then ! On a unit sphere

       call VECTR_angle( len(1), a(:), o(:), b(:) )
       call VECTR_angle( len(2), b(:), o(:), c(:) )
       call VECTR_angle( len(3), c(:), o(:), a(:) )

       ! calc area (lHuillier formula)
       len(1) = 0.5_RP * len(1)
       len(2) = 0.5_RP * len(2)
       len(3) = 0.5_RP * len(3)
       s      = 0.5_RP * ( len(1) + len(2) + len(3) )

       x = tan( s ) * tan( s-len(1) ) * tan( s-len(2) ) * tan( s-len(3) )
       x = max(x,0.0_RP)

       area = 4.0_RP * atan(sqrt(x)) * radius*radius

    endif

    return
  end function VECTR_triangle

  !-----------------------------------------------------------------------------
  !> calc triangle area on plane
  !> @return area
  function VECTR_triangle_plane( &
       a, b, c ) &
       result(area)
    implicit none

    real(RP), intent(in) :: a(3), b(3), c(3)
    real(RP)             :: area

    real(RP) :: len_ab, len_ac, prd
    !---------------------------------------------------------------------------

    call VECTR_dot( len_ab, a, b, a, b )
    call VECTR_dot( len_ac, a, c, a, c )
    call VECTR_dot( prd   , a, b, a, c )

    area = 0.5_RP * sqrt( len_ab * len_ac - prd * prd )

  end function VECTR_triangle_plane

  !-----------------------------------------------------------------------------
  !> Apply rotation matrix
  subroutine VECTR_rotation( &
      a,     &
      angle, &
      iaxis  )
    implicit none

    real(RP), intent(inout) :: a(3)
    real(RP), intent(in)    :: angle
    integer,  intent(in)    :: iaxis

    real(RP) :: m(3,3), b(3)
    !---------------------------------------------------------------------------

    if ( iaxis == I_Xaxis ) then
       m(1,1) =        1.0_RP
       m(1,2) =        0.0_RP
       m(1,3) =        0.0_RP

       m(2,1) =        0.0_RP
       m(2,2) =  cos(angle)
       m(2,3) =  sin(angle)

       m(3,1) =        0.0_RP
       m(3,2) = -sin(angle)
       m(3,3) =  cos(angle)
    elseif( iaxis == I_Yaxis ) then
       m(1,1) =  cos(angle)
       m(1,2) =        0.0_RP
       m(1,3) = -sin(angle)

       m(2,1) =        0.0_RP
       m(2,2) =        1.0_RP
       m(2,3) =        0.0_RP

       m(3,1) =  sin(angle)
       m(3,2) =        0.0_RP
       m(3,3) =  cos(angle)
    elseif( iaxis == I_Zaxis ) then
       m(1,1) =  cos(angle)
       m(1,2) =  sin(angle)
       m(1,3) =        0.0_RP

       m(2,1) = -sin(angle)
       m(2,2) =  cos(angle)
       m(2,3) =        0.0_RP

       m(3,1) =        0.0_RP
       m(3,2) =        0.0_RP
       m(3,3) =        1.0_RP
    else
       return
    endif

    b(1) = m(1,1) * a(1) + m(1,2) * a(2) + m(1,3) * a(3)
    b(2) = m(2,1) * a(1) + m(2,2) * a(2) + m(2,3) * a(3)
    b(3) = m(3,1) * a(1) + m(3,2) * a(2) + m(3,3) * a(3)

    a(:) = b(:)

    return
  end subroutine VECTR_rotation

  !-----------------------------------------------------------------------
  !> Get horizontal distance on the sphere
  subroutine VECTR_distance( &
       r,    &
       lon1, &
       lat1, &
       lon2, &
       lat2, &
       dist  )
    implicit none

    real(RP), intent(in)  :: r          ! radius in meter
    real(RP), intent(in)  :: lon1, lat1 ! in radian
    real(RP), intent(in)  :: lon2, lat2 ! in radian
    real(RP), intent(out) :: dist       ! distance of the two points in meter

    real(RP) :: gmm, gno_x, gno_y
    !-----------------------------------------------------------------------

    gmm = sin(lat1) * sin(lat2) &
        + cos(lat1) * cos(lat2) * cos(lon2-lon1)

    gno_x = gmm * ( cos(lat2) * sin(lon2-lon1) )
    gno_y = gmm * ( cos(lat1) * sin(lat2) &
                  - sin(lat1) * cos(lat2) * cos(lon2-lon1) )

    dist = r * atan2( sqrt(gno_x*gno_x+gno_y*gno_y), gmm )

    return
  end subroutine VECTR_distance

end module mod_vector


module core
contains
  SUBROUTINE qcheck(ret, q)
    LOGICAL, INTENT(OUT) :: ret
    REAL, DIMENSION(4), INTENT(IN) :: q
    REAL :: qout
    qout = q(1)**2 + q(2)**2 + q(3)**2 + q(4)**2
    IF (qout > 1.0) THEN
       ret = .FALSE.
    ELSE
       ret = .TRUE.
    END IF
  END SUBROUTINE qcheck
  subroutine mul(qproduct, q1, q2)
    ! q = [x, y, z, w]
    REAL, DIMENSION(4), INTENT(OUT) :: qout
    REAL, DIMENSION(4), INTENT(IN) :: q1, q2
    qproduct = &
         [q1(4)*q2(1) + q1(1)*q2(4) + q1(2)*q2(3) - q1(3)*q2(2), &
         q1(4)*q2(2) - q1(1)*q2(3) + q1(2)*q2(3) + q1(3)*q2(1), &
         q1(4)*q2(3) + q1(1)*q2(2) - q1(2)*q2(1) + q1(3)*q2(4), &
         q1(4)*q2(4) - q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3)]
  END SUBROUTINE mul
  subroutine norm(rotmat, euler)
    real, dimension :: rotmat
  end subroutine norm
  subroutine conj(quat)
    real, dimension :: quat
  end subroutine conj
end module core


program tarefae
    implicit real(8) (a-h,o-z)
    parameter (pi = 4d0*datan(1d0))
    character(70) filename
    dimension alfa0(3), r0(2)
    dimension r(2), v(2), a(2), g(2), d(2)

    e0 = 0.001
    dx = 0.3
    dy = 0.3
    alfa0 = (/-pi/4, 0d0, pi/4/)
    g0 = 10d0
    r0 = (/0d0, 100d0/)
    v0 = 10d0
    t0 = 0d0

    do i = 1, 3
        ! Parametros iniciais
        e = e0                              ! Incremento temporal
        alfa = alfa0(i)                     ! Angulo alfa
        d = (/dx, dy/)                      ! fator de choque
        g = (/0d0, g0/)                     ! gravidade
        a = -g                              ! Aceleração
        r = r0                              ! Posição
        v = v0*(/dcos(alfa), dsin(alfa)/)   ! Velocidade Decomposta
        v = v + e*a/2                       ! Velocidade
        t = t0                              ! Tempo

        write(filename,'(A,1(I0,A))') 'saida-e-al',i,'-10407962.dat'

        open(10, file=filename)

        do while (norm2(d*v).ge.1e-3)
            write(10, '(F0.6,4(" ",F0.6))') t, r, v
            a = -g
            v = v + e*a/2
            r = r + e*v
            t = t + e
            if (r(2).lt.0) then
                r(2) = abs(r(2))
                v = abs(v) - d*abs(v)
            end if
        end do

        close(10)
    end do

end program tarefae


module constitutiveLib
use funcAux
implicit none
private  Delfino, MooneyRivlin, Fibre, vol, calcInvCfromF, calcInvCfromC, calcInvCMfromF, calcInvCMfromC
public StrainEnergy, StrainEnergyC

contains

subroutine vol(Psi,InvC,MatPar)
	implicit none
	real*8 , intent(out) :: Psi
	real*8 , intent(in) :: InvC(:),MatPar
	real*8 :: c1,I3, J
	c1 = MatPar ! kappa

	J = dsqrt(InvC(3)) ! J

	Psi = c1*(J - 1.0d0)**2.0d0

end subroutine

!~ subroutine vol(Psi,invC,MatPar)
!~ 	implicit none
!~ 	real*8 , intent(out) :: Psi
!~ 	real*8 , intent(in) :: InvC(:),MatPar
!~ 	real*8 :: c1,J
!~ 	c1 = MatPar ! kappa
!~
!~ 	J = dsqrt(invC(3)) ! J
!~
!~ 	Psi = 0.5d0*c1*( J*J - 1.0d0 - 2.0d0*dlog(J) )
!~ 	return
!~ end subroutine

!~ subroutine vol(Psi,invC,MatPar)
!~ 	implicit none
!~ 	real*8 , intent(out) :: Psi
!~ 	real*8 , intent(in) :: InvC(:),MatPar
!~ 	real*8 :: c1,J
!~ 	c1 = MatPar ! kappa
!~
!~ 	J = dsqrt(invC(3)) ! J
!~
!~ 	Psi = 0.5d0*c1*( 0.5d0*(J*J - 1.0d0) - dlog(J) )
!~ 	return
!~ end subroutine

!~
!~ subroutine vol(Psi,invC,MatPar)
!~ 	implicit none
!~ 	real*8 , intent(out) :: Psi
!~ 	real*8 , intent(in) :: InvC(:),MatPar
!~ 	real*8 :: c1, J
!~ 	c1 = MatPar ! kappa
!~
!~ 	J = dsqrt(InvC(3)) ! J

!~ 	Psi = c1*dlog(J)**2.0d0
!~ 	return
!~ end subroutine

!~ subroutine vol2(Psi,C,MatPar)
!~ 	implicit none
!~ 	real*8 , intent(out) :: Psi
!~ 	real*8 , intent(in) :: C(:,:),MatPar(2)
!~ 	real*8 :: c1,c2,I3
!~
!~ 	c1 = 2.0d0*MatPar(1) ! kappa
!~ 	c2 = MatPar(2) ! kappa
!~
!~ 	write(0,*) "New volumetric penalty function"
!~
!~ 	call calcI3(I3,C)
!~ 	I3 = dlog(dsqrt(I3)) ! J
!~
!~ 	Psi = -c1*I3 + c2*I3*I3
!~ 	return
!~ end subroutine

subroutine Delfino(Psi,invC,MatPar)
! Delfino et al. [10] proposed an (isotropic) rubber-like potential for carotid arteries
          ! [10] A. Delfino, N. Stergiopulos, J.E. Moore and J.-J. Meister,
          ! Residual strain effects on the stress field in a thick wall
          ! finite element model of the human carotid bifurcation.
          ! J. Biomech. 30 (1997) 777786.
	implicit none
	real*8 , intent(out) :: Psi
	real*8 , intent(in) :: invC(:),MatPar(2)
	real*8 :: c1,c2,c3,I1bar,J,rI33
	c1 = MatPar(1) ! a
	c2 = MatPar(2)*0.5d0 ! b/2

	J = dsqrt(InvC(3)) ! J
	rI33 = J**(-2.0D0/3.0D0)
	I1bar = rI33*InvC(1) ! I1bar

	Psi = 0.5d0*(c1/c2)*( Dexp(c2*(I1bar-3.0d0))-1.0d0 )

end subroutine

!~ subroutine MooneyRivlin(Psi,invC,MatPar)
!~  !Mooney Rivlin Bathe, K.J., Finite Element Procedures, pp.592; see also 6.4 pp.561
!~  !Crisfield M.A. Vol II, Chap 13, pp 62. See also, Section 10.3 pp 07
!~ 	implicit none
!~ 	real*8 , intent(out) :: Psi
!~ 	real*8 , intent(in) :: invC(:),MatPar(2)
!~ 	real*8 :: c1,c2,I1bar,I2bar,J,rI33
!~ 	c1 = MatPar(1)
!~ 	c2 = MatPar(2)
!~
!~ 	J = dsqrt(InvC(3)) ! J
!~ 	rI33 = J**(-2.0D0/3.0D0)
!~ 	I1bar = rI33*InvC(1) ! I1bar
!~ 	I2bar = rI33*rI33*invC(2) ! I2bar
!~ 	Psi = c1*(I1bar-3.0d0) + c2*(I2bar-3.0d0)
!~ 	return
!~ end subroutine

subroutine MooneyRivlin(Psi,invC,MatPar)
 !Mooney Rivlin Bathe, K.J., Finite Element Procedures, pp.592; see also 6.4 pp.561
 !Crisfield M.A. Vol II, Chap 13, pp 62. See also, Section 10.3 pp 07
	implicit none
	real*8 , intent(out) :: Psi
	real*8 , intent(in) :: invC(:),MatPar(2)
	real*8 :: c1,c2,I1bar,I2bar,J,rI33
	c1 = MatPar(1)
	c2 = MatPar(2)

	J = dsqrt(InvC(3)) ! J
	rI33 = J**(-2.0D0/3.0D0)
	I1bar = rI33*InvC(1) ! I1bar
	I2bar = rI33*rI33*invC(2) ! I2bar
	Psi = c1*(I1bar-3.0d0) + c2*(I2bar-3.0d0)
	return
end subroutine

subroutine Fibre(Psi,invCM,MatPar) ! see my dissertation
	implicit none
	real*8 , intent(out) :: Psi
	real*8 , intent(in) :: invCM(:),MatPar(2) ! MatPar is only the material coeficient, a0 was splitted
 	real*8 :: c1,c2,I4
	c1 = MatPar(1) ! k1
	c2 = MatPar(2) ! k2

	I4 = invCM(2) !! already incompressible

	Psi = 0.5d0*(c1/c2)*( Dexp(c2*(I4-1.0d0)**2.0d0)-1.0d0 )

end subroutine

subroutine calcInvCMfromF(invCM,F,beta)
	real*8 , intent(in) :: F(:,:), beta
	real*8 , intent(out) :: InvCM(:)
	real*8 :: v(size(F,1)), J , a0(size(F,1))

	a0(1) = dcos(beta)
	a0(2) = dsin(beta)

	v = matmul(F,a0)
	InvCM(1) = dot_product(v,v)

	call calcI3(J,F)

	InvCM(2) = (J**(-2.0D0/3.0D0))*InvCM(1) ! incompressible

end subroutine

subroutine calcInvCMfromC(invCM,C,beta)
	real*8 , intent(in) :: C(:,:), beta
	real*8 , intent(out) :: InvCM(:)
	real*8 :: v(size(C,1)), J, a0(size(C,1))

	a0(1) = dcos(beta)
	a0(2) = dsin(beta)

	v = matmul(C,a0)
	InvCM(1) = dot_product(v,a0)

	call calcI3(J,C)
	J = dsqrt(J)

	InvCM(2) = (J**(-2.0D0/3.0D0))*InvCM(1) ! incompressible

end subroutine

subroutine calcInvCfromC(invC,C)
	real*8 , intent(in) :: C(:,:)
	real*8 , intent(out) :: InvC(:)

	call calcI1(InvC(1),C)
	call calcI2(InvC(2),C)
	call calcI3(InvC(3),C)

end subroutine

subroutine calcInvCfromF(invC,F)
	real*8 , intent(in) :: F(:,:)
	real*8 , intent(out) :: InvC(:)
	real*8 :: C(size(F,1),size(F,2))

	C = matmul(transpose(F),F)

	call calcI1(InvC(1),C)
	call calcI2(InvC(2),C)
	call calcI3(InvC(3),C)

end subroutine

subroutine strainEnergy(energy,F,matPar,constLaw)
	real*8, intent(in) :: F(:,:) , matPar(:)
	real*8 , intent(out) :: energy
	integer , intent(in) :: constLaw
	real*8 :: energyTemp, invC(3)

	if(constLaw<10) then
		call calcInvCfromF(invC,F)
	else
		call calcInvCMfromF(invC,F,matPar(3))
	end if

	energy = 0.0d0
	energyTemp = 0.0d0

	selectcase(constLaw)
		case(1)
			call Delfino(energy,invC,matPar(1:2))
		case(2)
			call MooneyRivlin(energy,invC,matPar(1:2))
		case(3)
			call Delfino(energy,invC,matPar(1:2))
			call vol(energyTemp,invC,matPar(3))
			energy = energy + energyTemp
		case(4)
			call MooneyRivlin(energy,invC,matPar(1:2))
            call vol(energyTemp,invC,matPar(3))
            energy = energy + energyTemp
		case(13)
			call Fibre(energy,invC,matPar(1:2))
		case default
			write(0,*) constLaw, matpar(1:3), "Strain Energy case unknown"
			PAUSE
	end select

end subroutine

subroutine strainEnergyC(energy,C,matPar,constLaw)
	real*8, intent(in) :: C(:,:) , matPar(:)
	real*8 , intent(out) :: energy
	integer , intent(in) :: constLaw
	real*8 ::  energyTemp, invC(3)

	if(constLaw<10) then
		call calcInvCfromC(invC,C)
	else
		call calcInvCMfromC(invC,C,matPar(3))
	end if

	energy = 0.0d0
	energyTemp = 0.0d0

	selectcase(constLaw)
		case(1)
			call Delfino(energy,invC,matPar(1:2))
		case(2)
			call MooneyRivlin(energy,invC,matPar(1:2))
		case(3)
			call Delfino(energy,invC,matPar(1:2))
			call vol(energyTemp,invC,matPar(3))
			energy = energy + energyTemp
		case(4)
			call MooneyRivlin(energy,invC,matPar(1:2))
            call vol(energyTemp,invC,matPar(3))
            energy = energy + energyTemp
		case(13)
			call Fibre(energy,invC,matPar(1:2))
		case default
			write(0,*) constLaw, matpar(1:3), "Strain Energy case unknown"
			PAUSE
	end select

end subroutine

end module


      SUBROUTINE HOCAPR(
     &         CCENT1, CCENT2, CAPR, DIREC, EFFRAD)
      IMPLICIT NONE
      SAVE
C ********************************************************************
C *                                                                  *
C * This software is an unpublished work containing confidential and *
C * proprietary information of Birkbeck College. Use, disclosure,    *
C * reproduction and transfer of this work without the express       *
C * written consent of Birkbeck College are prohibited. This notice  *
C * must be attached to all copies or extracts of the software.      *
C *                                                                  *
C * (c) 1995 Oliver Smart & Birkbeck College, All rights reserved    *
C * (c) 1996 Oliver Smart & Birkbeck College, All rights reserved    *
C *                                                                  *
C ********************************************************************
C
C Modification history:
C
C Date  Author          Modification
C 10/95 O.S. Smart      Original version
C
C This s/r works out the effective radius on a direction
C of DIREC from the mid-point of a capsule defined by
C centres CCENT1 and CCENT2 and a capsule radius of CAPR.
C All vbles other than EFFRAD must be returned unchanged
C n.b. DIREC is assumed to be supplied as a unit vector
      DOUBLE PRECISION		CCENT1(3), CCENT2(3), CAPR,
     &				DIREC(3), EFFRAD

C internal vbles ***************

C unit capsule vector
      DOUBLE PRECISION		CAPVEC(3), CAPLEN
C critical angle, the real angle subtended to capvec
      DOUBLE PRECISION		ALPHAC, ALPHA

C pi
      DOUBLE PRECISION		PI

C end of decs ******************

C value for pi?
      IF (INT(PI).NE.3) PI = 2.*ACOS(0.)

C vector along capsule centre line
      CAPVEC(1) = CCENT1(1) - CCENT2(1)
      CAPVEC(2) = CCENT1(2) - CCENT2(2)
      CAPVEC(3) = CCENT1(3) - CCENT2(3)
C length of capsule half-vector
      CAPLEN = SQRT( CAPVEC(1)**2 + CAPVEC(2)**2 +
     &                   CAPVEC(3)**2)
C make capvec a unit
      CAPVEC(1) = CAPVEC(1)/CAPLEN
      CAPVEC(2) = CAPVEC(2)/CAPLEN
      CAPVEC(3) = CAPVEC(3)/CAPLEN
C want half vector length in rest though
      CAPLEN = 0.5*CAPLEN

C do we a sphere (special case of capsule?)
      IF (CAPLEN.LT.1E-09) THEN
        EFFRAD = CAPR
        GOTO 55555
      ENDIF

C the angle subtended by the capsule radius at the end of
C capsule
      ALPHAC = ATAN2(CAPR,CAPLEN)

C work out the angle between direct and the capsule vector
      ALPHA = ACOS( CAPVEC(1)*DIREC(1) +
     &              CAPVEC(2)*DIREC(2) +
     &              CAPVEC(3)*DIREC(3) )

C is the direction inside the "cylinder" bit of the capsule?
      IF ( (ALPHA.GT.ALPHAC) .AND. (ALPHA.LT.(PI-ALPHAC)) ) THEN
C yes - effective radius easy
        EFFRAD = CAPR/SIN(ALPHA)
      ELSE
C for angles greater than 90 then use the equivalent mirror angle
        IF (ALPHA.GT.0.5*PI) ALPHA = PI - ALPHA
C not in cylinder in one of the end caps
C got triangle with dimensions
C length of sides EFFR, CAPLEN and CAPR. The angle opposite CAPR in alpha.
C Applying cosine rule
C CAPR**2 = EFFRAD**2 - 2*CAPLEN*EFFRAD + CAPLEN**2
C only +ve solution must be real
        EFFRAD = CAPLEN*COS(ALPHA) +
     &           SQRT( (CAPLEN*COS(ALPHA))**2-(CAPLEN**2-CAPR**2))
      ENDIF

55555 RETURN
      END


      subroutine zsphbesjh(lmax,x,bj,hm,eiz,job)
c_______________________________________________________________________
c calculate spherical bessel and hankel functions
c input: lmax integer scalar, max l
c        x    complex*16 scalar, argument of the bessel functions
c        job  integer scalar. job=+1: retarded hankel function is calculated
c             job=-1: advanced hankel function is calculated
c returns: bj complex*16 array of (0:lmax), j_l
c          hm complex*16 array of (0:lmax), h(+)_l (retarded hankel function)
c             this is scaled out of a factor of exp(i*x)/x
c          eiz complex*16 scalar, factor exp(i*x)
c calls zsphbesj
c  computes spherical bessel/hankel functions j and h+
c    h+ = jl + i nl = sinx/x - i cosx/x
c    h- = jl - i nl = sinx/x + i cosx/x
c
c   xgz ornl 1994
      implicit real*8 (a-h,o-z)
      complex*16 ci
      parameter(ci=(0.0d0,1.0d0))
      complex*16 bj(0:lmax),hm(0:lmax)
      complex*16 x,x1,eiz,fll,ac,as
c
      if(x.eq.0.0d0) goto 200

c use hm for scratch in zsphbesj
c zsphbesj returns x1=1/x, as=sin(x),ac=cos(x)
      call zsphbesj(lmax,x,as,ac,x1,bj,hm)
c pull out factor of e^ix/x from h
c eiz = job*exp(job*ci*x)
      eiz=job*exp(job*ci*x)

c now use hm for hankel hm = jl + i nl
      hm(0)=-ci
c hm(1) = -1.d0 - ci*x1
      hm(1)=-dcmplx(1.0d0-dimag(x1),dreal(x1))

      do l=2,lmax
        hm(l)=(2*l-1)*hm(l-1)*x1-hm(l-2)
      enddo
      return

200   write(6,*)'error bessel: zero argument.'
      stop
      end


Real Function pplpk(srt)
  Save
  pmass = 0.9383
  pplpk = 0.
  scheck = ((srt**2-2.*pmass**2)/(2.*pmass))**2 - pmass**2
  If (scheck<0) Then
     Write (99, *) 'scheck35: ', scheck
     scheck = 0.
  End If
  plab = sqrt(scheck)
  pmin = 2.82
  pmax = 25.0
  If (plab>pmax) Then
     pplpk = 0.036
     Return
  End If
  If (plab<pmin) Then
     pplpk = 0.
     Return
  End If
  a = 0.0654
  b = -3.16
  c = -0.0029
  an = -4.14
  pplpk = a + b*(plab**an) + c*(alog(plab))**2
  If (pplpk<=0) pplpk = 0
  Return
End Function pplpk


!*******************************************************************
!                                                                  *
      SUBROUTINE C1E1SM(Q,QM,SM,C,CM,A)
!                                                                  *
!     THIS PACKAGE DETERMINES THE VALUES OF FOLLOWING              *
!                                                 ---         ---  *
!                                                 I  Q   1  C   I  *
!     CLEBSCH - GORDAN COEFFICIENT:               I             I  *
!                                                 I  QM  1  CM  I  *
!                                                 ---         ---  *
!                                                                  *
!   Written by G. Gaigalas,                                        *
!   Vilnius,  Lithuania                             December 1993  *
!   Transform to fortran 90/95 by G. Gaigalas       December 2012  *
!   The last modification made by G. Gaigalas       October  2017  *
!                                                                  *
!*******************************************************************
!
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY: DOUBLE
      USE CONS_C,          ONLY: ZERO, TENTH, ONE, TWO, THREE, EPS
!-----------------------------------------------
!   I n t e r f a c e   B l o c k s
!-----------------------------------------------
      USE ittk_I
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      REAL(DOUBLE), INTENT(IN)  :: Q, QM, SM, C, CM
      REAL(DOUBLE), INTENT(OUT) :: A
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER                    :: IE, IIQ, IIC
      REAL(DOUBLE), DIMENSION(2) :: GC
!-----------------------------------------------
      GC(1)=ONE
      GC(2)=-ONE
      A=ZERO
      IIQ=TWO*Q+TENTH
      IIC=TWO*C+TENTH
      IF(ITTK(IIQ,IIC,2).EQ.0)RETURN
      IF(DABS(QM+SM-CM).GT.EPS)RETURN
      IF((Q+TENTH).LT.DABS(QM))RETURN
      IF((C+TENTH).LT.DABS(CM))RETURN
      IE=0
      IF(DABS(SM-ONE).LT.EPS)IE=1
      IF(DABS(SM+ONE).LT.EPS)IE=2
      IF(IE.EQ.0)RETURN
      IF(DABS(Q+ONE-C).LT.EPS) THEN
        A=DSQRT((C+GC(IE)*CM-ONE)*(C+GC(IE)*CM)/ &
        ((TWO*C-ONE)*TWO*C))
      ELSE IF(DABS(Q-C).LT.EPS) THEN
        A=-GC(IE)*DSQRT((C+GC(IE)*CM)*(C-GC(IE)*CM+ONE)/ &
        ((C+ONE)*TWO*C))
      ELSE IF(DABS(Q-ONE-C).GT.EPS) THEN
        RETURN
      ELSE
        A=DSQRT((C-GC(IE)*CM+ONE)*(C-GC(IE)*CM+TWO)/ &
        ((TWO*C+TWO)*(TWO*C+THREE)))
      END IF
      RETURN
      END SUBROUTINE C1E1SM


program test
    use linalg

    real, dimension (:,:), allocatable :: a,b,c
    integer :: n
    real :: exec_time
    n = 10000

    call  init_system_clock()

    call init_matrix(a, b, c, n, exec_time)

    print *, exec_time

    call matmul_bench(a, b, c, n, exec_time)

    print *, exec_time

    call matmul_benchmark(n, exec_time)

    print *, exec_time

end program test


 real function avh_brig(r,l,ier)
!BUT/PURPOSE
! convert an avhrr ir channel l radiance into brightness temperature
! using band correction coefficients and central wave numbers
! already read from avhrr calibration coefficient file
! and stored into cavh_cpi.h
!
! (another way to develop later is to use 1B avhrr header variables
! from  avh_h_radtempcnv array)
!
!DESCRIPTION
!     FORTRAN 77 with UKMO/METEO-FRANCE accepted extensions
!
!PROPRIETAIRE/OWNER
!  Marcel Derrien
!
!HISTORIQUE/HISTORY (MODIFICATIONS/CHANGES)
!(version   date      comment                 <author>)
!  1.0      Nov. 95   initial                 Marcel Derrien
!
!ARGUMENTS
!     input real r
!     input integer l ir channel order
!                  1 for channle 3B
!                  2 for channel 4
!                  3 for channel 5
!     output real brightness temperature
!     output ier exit status 0 =ok
!FUNCTIONS AND SUBROUTINES CALLS  (RETURN CODES FOR FUNCTIONS)
!
!
!
!COPYRIGHT (c) Meteo-France (EVOLUNIX - ATOVS - EUMETSAT project <1994>)
!
!
!DECLARATIONS
!
      implicit none
!
! identification variable for "what"
      character*62 sccsid
!
      data sccsid/'@(#) avh_brig.F version 1.1 on 3/24/98'/
!
!arguments
      real r
      integer l
      integer ier
!commons
      include 'cavh_cpi.inc'
!local variables
      real alge,expn,tt
      integer k
!functions
      intrinsic alog
!-
      ier = 1
      k = l
      avh_brig = 0.0

      if (k.ge.1 .or. k.le.3) then
        if(r.lt.0.) r = 1.e-12
        expn = planck1(k)/r + 1.
        if (expn.ge.1.) then
          alge = alog(expn)
          if (alge.ge.1.) then
            tt = planck2(k)/alge
!
! Xu Li: 11/03/2005: band correction Tb = (T* - A) / B
! T* is the equavilent black body temperature (brightness temperature) calculated by plank function
! Tb is the BT after band correction
! How to apply the band corretion depends on the definition of A & B!
! After comparing A & B read from GAC 1b data set with those on NOAA web site, Dr. Chao pointed out
! that the band correction should be done (with the read A & B) as follows:
!           Tb = A + B x T*
!
!           avh_brig = (tt-avh_bandcor(2 ,k))/avh_bandcor(3,k)

            avh_brig = avh_bandcor(2 ,k) + avh_bandcor(3,k)*tt

            ier = 0
          endif
        endif
      endif
 end function avh_brig

 real function avh_cir_r(r,c)
!BUT/PURPOSE
! returns the  Kelvin corresponding to the radiance r from avhrr
! channel c
!
!DESCRIPTION
!     FORTRAN 77 with UKMO/METEO-FRANCE accepted extensions
!
!PROPRIETAIRE/OWNER
!  Marcel Derrien
!
!HISTORIQUE/HISTORY (MODIFICATIONS/CHANGES)
!(version   date      comment                 <author>)
!  1.0      Nov. 95   initial                 Marcel Derrien
!
!ARGUMENTS
!     input real r
!     input integer c ir channel order
!                  1 for channle 3B
!                  2 for channel 4
!                  3 for channel 5
!     output ier exit status 0 =ok
!
!
!FUNCTIONS AND SUBROUTINES CALLS  (RETURN CODES FOR FUNCTIONS)
!
!COPYRIGHT (c) Meteo-France (EVOLUNIX - ATOVS - EUMETSAT project <1994>)
!
!DECLARATIONS
!
        implicit none
!
! identification variable for "what"
        character*62 sccsid
!
        data sccsid/'@(#) avh_cir_r.F version 1.1 on 3/24/98'/
! arguments
	real r
 	integer c
!commons
         real rbeg(3),rstep(3),rend(3)
         real tconv(4000,3)
         common/temp_conv/rbeg,rstep,rend,tconv
! local variables
         integer k
!functions
!-
         k = int( ( r - rbeg(c) ) / rstep(c) ) + 1
!-saturations ?
         if(k.lt.1) then
            avh_cir_r = tconv(1,c)
         else if(k.ge.4000) then
            avh_cir_r = tconv(4000,c)
         else
!
! interpolation between k and k+1
!
            avh_cir_r = tconv(k,c) + ( tconv(k+1,c) -tconv(k,c) ) * &
                 ( r - ( rbeg(c) + (k-1)*rstep(c)) ) / rstep(c)
         endif

 end function avh_cir_r


      SUBROUTINE RKDUMB(VSTART,NVAR,X1,X2,NSTEP,DERIVS)
      PARAMETER (NMAX=10)
      COMMON /PATH/ XX(200),Y(10,200)
      DIMENSION VSTART(NVAR),V(NMAX),DV(NMAX)
      DO 11 I=1,NVAR
        V(I)=VSTART(I)
        Y(I,1)=V(I)
11    CONTINUE
      XX(1)=X1
      X=X1
      H=(X2-X1)/NSTEP
      DO 13 K=1,NSTEP
        CALL DERIVS(X,V,DV)
        CALL RK4(V,DV,NVAR,X,H,V,DERIVS)
        IF(X+H.EQ.X)PAUSE 'Stepsize not significant in RKDUMB.'
        X=X+H
        XX(K+1)=X
        DO 12 I=1,NVAR
          Y(I,K+1)=V(I)
12      CONTINUE
13    CONTINUE
      RETURN
      END