!DEC$ FREEFORM

!-----------------------------------------------------------------------------------
!-----------------------------------------------------------------------------------
! MATH module provides utility routines for computing
! - inverse matrices (e.g. for inverse Jacobian)
! - determinants
! - ...
!
! presumably it is usefull to use existing libraries like Intel MKL or BLAS/LAPACK
!
! Geralf Hütter,
! Rostyslav Skrypnyk, 28.02.2014
! Stephan Roth, 18.10.2017
!-----------------------------------------------------------------------------------
!-----------------------------------------------------------------------------------

MODULE MATH
     USE ABQINTERFACE
     IMPLICIT NONE
     REAL(KIND=AbqRK), PARAMETER :: EPSDET = 1E-10,EPSDET2 = EPSDET**2,EPSDET3 = EPSDET**3
     
     INTERFACE Inverse
       MODULE PROCEDURE InverseSub
       MODULE PROCEDURE InverseFct
     END INTERFACE
     CONTAINS
!-------------------------------------------------
!  M22INV  -  Compute the inverse of a 2x2 matrix.
!  A       - input 2x2 matrix to be inverted.
!  AINV    - output 2x2 inverse of matrix A.
!  DET     - output determinant of matrix A.
!-------------------------------------------------
          PURE SUBROUTINE M22INV (A, AINV, DET, InvExists)
               IMPLICIT NONE
               REAL(KIND=AbqRK), DIMENSION(2,2), INTENT(IN)  :: A
               REAL(KIND=AbqRK), DIMENSION(2,2), INTENT(OUT) :: AINV
               REAL(KIND=AbqRK), INTENT(OUT), OPTIONAL :: DET
               LOGICAL, INTENT(OUT) :: InvExists
               REAL(KIND=AbqRK), DIMENSION(2,2) :: COFACTOR
               REAL(KIND=AbqRK):: REFVAL
               DET =   A(1,1)*A(2,2) - A(1,2)*A(2,1)
               REFVAL= MAXVAL(ABS(A))
               !
               IF (ABS(DET) .LE. EPSDET2*REFVAL**2) THEN
                 AINV = 0.0_AbqRK
                 !WRITE(7,*) "Determinant doesn't exist. Inverse matrix cannot be computed."
                 InvExists=.FALSE.
                 RETURN
               ELSE
                 InvExists=.TRUE.
               END IF
               !
               COFACTOR(1,1) = +A(2,2)
               COFACTOR(1,2) = -A(2,1)
               COFACTOR(2,1) = -A(1,2)
               COFACTOR(2,2) = +A(1,1)
               !
               AINV = TRANSPOSE(COFACTOR) / DET
          END SUBROUTINE M22INV
!-------------------------------------------------
!  M33INV  -  Compute the inverse of a 3x3 matrix.
!  A       - input 3x3 matrix to be inverted.
!  AINV    - output 3x3 inverse of matrix A.
!  DET     - output determinant of matrix A.
!-------------------------------------------------
          PURE SUBROUTINE M33INV (A, AINV, DET, InvExists)
               IMPLICIT NONE
               REAL(KIND=AbqRK), DIMENSION(3,3), INTENT(IN)  :: A
               REAL(KIND=AbqRK), DIMENSION(3,3), INTENT(OUT) :: AINV
               REAL(KIND=AbqRK), INTENT(OUT), OPTIONAL :: DET
               LOGICAL, INTENT(OUT) :: InvExists
               REAL(KIND=AbqRK), DIMENSION(3,3) :: COFACTOR
               REAL(KIND=AbqRK):: REFVAL
               DET = A(1,1)*(A(2,2)*A(3,3) - A(2,3)*A(3,2)) - A(1,2)*(A(2,1)*A(3,3)-A(2,3)*A(3,1)) + A(1,3)*(A(2,1)*A(3,2)- A(3,1)*A(2,2))
               REFVAL= MAXVAL(ABS(A))
               !
               IF (ABS(DET) .LE. EPSDET3*REFVAL**3) THEN
                 AINV = 0.0_AbqRK
                 !WRITE(7,*) "Determinant doesn't exist. Inverse matrix cannot be computed."
                 InvExists=.FALSE.
                 RETURN
               ELSE
                 InvExists=.TRUE.
               END IF
               !
               COFACTOR(1,1) = +(A(2,2)*A(3,3)-A(2,3)*A(3,2))
               COFACTOR(1,2) = -(A(2,1)*A(3,3)-A(2,3)*A(3,1))
               COFACTOR(1,3) = +(A(2,1)*A(3,2)-A(2,2)*A(3,1))
               COFACTOR(2,1) = -(A(1,2)*A(3,3)-A(1,3)*A(3,2))
               COFACTOR(2,2) = +(A(1,1)*A(3,3)-A(1,3)*A(3,1))
               COFACTOR(2,3) = -(A(1,1)*A(3,2)-A(1,2)*A(3,1))
               COFACTOR(3,1) = +(A(1,2)*A(2,3)-A(1,3)*A(2,2))
               COFACTOR(3,2) = -(A(1,1)*A(2,3)-A(1,3)*A(2,1))
               COFACTOR(3,3) = +(A(1,1)*A(2,2)-A(1,2)*A(2,1))
               !
               AINV = TRANSPOSE(COFACTOR) / DET
          END SUBROUTINE M33INV
!-----------------------------------------------------------
! Inverse - Select the dimension and compute inverse matrix
! A       - input matrix (1x1, 2x2 or 3x3)
! B       - output matrix
!-----------------------------------------------------------
          PURE SUBROUTINE InverseSub (A, B, Det, InvExists)
               IMPLICIT NONE
               REAL(KIND=AbqRK), DIMENSION(:,:), INTENT(IN)  :: A
               REAL(KIND=AbqRK), DIMENSION(:,:), INTENT(OUT) :: B
               REAL(KIND=AbqRK), INTENT(OUT), OPTIONAL :: Det
               LOGICAL, INTENT(OUT), OPTIONAL :: InvExists
               REAL(KIND=AbqRK) :: Det_temp
               LOGICAL :: InvExists_temp
               IF ( SIZE(A,1) /= SIZE(A,2) .OR. SIZE(B,1) /= SIZE(B,2) .OR. SIZE(A,1) /= SIZE(B,1) ) THEN
                 !WRITE(7,*) "Only square matrices can be inverted. Check arguments in 'Inverse' subroutine. "
                 InvExists=.FALSE.
                 RETURN
               ENDIF
               SELECT CASE (SIZE(A,1))
                 CASE(1)
				   InvExists_temp=( A(1,1) .NE. 0.0_AbqRK )
                   IF (InvExists_temp) THEN
                     B(1,1)=1.0_AbqRK/A(1,1)
                   ELSE
                     B(1,1)=0.0_AbqRK
                   END IF
                   Det_temp=A(1,1)
                 CASE(2)
                   CALL M22INV(A, B, Det_temp, InvExists_temp)
                 CASE(3)
                   CALL M33INV(A, B, Det_temp, InvExists_temp)
               END SELECT
               IF (PRESENT(Det)) Det=Det_temp
               IF (PRESENT(InvExists)) InvExists=InvExists_temp
          END SUBROUTINE
          ! And as function
          PURE FUNCTION InverseFct (A)    
               IMPLICIT NONE
               REAL(KIND=AbqRK), DIMENSION(:,:), INTENT(IN)  :: A
               REAL(KIND=AbqRK), DIMENSION(SIZE(A,1),SIZE(A,2)) :: InverseFct
               CALL InverseSub(A,InverseFct)
          END FUNCTION
!---------------------------------------------------------------------------------------
! Jacobi         - computes Jacobi matrix
! ShapeFuncDeriv - input array
! Coords         - coordinates of nodes of finite element (e.g. array COORDS in Abaqus)
!---------------------------------------------------------------------------------------
          PURE FUNCTION Jacobi (ShapeFuncDeriv, Coords)
               IMPLICIT NONE
               REAL(KIND=AbqRK), INTENT (IN), DIMENSION (:,:) :: ShapeFuncDeriv, Coords
               REAL(KIND=AbqRK), DIMENSION (SIZE(ShapeFuncDeriv,1), SIZE(ShapeFuncDeriv,1)) :: Jacobi
               Jacobi = MATMUL(ShapeFuncDeriv, TRANSPOSE(Coords))
          END FUNCTION Jacobi
!-------------------------------------------------
! FindDet - computes determinant of square matrix
! A       - input matrix
!-------------------------------------------------
          PURE FUNCTION FindDet(A)
               IMPLICIT NONE
               REAL(KIND=AbqRK), DIMENSION(:,:), INTENT(IN) :: A
               REAL(KIND=AbqRK) :: FindDet
               SELECT CASE (SIZE(A,1))
                 CASE(1)
                   FindDet = A(1,1)
                 CASE(2)
                   FindDet = A(1,1)*A(2,2) - A(1,2)*A(2,1)
                 CASE(3)
                   FindDet = A(1,1)*(A(2,2)*A(3,3) - A(2,3)*A(3,2)) - A(1,2)*(A(2,1)*A(3,3)-A(2,3)*A(3,1)) + A(1,3)*(A(2,1)*A(3,2)- A(3,1)*A(2,2))
               END SELECT
          END FUNCTION
END MODULE MATH
