program test4
#if (_DP==0)
 use iso_fortran_env,only:int32,int64,wp=>real32
#else
 use iso_fortran_env,only:int32,int64,wp=>real64
#endif
 use modsparse
 implicit none
 integer(kind=int32)::nrow
 integer(kind=int32)::row
 integer(kind=int32)::col
 integer(kind=int32)::iunit, istat
 real(kind=wp)::val
 logical::lup=.false.
 type(coosparse)::coo,subcoo

 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 !COO  FULL
 open(newunit=iunit,file='crsinput.ascii',status='old',action='read')
 read(iunit,*) nrow

 coo=coosparse(nrow,nel=4_int64,lupper=.false.)

 do
  read(iunit,*,iostat=istat) row,col,val
  if(istat.ne.0)exit
  call coo%add(row,col,val)
 end do
 close(iunit)

 print*,'FULL => FULL xxxxxxxxxxxxxxxxxxxxxxxxxx'
 call coo%printsquare()
 print*,'SELECT UPPER BLOCK xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 1 2 3 4'
 subcoo=coo%submatrix(1,2,3,4)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,'SELECT LOWER BLOCK xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 3 4 1 2'
 subcoo=coo%submatrix(3,4,1,2)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,'SELECT BLOCK + DIAG xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 1 2 1 3'
 subcoo=coo%submatrix(1,2,1,3)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,''
 print*,'FULL => UPPER xxxxxxxxxxxxxxxxxxxxxxxxxx'
 call coo%printsquare()
 print*,'SELECT UPPER BLOCK xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 1 2 3 4'
 subcoo=coo%submatrix(1,2,3,4,.true.)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,'SELECT LOWER BLOCK a xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 3 4 1 2'
 subcoo=coo%submatrix(3,4,1,2,.true.)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,'SELECT BLOCK + DIAG xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 1 2 1 3'
 subcoo=coo%submatrix(1,2,1,3,.true.)
 call subcoo%printstats()
 call subcoo%printsquare()

 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 !COO UPPER
 open(newunit=iunit,file='crsinput.ascii',status='old',action='read')
 read(iunit,*) nrow

 coo=coosparse(nrow,nel=4_int64,lupper=.true.)

 do
  read(iunit,*,iostat=istat) row,col,val
  if(istat.ne.0)exit
  call coo%add(row,col,val)
 end do
 close(iunit)

 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 !COO UPPER
 print*,'***COO UPPER'
 call coo%printstats()
 call coo%print(lint=.true.)

 print*,'UPPER => FULL xxxxxxxxxxxxxxxxxxxxxxxxxx'
 call coo%printsquare()
 print*,'SELECT UPPER BLOCK xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 1 2 3 4'
 subcoo=coo%submatrix(1,2,3,4)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,'SELECT LOWER BLOCK xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 3 4 1 2'
 subcoo=coo%submatrix(3,4,1,2)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,'SELECT BLOCK + DIAG xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 1 2 1 3'
 subcoo=coo%submatrix(1,2,1,3)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,''
 print*,'UPPER => UPPER xxxxxxxxxxxxxxxxxxxxxxxxxx'
 call coo%printsquare()
 print*,'SELECT UPPER BLOCK xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 1 2 3 4'
 subcoo=coo%submatrix(1,2,3,4,.true.)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,'SELECT LOWER BLOCK xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 3 4 1 2'
 subcoo=coo%submatrix(3,4,1,2,.true.)
 call subcoo%printstats()
 call subcoo%printsquare()

 print*,'SELECT BLOCK + DIAG xxxxxxxxxxxxxxxxxxxxxxxxxx'
 print*,'aaaa 1 2 1 3'
 subcoo=coo%submatrix(1,2,1,3,.true.)
 call subcoo%printstats()
 call subcoo%printsquare()

end program


    Program zpoequ_example

!     ZPOEQU Example Program Text

!     Copyright (c) 2018, Numerical Algorithms Group (NAG Ltd.)
!     For licence see
!       https://github.com/numericalalgorithmsgroup/LAPACK_Examples/blob/master/LICENCE.md

!     .. Use Statements ..
      Use blas_interfaces, Only: zdscal
      Use lapack_example_aux, Only: nagf_blas_zddscl, &
        nagf_file_print_matrix_complex_gen_comp
      Use lapack_interfaces, Only: zpoequ
      Use lapack_precision, Only: dp
!     .. Implicit None Statement ..
      Implicit None
!     .. Parameters ..
      Real (Kind=dp), Parameter :: one = 1.0_dp
      Real (Kind=dp), Parameter :: thresh = 0.1_dp
      Integer, Parameter :: nin = 5, nout = 6
!     .. Local Scalars ..
      Real (Kind=dp) :: amax, big, scond, small
      Integer :: i, ifail, info, j, lda, n
!     .. Local Arrays ..
      Complex (Kind=dp), Allocatable :: a(:, :)
      Real (Kind=dp), Allocatable :: s(:)
      Character (1) :: clabs(1), rlabs(1)
!     .. Intrinsic Procedures ..
      Intrinsic :: epsilon, radix, real, tiny
!     .. Executable Statements ..
      Write (nout, *) 'ZPOEQU Example Program Results'
      Write (nout, *)
      Flush (nout)
!     Skip heading in data file
      Read (nin, *)
      Read (nin, *) n
      lda = n
      Allocate (a(lda,n), s(n))

!     Read the upper triangular part of the matrix A from data file

      Read (nin, *)(a(i,i:n), i=1, n)

!     Print the matrix A

!     ifail: behaviour on error exit
!             =0 for hard exit, =1 for quiet-soft, =-1 for noisy-soft
      ifail = 0
      Call nagf_file_print_matrix_complex_gen_comp('Upper', 'Non-unit', n, n, &
        a, lda, 'Bracketed', '1P,E10.2', 'Matrix A', 'Integer', rlabs, &
        'Integer', clabs, 80, 0, ifail)

      Write (nout, *)

!     Compute diagonal scaling factors

      Call zpoequ(n, a, lda, s, scond, amax, info)

      If (info>0) Then
        Write (nout, 100) 'Diagonal element', info, ' of A is non positive'
      Else

!       Print SCOND, AMAX and the scale factors

        Write (nout, 110) 'SCOND =', scond, ', AMAX =', amax
        Write (nout, *)
        Write (nout, *) 'Diagonal scaling factors'
        Write (nout, 120) s(1:n)
        Write (nout, *)
        Flush (nout)

!       Compute values close to underflow and overflow

        small = tiny(1.0E0_dp)/(epsilon(1.0E0_dp)*real(radix(1.0E0_dp),kind=dp &
          ))
        big = one/small
        If ((scond<thresh) .Or. (amax<small) .Or. (amax>big)) Then

!         Scale A
          Do j = 1, n
            Call zdscal(j, s(j), a(1,j), 1)
            Call nagf_blas_zddscl(j, s, 1, a(1,j), 1)
          End Do

!         Print the scaled matrix

          ifail = 0
          Call nagf_file_print_matrix_complex_gen_comp('Upper', 'Non-unit', n, &
            n, a, lda, 'Bracketed', 'F8.4', 'Scaled matrix', 'Integer', rlabs, &
            'Integer', clabs, 80, 0, ifail)

        End If
      End If

100   Format (1X, A, I4, A)
110   Format (1X, 2(A,1P,E8.1))
120   Format ((1X,1P,7E11.1))
    End Program


! 7.32g в учебнике
! Упорядочить элементы в СТРОКАХ матрицы по модулю по возрастанию

program ex_7_32g
    implicit none

    integer, allocatable    :: B(:,:), AbsLine(:), Order(:)
    integer                 :: In, Out, x, y, i, j

    character(*), parameter :: output_file = "output.txt", &
                               input_file = "../data/input.txt"

    open (file=input_file, newunit=In)
        read(In, *) x, y
        allocate(B(x,y), Order(x))
        read(In, *) (B(:,i), i = 1, y)
    close (In)

    do concurrent (j = 1:y)
        AbsLine = abs(B(:,j))
        Order = [(i,i = 1, x)]
        do i = 1, x
            Order(i:) = cshift(Order(i:), minloc(AbsLine(i:), 1) - 1)
            AbsLine = cshift(AbsLine, minloc(AbsLine(i:), 1) - 1)
        enddo
        B(:,j) = B(order,j)
    end do

    open (file=output_file, newunit=Out)
        write(Out, *) (B(:,j), j = 1, y)
    close (Out)

end program ex_7_32g


! { dg-do compile }
! { dg-options "-floop-nest-optimize -O2 -ffast-math" }
! PR53852 : compile time / memory hog
SUBROUTINE  build_d_tensor_gks(d5f,v,d5)
    INTEGER, PARAMETER :: dp=8
    REAL(KIND=dp),  DIMENSION(3, 3, 3, 3, 3), &
      INTENT(OUT) :: d5f
    REAL(KIND=dp), DIMENSION(3), INTENT(IN)  :: v
    REAL(KIND=dp), INTENT(IN) :: d5
    INTEGER       :: k1, k2, k3, k4, k5
    REAL(KIND=dp) :: w

    d5f = 0.0_dp
    DO k1=1,3
       DO k2=1,3
          DO k3=1,3
             DO k4=1,3
                DO k5=1,3
                   d5f(k5,k4,k3,k2,k1)=d5f(k5,k4,k3,k2,k1)+ &
                          v(k1)*v(k2)*v(k3)*v(k4)*v(k5)*d5
                ENDDO
                w=v(k1)*v(k2)*v(k3)*d4
                d5f(k1,k2,k3,k4,k4)=d5f(k1,k2,k3,k4,k4)+w
                d5f(k1,k2,k4,k3,k4)=d5f(k1,k2,k4,k3,k4)+w
                d5f(k1,k4,k2,k3,k4)=d5f(k1,k4,k2,k3,k4)+w
                d5f(k4,k1,k2,k3,k4)=d5f(k4,k1,k2,k3,k4)+w
                d5f(k1,k2,k4,k4,k3)=d5f(k1,k2,k4,k4,k3)+w
                d5f(k1,k4,k2,k4,k3)=d5f(k1,k4,k2,k4,k3)+w
                d5f(k4,k1,k2,k4,k3)=d5f(k4,k1,k2,k4,k3)+w
                d5f(k1,k4,k4,k2,k3)=d5f(k1,k4,k4,k2,k3)+w
                d5f(k4,k1,k4,k2,k3)=d5f(k4,k1,k4,k2,k3)+w
                d5f(k4,k4,k1,k2,k3)=d5f(k4,k4,k1,k2,k3)+w
             ENDDO
          ENDDO
       ENDDO
    ENDDO
END SUBROUTINE build_d_tensor_gks


module euler_multiprecision_util_m

    implicit none
    private
    public :: add, sub, compare, mul, pow2

contains

    !> The core function to cut leading zeros of a contiguous array.
    !>
    !> As the name of the function suggests, the function
    !> deletes all the leading zeros of a contiguous array.
    !> This procedure uses the intrinsic function `findloc`
    !> that is only available with gfortran > 9.0.0
    pure function cut_leading_zeros(arr) result(ret)
        integer, contiguous, intent(in) :: arr(:)
        integer, allocatable :: ret(:)

        ret = arr(findloc(arr == 0, value=.false., dim=1):)
    end function cut_leading_zeros

    !> The core function to carry digits during add and multiplication.
    !>
    !> Each element of the input array is an 32bit integer greater than or
    !> equal to zero and less than or equal to nine.
    pure function carry(arr) result(ret)
        integer, contiguous, intent(in) :: arr(:)
        integer, dimension(size(arr) + 2) :: tmp1, tmp2, ret

        tmp1 = 0; tmp2 = 0; ret(1:2) = 0; ret(3:) = arr(:)
        do
            if (all(ret <= 9)) exit
            tmp1(:) = ret(:) - ret(:)/10*10
            tmp2(1:size(tmp2) - 1) = ret(2:)/10
            ret(:) = tmp1(:) + tmp2(:)
        end do
    end function carry

    !> The core function to provide the ability of addition.
    !>
    !> Each element of the input array is an 32bit integer greater than or
    !> equal to zero and less than or equal to nine. When two such arrays
    !> are "added", first of all, the shorter array will be extended to the
    !> length of the longer array and two arrays with the same dimension
    !> are added. Then the function `carry`
    !> "[carries](https://en.wikipedia.org/wiki/Carry_(arithmetic))" the
    !> added array iteratively until all elements of the array are greater
    !> than or equal to zero and less than or equal to nine. Finally, the
    !> leading zeros are deleted by `cut_leading_zeros`.
    pure function add(arr1, arr2) result(ret)
        integer, contiguous, intent(in) :: arr1(:), arr2(:)
        integer, allocatable, dimension(:) :: ret, tmp1, tmp2

        associate (x => max(size(arr1), size(arr2)) + 1)
            allocate (tmp1(x), tmp2(x))
            tmp1 = 0; tmp1(x - size(arr1) + 1:x) = arr1(:)
            tmp2 = 0; tmp2(x - size(arr2) + 1:x) = arr2(:)
        end associate
        ret = cut_leading_zeros(carry(tmp1 + tmp2))
    end function add

    !> The core function to provide the ability of subtraction.
    !>
    !> Each element of the input array is an 32bit integer greater than or
    !> equal to zero and less than or equal to nine.
    pure function sub(arr1, arr2) result(ret)
        integer, contiguous, intent(in) :: arr1(:), arr2(:)
        integer, allocatable, dimension(:) :: ret, tmp1, tmp2, tmp
        integer :: i

        associate (x => max(size(arr1), size(arr2)) + 1)
            allocate (tmp1(x), tmp2(x), tmp(x))
            tmp1 = 0; tmp2 = 0; tmp = 0
            tmp1(x - size(arr1) + 1:x) = arr1(:)
            tmp2(x - size(arr2) + 1:x) = arr2(:)
        end associate

        do i = size(tmp1), 2, -1
            if (tmp1(i) >= tmp2(i)) then
                tmp(i) = tmp1(i) - tmp2(i)
            else
                tmp(i) = tmp1(i) - tmp2(i) + 10
                tmp1(i - 1) = tmp1(i - 1) - 1
            end if
        end do
        ret = cut_leading_zeros(tmp)
    end function sub

    !> The core function to provide the ability of comparison.
    !>
    !>### Result
    !> | return value | result |
    !> |:------------:|:------:|
    !> |`ret = 1`  |arr1 > arr2|
    !> |`ret = 0`  |arr1 = arr2|
    !> |`ret = -1` |arr1 < arr2|
    pure function compare(arr1, arr2) result(ret)
        integer, contiguous, intent(in) :: arr1(:), arr2(:)
        integer :: ret, i

        if (size(arr1) > size(arr2)) then
            ret = 1; return
        else if (size(arr1) < size(arr2)) then
            ret = -1; return
        end if

        do i = size(arr1), 1, -1
            if (arr1(i) > arr2(i)) then
                ret = 1; return
            else if (arr1(i) < arr2(i)) then
                ret = -1; return
            else
                ret = 0
            end if
        end do
        if (ret /= 0) error stop 'compare: invalid output.'
    end function compare

    !> The core function to provide the ability of multiplication.
    !>
    !> Each element of the input array is an 32bit integer greater than or
    !> equal to zero and less than or equal to nine.
    pure function mul(arr1, arr2) result(ret)
        integer, contiguous, intent(in) :: arr1(:), arr2(:)
        integer, allocatable :: tmp(:), tmp_row(:), ret(:)
        integer :: i

        associate (s1 => size(arr1), s2 => size(arr2))
            allocate (tmp_row(s1 + s2), tmp(s1 + s2 + 2))
            tmp = 0

            do i = s2, 1, -1
                tmp_row = 0; tmp_row(i + 1:i + s1) = arr1
                tmp = tmp + carry(tmp_row*arr2(i))
            end do
        end associate
        ret = cut_leading_zeros(carry(tmp))
    end function mul

    !> The core function to provide the ability of power.
    !>
    !> Each element of the input array is an 32bit integer greater than or
    !> equal to zero and less than or equal to nine. The function uses
    !> a very simple implementation of exponentiation
    !> ([wiki links](https://en.wikipedia.org/wiki/
    !>Exponentiation_by_squaring)). The function currently doesn't
    !> support negative powers.
    pure recursive function pow2(arr, n) result(ret)
        integer, contiguous, intent(in) :: arr(:)
        integer, intent(in) :: n
        integer, allocatable :: ret(:)

        if (n < 0) error stop 'pow2: n is nonnegative.'

        if (n == 0) then
            ret = [1]
        else if (n == 1) then
            ret = arr
        else if (mod(n, 2) == 0) then
            ret = pow2(mul(arr, arr), n/2)
        else if (mod(n, 2) /= 0) then
            ret = pow2(mul(arr, arr), (n - 1)/2)
            ret = mul(arr, ret)
        end if
    end function pow2

end module euler_multiprecision_util_m


: MAIN
    0
    BEGIN
        1+
        DUP 100 <= WHILE
        DUP .
    REPEAT
    DROP
;

MAIN


      SUBROUTINE SB02MD( DICO, HINV, UPLO, SCAL, SORT, N, A, LDA, G,
     $                   LDG, Q, LDQ, RCOND, WR, WI, S, LDS, U, LDU,
     $                   IWORK, DWORK, LDWORK, BWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To solve for X either the continuous-time algebraic Riccati
C     equation
C                              -1
C        Q + A'*X + X*A - X*B*R  B'*X = 0                            (1)
C
C     or the discrete-time algebraic Riccati equation
C                                        -1
C        X = A'*X*A - A'*X*B*(R + B'*X*B)  B'*X*A + Q                (2)
C
C     where A, B, Q and R are N-by-N, N-by-M, N-by-N and M-by-M matrices
C     respectively, with Q symmetric and R symmetric nonsingular; X is
C     an N-by-N symmetric matrix.
C                       -1
C     The matrix G = B*R  B' must be provided on input, instead of B and
C     R, that is, for instance, the continuous-time equation
C
C        Q + A'*X + X*A - X*G*X = 0                                  (3)
C
C     is solved, where G is an N-by-N symmetric matrix. SLICOT Library
C     routine SB02MT should be used to compute G, given B and R. SB02MT
C     also enables to solve Riccati equations corresponding to optimal
C     problems with coupling terms.
C
C     The routine also returns the computed values of the closed-loop
C     spectrum of the optimal system, i.e., the stable eigenvalues
C     lambda(1),...,lambda(N) of the corresponding Hamiltonian or
C     symplectic matrix associated to the optimal problem.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     DICO    CHARACTER*1
C             Specifies the type of Riccati equation to be solved as
C             follows:
C             = 'C':  Equation (3), continuous-time case;
C             = 'D':  Equation (2), discrete-time case.
C
C     HINV    CHARACTER*1
C             If DICO = 'D', specifies which symplectic matrix is to be
C             constructed, as follows:
C             = 'D':  The matrix H in (5) (see METHOD) is constructed;
C             = 'I':  The inverse of the matrix H in (5) is constructed.
C             HINV is not used if DICO = 'C'.
C
C     UPLO    CHARACTER*1
C             Specifies which triangle of the matrices G and Q is
C             stored, as follows:
C             = 'U':  Upper triangle is stored;
C             = 'L':  Lower triangle is stored.
C
C     SCAL    CHARACTER*1
C             Specifies whether or not a scaling strategy should be
C             used, as follows:
C             = 'G':  General scaling should be used;
C             = 'N':  No scaling should be used.
C
C     SORT    CHARACTER*1
C             Specifies which eigenvalues should be obtained in the top
C             of the Schur form, as follows:
C             = 'S':  Stable   eigenvalues come first;
C             = 'U':  Unstable eigenvalues come first.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrices A, Q, G and X.  N >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the coefficient matrix A of the equation.
C             On exit, if DICO = 'D', and INFO = 0 or INFO > 1, the
C                                                                    -1
C             leading N-by-N part of this array contains the matrix A  .
C             Otherwise, the array A is unchanged on exit.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     G       (input) DOUBLE PRECISION array, dimension (LDG,N)
C             The leading N-by-N upper triangular part (if UPLO = 'U')
C             or lower triangular part (if UPLO = 'L') of this array
C             must contain the upper triangular part or lower triangular
C             part, respectively, of the symmetric matrix G.
C             The strictly lower triangular part (if UPLO = 'U') or
C             strictly upper triangular part (if UPLO = 'L') is not
C             referenced.
C
C     LDG     INTEGER
C             The leading dimension of array G.  LDG >= MAX(1,N).
C
C     Q       (input/output) DOUBLE PRECISION array, dimension (LDQ,N)
C             On entry, the leading N-by-N upper triangular part (if
C             UPLO = 'U') or lower triangular part (if UPLO = 'L') of
C             this array must contain the upper triangular part or lower
C             triangular part, respectively, of the symmetric matrix Q.
C             The strictly lower triangular part (if UPLO = 'U') or
C             strictly upper triangular part (if UPLO = 'L') is not
C             used.
C             On exit, if INFO = 0, the leading N-by-N part of this
C             array contains the solution matrix X of the problem.
C
C     LDQ     INTEGER
C             The leading dimension of array N.  LDQ >= MAX(1,N).
C
C     RCOND   (output) DOUBLE PRECISION
C             An estimate of the reciprocal of the condition number (in
C             the 1-norm) of the N-th order system of algebraic
C             equations from which the solution matrix X is obtained.
C
C     WR      (output) DOUBLE PRECISION array, dimension (2*N)
C     WI      (output) DOUBLE PRECISION array, dimension (2*N)
C             If INFO = 0 or INFO = 5, these arrays contain the real and
C             imaginary parts, respectively, of the eigenvalues of the
C             2N-by-2N matrix S, ordered as specified by SORT (except
C             for the case HINV = 'D', when the order is opposite to
C             that specified by SORT). The leading N elements of these
C             arrays contain the closed-loop spectrum of the system
C                           -1
C             matrix A - B*R  *B'*X, if DICO = 'C', or of the matrix
C                               -1
C             A - B*(R + B'*X*B)  B'*X*A, if DICO = 'D'. Specifically,
C                lambda(k) = WR(k) + j*WI(k), for k = 1,2,...,N.
C
C     S       (output) DOUBLE PRECISION array, dimension (LDS,2*N)
C             If INFO = 0 or INFO = 5, the leading 2N-by-2N part of this
C             array contains the ordered real Schur form S of the
C             Hamiltonian or symplectic matrix H. That is,
C
C                    (S   S  )
C                    ( 11  12)
C                S = (       ),
C                    (0   S  )
C                    (     22)
C
C             where S  , S   and S   are N-by-N matrices.
C                    11   12      22
C
C     LDS     INTEGER
C             The leading dimension of array S.  LDS >= MAX(1,2*N).
C
C     U       (output) DOUBLE PRECISION array, dimension (LDU,2*N)
C             If INFO = 0 or INFO = 5, the leading 2N-by-2N part of this
C             array contains the transformation matrix U which reduces
C             the Hamiltonian or symplectic matrix H to the ordered real
C             Schur form S. That is,
C
C                    (U   U  )
C                    ( 11  12)
C                U = (       ),
C                    (U   U  )
C                    ( 21  22)
C
C             where U  , U  , U   and U   are N-by-N matrices.
C                    11   12   21      22
C
C     LDU     INTEGER
C             The leading dimension of array U.  LDU >= MAX(1,2*N).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (2*N)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK and DWORK(2) returns the scaling factor used
C             (set to 1 if SCAL = 'N'), also set if INFO = 5;
C             if DICO = 'D', DWORK(3) returns the reciprocal condition
C             number of the given matrix  A.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= MAX(2,6*N) if DICO = 'C';
C             LDWORK >= MAX(3,6*N) if DICO = 'D'.
C             For optimum performance LDWORK should be larger.
C
C     BWORK   LOGICAL array, dimension (2*N)
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  if matrix A is (numerically) singular in discrete-
C                   time case;
C             = 2:  if the Hamiltonian or symplectic matrix H cannot be
C                   reduced to real Schur form;
C             = 3:  if the real Schur form of the Hamiltonian or
C                   symplectic matrix H cannot be appropriately ordered;
C             = 4:  if the Hamiltonian or symplectic matrix H has less
C                   than N stable eigenvalues;
C             = 5:  if the N-th order system of linear algebraic
C                   equations, from which the solution matrix X would
C                   be obtained, is singular to working precision.
C
C     METHOD
C
C     The method used is the Schur vector approach proposed by Laub.
C     It is assumed that [A,B] is a stabilizable pair (where for (3) B
C     is any matrix such that B*B' = G with rank(B) = rank(G)), and
C     [E,A] is a detectable pair, where E is any matrix such that
C     E*E' = Q with rank(E) = rank(Q). Under these assumptions, any of
C     the algebraic Riccati equations (1)-(3) is known to have a unique
C     non-negative definite solution. See [2].
C     Now consider the 2N-by-2N Hamiltonian or symplectic matrix
C
C                 ( A   -G )
C            H =  (        ),                                    (4)
C                 (-Q   -A'),
C
C     for continuous-time equation, and
C                    -1        -1
C                 ( A         A  *G   )
C            H =  (   -1          -1  ),                         (5)
C                 (Q*A    A' + Q*A  *G)
C                                                            -1
C     for discrete-time equation, respectively, where G = B*R  *B'.
C     The assumptions guarantee that H in (4) has no pure imaginary
C     eigenvalues, and H in (5) has no eigenvalues on the unit circle.
C     If Y is an N-by-N matrix then there exists an orthogonal matrix U
C     such that U'*Y*U is an upper quasi-triangular matrix. Moreover, U
C     can be chosen so that the 2-by-2 and 1-by-1 diagonal blocks
C     (corresponding to the complex conjugate eigenvalues and real
C     eigenvalues respectively) appear in any desired order. This is the
C     ordered real Schur form. Thus, we can find an orthogonal
C     similarity transformation U which puts (4) or (5) in ordered real
C     Schur form
C
C            U'*H*U = S = (S(1,1)  S(1,2))
C                         (  0     S(2,2))
C
C     where S(i,j) is an N-by-N matrix and the eigenvalues of S(1,1)
C     have negative real parts in case of (4), or moduli greater than
C     one in case of (5). If U is conformably partitioned into four
C     N-by-N blocks
C
C               U = (U(1,1)  U(1,2))
C                   (U(2,1)  U(2,2))
C
C     with respect to the assumptions we then have
C     (a) U(1,1) is invertible and X = U(2,1)*inv(U(1,1)) solves (1),
C         (2), or (3) with X = X' and non-negative definite;
C     (b) the eigenvalues of S(1,1) (if DICO = 'C') or S(2,2) (if
C         DICO = 'D') are equal to the eigenvalues of optimal system
C         (the 'closed-loop' spectrum).
C
C     [A,B] is stabilizable if there exists a matrix F such that (A-BF)
C     is stable. [E,A] is detectable if [A',E'] is stabilizable.
C
C     REFERENCES
C
C     [1] Laub, A.J.
C         A Schur Method for Solving Algebraic Riccati equations.
C         IEEE Trans. Auto. Contr., AC-24, pp. 913-921, 1979.
C
C     [2] Wonham, W.M.
C         On a matrix Riccati equation of stochastic control.
C         SIAM J. Contr., 6, pp. 681-697, 1968.
C
C     [3] Sima, V.
C         Algorithms for Linear-Quadratic Optimization.
C         Pure and Applied Mathematics: A Series of Monographs and
C         Textbooks, vol. 200, Marcel Dekker, Inc., New York, 1996.
C
C     NUMERICAL ASPECTS
C                               3
C     The algorithm requires 0(N ) operations.
C
C     FURTHER COMMENTS
C
C     To obtain a stabilizing solution of the algebraic Riccati
C     equation for DICO = 'D', set SORT = 'U', if HINV = 'D', or set
C     SORT = 'S', if HINV = 'I'.
C
C     The routine can also compute the anti-stabilizing solutions of
C     the algebraic Riccati equations, by specifying
C         SORT = 'U' if DICO = 'D' and HINV = 'I', or DICO = 'C', or
C         SORT = 'S' if DICO = 'D' and HINV = 'D'.
C
C     Usually, the combinations HINV = 'D' and SORT = 'U', or HINV = 'I'
C     and SORT = 'U', will be faster then the other combinations [3].
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Aug. 1997.
C     Supersedes Release 2.0 routine SB02AD by Control Systems Research
C     Group, Kingston Polytechnic, United Kingdom, March 1982.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Dec. 2002.
C
C     KEYWORDS
C
C     Algebraic Riccati equation, closed loop system, continuous-time
C     system, discrete-time system, optimal regulator, Schur form.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, HALF, ONE
      PARAMETER         ( ZERO = 0.0D0, HALF = 0.5D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         DICO, HINV, SCAL, SORT, UPLO
      INTEGER           INFO, LDA, LDG, LDQ, LDS, LDU, LDWORK, N
      DOUBLE PRECISION  RCOND
C     .. Array Arguments ..
      LOGICAL           BWORK(*)
      INTEGER           IWORK(*)
      DOUBLE PRECISION  A(LDA,*), DWORK(*), G(LDG,*), Q(LDQ,*),
     $                  S(LDS,*), U(LDU,*), WR(*), WI(*)
C     .. Local Scalars ..
      LOGICAL           DISCR, LHINV, LSCAL, LSORT, LUPLO
      INTEGER           I, IERR, ISCL, N2, NP1, NROT
      DOUBLE PRECISION  GNORM, QNORM, RCONDA, UNORM, WRKOPT
C     .. External Functions ..
      LOGICAL           LSAME, SB02MR, SB02MS, SB02MV, SB02MW
      DOUBLE PRECISION  DLAMCH, DLANGE, DLANSY
      EXTERNAL          DLAMCH, DLANGE, DLANSY, LSAME, SB02MR, SB02MS,
     $                  SB02MV, SB02MW
C     .. External Subroutines ..
      EXTERNAL          DAXPY, DCOPY, DGECON, DGEES, DGETRF, DGETRS,
     $                  DLACPY, DLASCL, DLASET, DSCAL, DSWAP, SB02MU,
     $                  XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX
C     .. Executable Statements ..
C
      INFO = 0
      N2  = N + N
      NP1 = N + 1
      DISCR = LSAME( DICO, 'D' )
      LSCAL = LSAME( SCAL, 'G' )
      LSORT = LSAME( SORT, 'S' )
      LUPLO = LSAME( UPLO, 'U' )
      IF ( DISCR ) LHINV = LSAME( HINV, 'D' )
C
C     Test the input scalar arguments.
C
      IF( .NOT.DISCR .AND. .NOT.LSAME( DICO, 'C' ) ) THEN
         INFO = -1
      ELSE IF( DISCR ) THEN
         IF( .NOT.LHINV .AND. .NOT.LSAME( HINV, 'I' ) )
     $      INFO = -2
      END IF
      IF( .NOT.LUPLO .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -3
      ELSE IF( .NOT.LSCAL .AND. .NOT.LSAME( SCAL, 'N' ) ) THEN
         INFO = -4
      ELSE IF( .NOT.LSORT .AND. .NOT.LSAME( SORT, 'U' ) ) THEN
         INFO = -5
      ELSE IF( N.LT.0 ) THEN
         INFO = -6
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDG.LT.MAX( 1, N ) ) THEN
         INFO = -10
      ELSE IF( LDQ.LT.MAX( 1, N ) ) THEN
         INFO = -12
      ELSE IF( LDS.LT.MAX( 1, N2 ) ) THEN
         INFO = -17
      ELSE IF( LDU.LT.MAX( 1, N2 ) ) THEN
         INFO = -19
      ELSE IF( ( .NOT.DISCR .AND. LDWORK.LT.MAX( 2, 6*N ) ) .OR.
     $         (      DISCR .AND. LDWORK.LT.MAX( 3, 6*N ) ) ) THEN
         INFO = -22
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'SB02MD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( N.EQ.0 ) THEN
         RCOND = ONE
         DWORK(1) = ONE
         DWORK(2) = ONE
         IF ( DISCR ) DWORK(3) = ONE
         RETURN
      END IF
C
      IF ( LSCAL ) THEN
C
C        Compute the norms of the matrices Q and G.
C
         QNORM = DLANSY( '1-norm', UPLO, N, Q, LDQ, DWORK )
         GNORM = DLANSY( '1-norm', UPLO, N, G, LDG, DWORK )
      END IF
C
C     Initialise the Hamiltonian or symplectic matrix associated with
C     the problem.
C     Workspace:  need   1          if DICO = 'C';
C                        max(2,4*N) if DICO = 'D';
C                 prefer larger if DICO = 'D'.
C
      CALL SB02MU( DICO, HINV, UPLO, N, A, LDA, G, LDG, Q, LDQ, S, LDS,
     $             IWORK, DWORK, LDWORK, INFO )
      IF ( INFO.NE.0 ) THEN
         INFO = 1
         RETURN
      END IF
C
      WRKOPT = DWORK(1)
      IF ( DISCR ) RCONDA = DWORK(2)
C
      ISCL = 0
      IF ( LSCAL ) THEN
C
C        Scale the Hamiltonian or symplectic matrix.
C
         IF( QNORM.GT.GNORM .AND. GNORM.GT.ZERO ) THEN
            CALL DLASCL( 'G', 0, 0, QNORM, GNORM, N, N, S(NP1,1), N2,
     $                   IERR )
            CALL DLASCL( 'G', 0, 0, GNORM, QNORM, N, N, S(1,NP1), N2,
     $                   IERR )
            ISCL = 1
         END IF
      END IF
C
C     Find the ordered Schur factorization of S,   S = U*H*U'.
C     Workspace:  need   6*N;
C                 prefer larger.
C
      IF ( .NOT.DISCR ) THEN
         IF ( LSORT ) THEN
            CALL DGEES( 'Vectors', 'Sorted', SB02MV, N2, S, LDS, NROT,
     $                  WR, WI, U, LDU, DWORK, LDWORK, BWORK, INFO )
         ELSE
            CALL DGEES( 'Vectors', 'Sorted', SB02MR, N2, S, LDS, NROT,
     $                  WR, WI, U, LDU, DWORK, LDWORK, BWORK, INFO )
         END IF
      ELSE
         IF ( LSORT ) THEN
            CALL DGEES( 'Vectors', 'Sorted', SB02MW, N2, S, LDS, NROT,
     $                  WR, WI, U, LDU, DWORK, LDWORK, BWORK, INFO )
         ELSE
            CALL DGEES( 'Vectors', 'Sorted', SB02MS, N2, S, LDS, NROT,
     $                  WR, WI, U, LDU, DWORK, LDWORK, BWORK, INFO )
         END IF
         IF ( LHINV ) THEN
            CALL DSWAP( N, WR, 1, WR(NP1), 1 )
            CALL DSWAP( N, WI, 1, WI(NP1), 1 )
         END IF
      END IF
      IF ( INFO.GT.N2 ) THEN
         INFO = 3
      ELSE IF ( INFO.GT.0 ) THEN
         INFO = 2
      ELSE IF ( NROT.NE.N ) THEN
         INFO = 4
      END IF
      IF ( INFO.NE.0 )
     $   RETURN
C
      WRKOPT = MAX( WRKOPT, DWORK(1) )
C
C     Check if U(1,1) is singular.  Use the (2,1) block of S as a
C     workspace for factoring U(1,1).
C
      UNORM = DLANGE( '1-norm', N, N, U, LDU, DWORK )
C
      CALL DLACPY( 'Full', N, N, U, LDU, S(NP1,1), LDS )
      CALL DGETRF( N, N, S(NP1,1), LDS, IWORK, INFO )
C
      IF ( INFO.GT.0 ) THEN
C
C        Singular matrix.  Set INFO and RCOND for error return.
C
         INFO  = 5
         RCOND = ZERO
         GO TO 100
      END IF
C
C     Estimate the reciprocal condition of U(1,1).
C     Workspace: 6*N.
C
      CALL DGECON( '1-norm', N, S(NP1,1), LDS, UNORM, RCOND,
     $             DWORK, IWORK(NP1), INFO )
C
      IF ( RCOND.LT.DLAMCH( 'Epsilon' ) ) THEN
C
C        Nearly singular matrix.  Set INFO for error return.
C
         INFO = 5
         RETURN
      END IF
C
C     Transpose U(2,1) in Q and compute the solution.
C
      DO 60 I = 1, N
         CALL DCOPY( N, U(NP1,I), 1, Q(I,1), LDQ )
   60 CONTINUE
C
      CALL DGETRS( 'Transpose', N, N, S(NP1,1), LDS, IWORK, Q, LDQ,
     $             INFO )
C
C     Set S(2,1) to zero.
C
      CALL DLASET( 'Full', N, N, ZERO, ZERO, S(NP1,1), LDS )
C
C     Make sure the solution matrix X is symmetric.
C
      DO 80 I = 1, N - 1
         CALL DAXPY( N-I, ONE, Q(I,I+1), LDQ, Q(I+1,I), 1 )
         CALL DSCAL( N-I, HALF, Q(I+1,I), 1 )
         CALL DCOPY( N-I, Q(I+1,I), 1, Q(I,I+1), LDQ )
   80 CONTINUE
C
      IF( LSCAL ) THEN
C
C        Undo scaling for the solution matrix.
C
         IF( ISCL.EQ.1 )
     $      CALL DLASCL( 'G', 0, 0, GNORM, QNORM, N, N, Q, LDQ, IERR )
      END IF
C
C     Set the optimal workspace, the scaling factor, and reciprocal
C     condition number (if any).
C
      DWORK(1) = WRKOPT
  100 CONTINUE
      IF( ISCL.EQ.1 ) THEN
         DWORK(2) = QNORM / GNORM
      ELSE
         DWORK(2) = ONE
      END IF
      IF ( DISCR ) DWORK(3) = RCONDA
C
      RETURN
C *** Last line of SB02MD ***
      END


      SUBROUTINE SB03OD( DICO, FACT, TRANS, N, M, A, LDA, Q, LDQ, B,
     $                   LDB, SCALE, WR, WI, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To solve for X = op(U)'*op(U) either the stable non-negative
C     definite continuous-time Lyapunov equation
C                                   2
C        op(A)'*X + X*op(A) = -scale *op(B)'*op(B)                   (1)
C
C     or the convergent non-negative definite discrete-time Lyapunov
C     equation
C                                   2
C        op(A)'*X*op(A) - X = -scale *op(B)'*op(B)                   (2)
C
C     where op(K) = K or K' (i.e., the transpose of the matrix K), A is
C     an N-by-N matrix, op(B) is an M-by-N matrix, U is an upper
C     triangular matrix containing the Cholesky factor of the solution
C     matrix X, X = op(U)'*op(U), and scale is an output scale factor,
C     set less than or equal to 1 to avoid overflow in X. If matrix B
C     has full rank then the solution matrix X will be positive-definite
C     and hence the Cholesky factor U will be nonsingular, but if B is
C     rank deficient then X may be only positive semi-definite and U
C     will be singular.
C
C     In the case of equation (1) the matrix A must be stable (that
C     is, all the eigenvalues of A must have negative real parts),
C     and for equation (2) the matrix A must be convergent (that is,
C     all the eigenvalues of A must lie inside the unit circle).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     DICO    CHARACTER*1
C             Specifies the type of Lyapunov equation to be solved as
C             follows:
C             = 'C':  Equation (1), continuous-time case;
C             = 'D':  Equation (2), discrete-time case.
C
C     FACT    CHARACTER*1
C             Specifies whether or not the real Schur factorization
C             of the matrix A is supplied on entry, as follows:
C             = 'F':  On entry, A and Q contain the factors from the
C                     real Schur factorization of the matrix A;
C             = 'N':  The Schur factorization of A will be computed
C                     and the factors will be stored in A and Q.
C
C     TRANS   CHARACTER*1
C             Specifies the form of op(K) to be used, as follows:
C             = 'N':  op(K) = K    (No transpose);
C             = 'T':  op(K) = K**T (Transpose).
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrix A and the number of columns in
C             matrix op(B).  N >= 0.
C
C     M       (input) INTEGER
C             The number of rows in matrix op(B).  M >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the matrix A. If FACT = 'F', then A contains
C             an upper quasi-triangular matrix S in Schur canonical
C             form; the elements below the upper Hessenberg part of the
C             array A are not referenced.
C             On exit, the leading N-by-N upper Hessenberg part of this
C             array contains the upper quasi-triangular matrix S in
C             Schur canonical form from the Shur factorization of A.
C             The contents of array A is not modified if FACT = 'F'.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     Q       (input or output) DOUBLE PRECISION array, dimension
C             (LDQ,N)
C             On entry, if FACT = 'F', then the leading N-by-N part of
C             this array must contain the orthogonal matrix Q of the
C             Schur factorization of A.
C             Otherwise, Q need not be set on entry.
C             On exit, the leading N-by-N part of this array contains
C             the orthogonal matrix Q of the Schur factorization of A.
C             The contents of array Q is not modified if FACT = 'F'.
C
C     LDQ     INTEGER
C             The leading dimension of array Q.  LDQ >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,N)
C             if TRANS = 'N', and dimension (LDB,max(M,N)), if
C             TRANS = 'T'.
C             On entry, if TRANS = 'N', the leading M-by-N part of this
C             array must contain the coefficient matrix B of the
C             equation.
C             On entry, if TRANS = 'T', the leading N-by-M part of this
C             array must contain the coefficient matrix B of the
C             equation.
C             On exit, the leading N-by-N part of this array contains
C             the upper triangular Cholesky factor U of the solution
C             matrix X of the problem, X = op(U)'*op(U).
C             If M = 0 and N > 0, then U is set to zero.
C
C     LDB     INTEGER
C             The leading dimension of array B.
C             LDB >= MAX(1,N,M), if TRANS = 'N';
C             LDB >= MAX(1,N),   if TRANS = 'T'.
C
C     SCALE   (output) DOUBLE PRECISION
C             The scale factor, scale, set less than or equal to 1 to
C             prevent the solution overflowing.
C
C     WR      (output) DOUBLE PRECISION array, dimension (N)
C     WI      (output) DOUBLE PRECISION array, dimension (N)
C             If FACT = 'N', and INFO >= 0 and INFO <= 2, WR and WI
C             contain the real and imaginary parts, respectively, of
C             the eigenvalues of A.
C             If FACT = 'F', WR and WI are not referenced.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, or INFO = 1, DWORK(1) returns the
C             optimal value of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             If M > 0, LDWORK >= MAX(1,4*N + MIN(M,N));
C             If M = 0, LDWORK >= 1.
C             For optimum performance LDWORK should sometimes be larger.
C
C             If LDWORK = -1, then a workspace query is assumed; the
C             routine only calculates the optimal size of the DWORK
C             array, returns this value as the first entry of the DWORK
C             array, and no error message related to LDWORK is issued by
C             XERBLA.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  if the Lyapunov equation is (nearly) singular
C                   (warning indicator);
C                   if DICO = 'C' this means that while the matrix A
C                   (or the factor S) has computed eigenvalues with
C                   negative real parts, it is only just stable in the
C                   sense that small perturbations in A can make one or
C                   more of the eigenvalues have a non-negative real
C                   part;
C                   if DICO = 'D' this means that while the matrix A
C                   (or the factor S) has computed eigenvalues inside
C                   the unit circle, it is nevertheless only just
C                   convergent, in the sense that small perturbations
C                   in A can make one or more of the eigenvalues lie
C                   outside the unit circle;
C                   perturbed values were used to solve the equation;
C             = 2:  if FACT = 'N' and DICO = 'C', but the matrix A is
C                   not stable (that is, one or more of the eigenvalues
C                   of A has a non-negative real part), or DICO = 'D',
C                   but the matrix A is not convergent (that is, one or
C                   more of the eigenvalues of A lies outside the unit
C                   circle); however, A will still have been factored
C                   and the eigenvalues of A returned in WR and WI.
C             = 3:  if FACT = 'F' and DICO = 'C', but the Schur factor S
C                   supplied in the array A is not stable (that is, one
C                   or more of the eigenvalues of S has a non-negative
C                   real part), or DICO = 'D', but the Schur factor S
C                   supplied in the array A is not convergent (that is,
C                   one or more of the eigenvalues of S lies outside the
C                   unit circle);
C             = 4:  if FACT = 'F' and the Schur factor S supplied in
C                   the array A has two or more consecutive non-zero
C                   elements on the first sub-diagonal, so that there is
C                   a block larger than 2-by-2 on the diagonal;
C             = 5:  if FACT = 'F' and the Schur factor S supplied in
C                   the array A has a 2-by-2 diagonal block with real
C                   eigenvalues instead of a complex conjugate pair;
C             = 6:  if FACT = 'N' and the LAPACK Library routine DGEES
C                   has failed to converge. This failure is not likely
C                   to occur. The matrix B will be unaltered but A will
C                   be destroyed.
C
C     METHOD
C
C     The method used by the routine is based on the Bartels and Stewart
C     method [1], except that it finds the upper triangular matrix U
C     directly without first finding X and without the need to form the
C     normal matrix op(B)'*op(B).
C
C     The Schur factorization of a square matrix A is given by
C
C        A = QSQ',
C
C     where Q is orthogonal and S is an N-by-N block upper triangular
C     matrix with 1-by-1 and 2-by-2 blocks on its diagonal (which
C     correspond to the eigenvalues of A). If A has already been
C     factored prior to calling the routine however, then the factors
C     Q and S may be supplied and the initial factorization omitted.
C
C     If TRANS = 'N', the matrix B is factored as (QR factorization)
C            _   _                   _   _  _
C        B = P ( R ),  M >= N,   B = P ( R  Z ),  M < N,
C              ( 0 )
C           _                                    _
C     where P is an M-by-M orthogonal matrix and R is a square upper
C                                         _   _      _     _  _
C     triangular matrix. Then, the matrix B = RQ, or B = ( R  Z )Q (if
C     M < N) is factored as
C        _                       _
C        B = P ( R ),  M >= N,   B = P ( R  Z ),  M < N.
C
C     If TRANS = 'T', the matrix B is factored as (RQ factorization)
C                                         _
C                 _   _                 ( Z ) _
C        B = ( 0  R ) P,  M >= N,   B = ( _ ) P,  M < N,
C                                       ( R )
C           _                                    _
C     where P is an M-by-M orthogonal matrix and R is a square upper
C                                         _     _     _       _   _
C     triangular matrix. Then, the matrix B = Q'R, or B = Q'( Z'  R' )'
C     (if M < N) is factored as
C        _                       _
C        B = ( R ) P,  M >= N,   B = ( Z ) P,  M < N.
C                                    ( R )
C
C     These factorizations are utilised to either transform the
C     continuous-time Lyapunov equation to the canonical form
C                                                        2
C       op(S)'*op(V)'*op(V) + op(V)'*op(V)*op(S) = -scale *op(F)'*op(F),
C
C     or the discrete-time Lyapunov equation to the canonical form
C                                                        2
C       op(S)'*op(V)'*op(V)*op(S) - op(V)'*op(V) = -scale *op(F)'*op(F),
C
C     where V and F are upper triangular, and
C
C        F = R,  M >= N,   F = ( R  Z ),  M < N, if TRANS = 'N';
C                              ( 0  0 )
C
C        F = R,  M >= N,   F = ( 0  Z ),  M < N, if TRANS = 'T'.
C                              ( 0  R )
C
C     The transformed equation is then solved for V, from which U is
C     obtained via the QR factorization of V*Q', if TRANS = 'N', or
C     via the RQ factorization of Q*V, if TRANS = 'T'.
C
C     REFERENCES
C
C     [1] Bartels, R.H. and Stewart, G.W.
C         Solution of the matrix equation  A'X + XB = C.
C         Comm. A.C.M., 15, pp. 820-826, 1972.
C
C     [2] Hammarling, S.J.
C         Numerical solution of the stable, non-negative definite
C         Lyapunov equation.
C         IMA J. Num. Anal., 2, pp. 303-325, 1982.
C
C     NUMERICAL ASPECTS
C                               3
C     The algorithm requires 0(N ) operations and is backward stable.
C
C     FURTHER COMMENTS
C
C     The Lyapunov equation may be very ill-conditioned. In particular,
C     if A is only just stable (or convergent) then the Lyapunov
C     equation will be ill-conditioned.  A symptom of ill-conditioning
C     is "large" elements in U relative to those of A and B, or a
C     "small" value for scale. A condition estimate can be computed
C     using SLICOT Library routine SB03MD.
C
C     SB03OD routine can be also used for solving "unstable" Lyapunov
C     equations, i.e., when matrix A has all eigenvalues with positive
C     real parts, if DICO = 'C', or with moduli greater than one,
C     if DICO = 'D'. Specifically, one may solve for X = op(U)'*op(U)
C     either the continuous-time Lyapunov equation
C                                  2
C        op(A)'*X + X*op(A) = scale *op(B)'*op(B),                   (3)
C
C     or the discrete-time Lyapunov equation
C                                  2
C        op(A)'*X*op(A) - X = scale *op(B)'*op(B),                   (4)
C
C     provided, for equation (3), the given matrix A is replaced by -A,
C     or, for equation (4), the given matrices A and B are replaced by
C     inv(A) and B*inv(A), if TRANS = 'N' (or inv(A)*B, if TRANS = 'T'),
C     respectively. Although the inversion generally can rise numerical
C     problems, in case of equation (4) it is expected that the matrix A
C     is enough well-conditioned, having only eigenvalues with moduli
C     greater than 1. However, if A is ill-conditioned, it could be
C     preferable to use the more general SLICOT Lyapunov solver SB03MD.
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Aug. 1997.
C     Supersedes Release 2.0 routine SB03CD by Sven Hammarling,
C     NAG Ltd, United Kingdom.
C
C     REVISIONS
C
C     Dec. 1997, April 1998, May 1998, May 1999, Oct. 2001 (V. Sima).
C     March 2002 (A. Varga).
C     V. Sima, Research Institute for Informatics, Bucharest, July 2011.
C
C     KEYWORDS
C
C     Lyapunov equation, orthogonal transformation, real Schur form,
C     Sylvester equation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         DICO, FACT, TRANS
      INTEGER           INFO, LDA, LDB, LDQ, LDWORK, M, N
      DOUBLE PRECISION  SCALE
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), DWORK(*), Q(LDQ,*), WI(*),
     $                  WR(*)
C     .. Local Scalars ..
      LOGICAL           CONT, LQUERY, LTRANS, NOFACT
      INTEGER           I, IFAIL, INFORM, ITAU, J, JWORK, K, L, MAXMN,
     $                  MINMN, MINWRK, NE, SDIM, WRKOPT
      DOUBLE PRECISION  EMAX, TEMP
C     .. Local Arrays ..
      LOGICAL           BWORK(1)
C     .. External Functions ..
      LOGICAL           LSAME, SELECT
      DOUBLE PRECISION  DLAPY2
      EXTERNAL          DLAPY2, LSAME, SELECT
C     .. External Subroutines ..
      EXTERNAL          DCOPY, DGEES, DGEMM, DGEMV, DGEQRF, DGERQF,
     $                  DLACPY, DLASET, DTRMM, SB03OU, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX, MIN
C     .. Executable Statements ..
C
C     Test the input scalar arguments.
C
      CONT   = LSAME( DICO,  'C' )
      NOFACT = LSAME( FACT,  'N' )
      LTRANS = LSAME( TRANS, 'T' )
      LQUERY = LDWORK.EQ.-1
      MINMN  = MIN( M, N )
      MAXMN  = MAX( M, N )
C
      INFO = 0
      IF( .NOT.CONT .AND. .NOT.LSAME( DICO, 'D' ) ) THEN
         INFO = -1
      ELSE IF( .NOT.NOFACT .AND. .NOT.LSAME( FACT, 'F' ) ) THEN
         INFO = -2
      ELSE IF( .NOT.LTRANS .AND. .NOT.LSAME( TRANS, 'N' ) ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -4
      ELSE IF( M.LT.0 ) THEN
         INFO = -5
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDQ.LT.MAX( 1, N ) ) THEN
         INFO = -9
      ELSE IF( ( LDB.LT.MAX( 1, N ) )  .OR.
     $         ( LDB.LT.MAX( 1, MAXMN ) .AND. .NOT.LTRANS ) ) THEN
         INFO = -11
      ELSE
         IF( M.GT.0 ) THEN
            MINWRK = 4*N + MINMN
         ELSE
            MINWRK = 1
         END IF
         IF( LQUERY ) THEN
            IF ( NOFACT ) THEN
               CALL DGEES( 'Vectors', 'Not ordered', SELECT, N, A, LDA,
     $                     SDIM, WR, WI, Q, LDQ, DWORK, -1, BWORK,
     $                     IFAIL )
               WRKOPT = MAX( MINWRK, INT( DWORK(1) ) )
            ELSE
               WRKOPT = MINWRK
            END IF
            IF ( LTRANS ) THEN
               CALL DGERQF( N, MAXMN, B, LDB, DWORK, DWORK, -1, IFAIL )
            ELSE
               CALL DGEQRF( MAXMN, N, B, LDB, DWORK, DWORK, -1, IFAIL )
            END IF
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + MAXMN, MINMN*N )
            CALL SB03OU( .NOT.CONT, LTRANS, N, MINMN, A, LDA, B, LDB,
     $                   DWORK, B, LDB, SCALE, DWORK, -1, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) + MINMN, N*N )
         ELSE IF( LDWORK.LT.MINWRK ) THEN
            INFO = -16
         END IF
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'SB03OD', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK(1) = WRKOPT
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( MINMN.EQ.0 ) THEN
         IF( M.EQ.0 )
     $      CALL DLASET( 'Full', N, N, ZERO, ZERO, B, LDB )
         SCALE = ONE
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Start the solution.
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
      IF ( NOFACT ) THEN
C
C        Find the Schur factorization of A,   A = Q*S*Q'.
C        Workspace:  need   3*N;
C                    prefer larger.
C
         CALL DGEES( 'Vectors', 'Not ordered', SELECT, N, A, LDA, SDIM,
     $               WR, WI, Q, LDQ, DWORK, LDWORK, BWORK, INFORM )
         IF ( INFORM.NE.0 ) THEN
            INFO = 6
            RETURN
         END IF
         WRKOPT = DWORK(1)
C
C        Check the eigenvalues for stability.
C
         IF ( CONT ) THEN
            EMAX = WR(1)
C
            DO 20 J = 2, N
               IF ( WR(J).GT.EMAX )
     $            EMAX = WR(J)
   20       CONTINUE
C
         ELSE
            EMAX = DLAPY2( WR(1), WI(1) )
C
            DO 40 J = 2, N
               TEMP = DLAPY2( WR(J), WI(J) )
               IF ( TEMP.GT.EMAX )
     $            EMAX = TEMP
   40       CONTINUE
C
         END IF
C
         IF (    ( CONT ) .AND. ( EMAX.GE.ZERO ) .OR.
     $      ( .NOT.CONT ) .AND. ( EMAX.GE.ONE  ) ) THEN
            INFO = 2
            RETURN
         END IF
      ELSE
         WRKOPT = 0
      END IF
C
C     Perform the QR or RQ factorization of B,
C            _   _           _   _  _
C        B = P ( R ), or B = P ( R  Z ), if TRANS = 'N', or
C              ( 0 )
C                                 _
C                 _   _         ( Z ) _
C        B = ( 0  R ) P, or B = ( _ ) P, if TRANS = 'T'.
C                               ( R )
C     Workspace:  need   MIN(M,N) + N;
C                 prefer MIN(M,N) + N*NB.
C
      ITAU  = 1
      JWORK = ITAU + MINMN
      IF ( LTRANS ) THEN
         CALL DGERQF( N, M, B, LDB, DWORK(ITAU), DWORK(JWORK),
     $                LDWORK-JWORK+1, IFAIL )
         WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1, MINMN*N)
         JWORK = ITAU
C
C        Form in B
C        _      _              _         _   _                    _
C        B := Q'R,   m >= n,   B := Q'*( Z'  R' )',   m < n, with B an
C        n-by-min(m,n) matrix.
C        Use a BLAS 3 operation if enough workspace, and BLAS 2,
C                   _
C        otherwise: B is formed column by column.
C
         IF ( LDWORK.GE.JWORK+MINMN*N-1 ) THEN
            K = JWORK
C
            DO 60 I = 1, MINMN
               CALL DCOPY( N, Q(N-MINMN+I,1), LDQ, DWORK(K), 1 )
               K = K + N
   60       CONTINUE
C
            CALL DTRMM( 'Right', 'Upper', 'No transpose', 'Non-unit',
     $                  N, MINMN, ONE, B(N-MINMN+1,M-MINMN+1), LDB,
     $                  DWORK(JWORK), N )
            IF ( M.LT.N )
     $         CALL DGEMM( 'Transpose', 'No transpose', N, M, N-M,
     $                     ONE, Q, LDQ, B, LDB, ONE, DWORK(JWORK), N )
            CALL DLACPY( 'Full', N, MINMN, DWORK(JWORK), N, B, LDB )
         ELSE
            NE = N - MINMN
C
            DO 80 J = 1, MINMN
               NE = NE + 1
               CALL DCOPY( NE, B(1,M-MINMN+J), 1, DWORK(JWORK), 1 )
               CALL DGEMV( 'Transpose', NE, N, ONE, Q, LDQ,
     $                     DWORK(JWORK), 1, ZERO, B(1,J), 1 )
   80       CONTINUE
C
         END IF
      ELSE
         CALL DGEQRF( M, N, B, LDB, DWORK(ITAU), DWORK(JWORK),
     $                LDWORK-JWORK+1, IFAIL )
         WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1, MINMN*N)
         JWORK = ITAU
C
C        Form in B
C        _    _               _      _  _                    _
C        B := RQ,   m >= n,   B := ( R  Z )*Q,   m < n, with B an
C        min(m,n)-by-n matrix.
C        Use a BLAS 3 operation if enough workspace, and BLAS 2,
C                   _
C        otherwise: B is formed row by row.
C
         IF ( LDWORK.GE.JWORK+MINMN*N-1 ) THEN
            CALL DLACPY( 'Full', MINMN, N, Q, LDQ, DWORK(JWORK), MINMN )
            CALL DTRMM( 'Left', 'Upper', 'No transpose', 'Non-unit',
     $                  MINMN, N, ONE, B, LDB, DWORK(JWORK), MINMN )
            IF ( M.LT.N )
     $         CALL DGEMM( 'No transpose', 'No transpose', M, N, N-M,
     $                     ONE, B(1,M+1), LDB, Q(M+1,1), LDQ, ONE,
     $                     DWORK(JWORK), MINMN )
            CALL DLACPY( 'Full', MINMN, N, DWORK(JWORK), MINMN, B, LDB )
         ELSE
            NE = MINMN + MAX( 0, N-M )
C
            DO 100 J = 1, MINMN
               CALL DCOPY( NE, B(J,J), LDB, DWORK(JWORK), 1 )
               CALL DGEMV( 'Transpose', NE, N, ONE, Q(J,1), LDQ,
     $                     DWORK(JWORK), 1, ZERO, B(J,1), LDB )
               NE = NE - 1
  100       CONTINUE
C
         END IF
      END IF
      JWORK  = ITAU + MINMN
C
C     Solve for U the transformed Lyapunov equation
C                                                      2    _      _
C     op(S)'*op(U)'*op(U) + op(U)'*op(U)*op(S) = -scale *op(B)'*op(B),
C
C     or
C                                                      2    _      _
C     op(S)'*op(U)'*op(U)*op(S) - op(U)'*op(U) = -scale *op(B)'*op(B)
C
C     Workspace:  need   MIN(M,N) + 4*N;
C                 prefer larger.
C
      CALL SB03OU( .NOT.CONT, LTRANS, N, MINMN, A, LDA, B, LDB,
     $             DWORK(ITAU), B, LDB, SCALE, DWORK(JWORK),
     $             LDWORK-JWORK+1, INFO )
      IF ( INFO.GT.1 ) THEN
         INFO = INFO + 1
         RETURN
      END IF
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1 )
      JWORK  = ITAU
C
C     Form   U :=  U*Q' or U := Q*U in the array B.
C     Use a BLAS 3 operation if enough workspace, and BLAS 2, otherwise.
C     Workspace:  need   N;
C                 prefer N*N;
C
      IF ( LDWORK.GE.JWORK+N*N-1 ) THEN
         IF ( LTRANS ) THEN
            CALL DLACPY( 'Full', N, N, Q, LDQ, DWORK(JWORK), N )
            CALL DTRMM( 'Right', 'Upper', 'No transpose', 'Non-unit', N,
     $                  N, ONE, B, LDB, DWORK(JWORK), N )
         ELSE
            K = JWORK
C
            DO 120 I = 1, N
               CALL DCOPY( N, Q(1,I), 1, DWORK(K), N )
               K = K + 1
  120       CONTINUE
C
            CALL DTRMM( 'Left', 'Upper', 'No transpose', 'Non-unit', N,
     $                  N, ONE, B, LDB, DWORK(JWORK), N )
         END IF
         CALL DLACPY( 'Full', N, N, DWORK(JWORK), N, B, LDB )
         WRKOPT = MAX( WRKOPT, JWORK + N*N - 1 )
      ELSE
         IF ( LTRANS ) THEN
C
C           U is formed column by column ( U := Q*U ).
C
            DO 140 I = 1, N
               CALL DCOPY( I, B(1,I), 1, DWORK(JWORK), 1 )
               CALL DGEMV( 'No transpose', N, I, ONE, Q, LDQ,
     $                     DWORK(JWORK), 1, ZERO, B(1,I), 1 )
  140       CONTINUE
         ELSE
C
C           U is formed row by row ( U' := Q*U' ).
C
            DO 160 I = 1, N
               CALL DCOPY( N-I+1, B(I,I), LDB, DWORK(JWORK), 1 )
               CALL DGEMV( 'No transpose', N, N-I+1, ONE, Q(1,I), LDQ,
     $                     DWORK(JWORK), 1, ZERO, B(I,1), LDB )
  160       CONTINUE
         END IF
      END IF
C
C     Lastly find the QR or RQ factorization of U, overwriting on B,
C     to give the required Cholesky factor.
C     Workspace:  need   2*N;
C                 prefer N + N*NB;
C
      JWORK = ITAU + N
      IF ( LTRANS ) THEN
         CALL DGERQF( N, N, B, LDB, DWORK(ITAU), DWORK(JWORK),
     $                LDWORK-JWORK+1, IFAIL )
      ELSE
         CALL DGEQRF( N, N, B, LDB, DWORK(ITAU), DWORK(JWORK),
     $                LDWORK-JWORK+1, IFAIL )
      END IF
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1 )
C
C     Make the diagonal elements of U non-negative.
C
      IF ( LTRANS ) THEN
C
         DO 200 J = 1, N
            IF ( B(J,J).LT.ZERO ) THEN
C
               DO 180 I = 1, J
                  B(I,J) = -B(I,J)
  180          CONTINUE
C
            END IF
  200    CONTINUE
C
      ELSE
         K = JWORK
C
         DO 240 J = 1, N
            DWORK(K) = B(J,J)
            L = JWORK
C
            DO 220 I = 1, J
               IF ( DWORK(L).LT.ZERO ) B(I,J) = -B(I,J)
               L = L + 1
  220       CONTINUE
C
            K = K + 1
  240    CONTINUE
      END IF
C
      IF( N.GT.1 )
     $   CALL DLASET( 'Lower', N-1, N-1, ZERO, ZERO, B(2,1), LDB )
C
C     Set the optimal workspace.
C
      DWORK(1) = WRKOPT
C
      RETURN
C *** Last line of SB03OD ***
      END


subroutine CalcEnergy(Pos, M, L, rc, PEnergy, Dim, NAtom)
    implicit none
    integer, intent(in) :: M, Dim, NAtom
    real(8), intent(in), dimension(0:NAtom-1, 0:Dim-1) :: Pos
    real(8), intent(in) :: L, rc
    real(8), intent(out) :: PEnergy
    real(8), parameter :: k = 3000., r0 = 1.
    integer:: i, j
    real(8) :: kfac, d2, sep, disp, shiftval, rc2, id6, id2, id12
    real(8), dimension(Dim) :: rij, Posi

    !Assign initially zeros to the force array and potential energy
    PEnergy = 0.
    rc2 = rc**2

    !Precompute constants such as LJ shift value up here
    kfac = k/2.
    id6 = 1./rc**6
    shiftval = 4*(id6**2 - id6)

    do i=0, NAtom - 1
        !store Pos(i,:) in a temporary array for faster access in j looop
        Posi = Pos(i,:)
        do j = i+1, NAtom - 1
            !compute initial rij
            rij = Pos(j,:) - Posi
            !Apply minimum image convention
            rij = rij - L*dnint(rij / L)

            !Do bonded calculation
            if ( j == i+1 .and. mod(j, M)> 0) then
                !Compute scalar separation
                sep = sqrt( sum( rij * rij ) )
                !Compute displacement relative to preferred separation
                disp = sep - r0
                !Calculate energy
                PEnergy = PEnergy + kfac * disp*disp
            else   !Do nonbonded calculation
                !compute scalar distance
                d2 = sum(rij*rij)
                !Check against cutoff
                if ( d2 > rc2) then
                    cycle
                end if
                id2 = 1./d2                 !inverse squared distance
                id6 = id2 * id2 * id2       !inverse sixth distance
                id12 = id6 * id6            !inverse twelfth distance
                PEnergy = Penergy + 4. * (id12 - id6) + shiftval

                !end nonbonded calculation
            endif
        enddo
    enddo
end subroutine

subroutine CalcEnergyForces(Pos, M, L, rc, PEnergy, Forces, Dim, NAtom)
    implicit none
    integer, intent(in) :: M, Dim, NAtom
    real(8), intent(in), dimension(0:NAtom-1, 0:Dim-1) :: Pos
    real(8), intent(in) :: L, rc
    real(8), intent(out) :: PEnergy
    real(8), intent(inout), dimension(0:NAtom-1, 0:Dim-1) :: Forces
!f2py intent(in, out, inplace) :: Forces
    real(8), parameter :: k = 3000., r0 = 1.
    integer :: i, j
    real(8) :: kfac, d2, sep, disp, shiftval, rc2, id6, id2, id12
    real(8), dimension(Dim) :: rij, Fij, Posi

    !Assign initially zeros to the force array and potential energy
    Forces = 0.
    PEnergy = 0.
    rc2 = rc**2

    !Precompute constants such as LJ shift value up here
    kfac = k/2.
    id6 = 1./rc**6
    shiftval = 4.*(id6**2 - id6)

    do i=0, NAtom - 1
        !store Pos(i,:) in a temporary array for faster access in j looop
        Posi = Pos(i,:)
        do j = i+1, NAtom - 1
            !compute initial rij
            rij = Pos(j,:) - Posi
            !Apply minimum image convention
            rij = rij - L*dnint(rij / L)

            !Do bonded calculation
            if ( j == i+1 .and. mod(j, M)> 0) then
                !Compute scalar separation
                sep = sqrt( sum( rij * rij ) )
                !Compute displacement relative to preferred separation
                disp = sep - r0
                !Calculate energy
                PEnergy = PEnergy + kfac * disp*disp
                !Calculate force: - k*displacement in direction of rij
                Fij = k * ( disp ) * rij/sep
                Forces(i,:) = Forces(i,:) + Fij
                Forces(j,:) = Forces(j,:) - Fij
            else   !Do nonbonded calculation
                !compute scalar distance
                d2 = sum(rij*rij)
                !Check against cutoff
                if ( d2 > rc2) then
                    cycle
                end if
                id2 = 1./d2                 !inverse squared distance
                id6 = id2 * id2 * id2       !inverse sixth distance
                id12 = id6 * id6            !inverse twelfth distance
                PEnergy = Penergy + 4. * (id12 - id6) + shiftval
                Fij = rij * (-48. * id12 + 24. * id6) * id2
                Forces(i,:) = Forces(i,:) + Fij
                Forces(j,:) = Forces(j,:) - Fij

                !end nonbonded calculation
            endif
        enddo
    enddo
end subroutine

subroutine VVIntegrate(Pos, Vel, Accel, M, L, rc, dt, KEnergy, PEnergy, Dim, NAtom)
    implicit none
    integer, intent(in) :: M, Dim, NAtom
    real(8), intent(in) :: L, rc, dt
    real(8), intent(inout), dimension(0:NAtom-1, 0:Dim-1) :: Pos, Vel, Accel
!f2py intent(in,out,inplace) :: Pos, Vel, Accel
    real(8), intent(out) :: KEnergy, PEnergy
    external :: CalcEnergyForces
    Pos = Pos + dt * Vel + 0.5 * dt*dt * Accel
    Vel = Vel + 0.5 * dt * Accel
    call CalcEnergyForces(Pos, M, L, rc, PEnergy, Accel, Dim, NAtom)
    Vel = Vel + 0.5 * dt * Accel
    KEnergy = 0.5 * sum(Vel*Vel)
end subroutine


SUBROUTINE vmdpl(dee,stress,pl)
!
! This subroutine forms the plastic stress/strain matrix
! for a von-Mises material.
!
 IMPLICIT NONE
 INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
 REAL(iwp),INTENT(IN)::stress(:),dee(:,:)
 REAL(iwp),INTENT(OUT)::pl(:,:)
 REAL(iwp),ALLOCATABLE::dfds(:),ddfds(:)
 REAL(iwp)::t2,t6,t10,t14,t16,t17,t18,t19,t21,t22,t23,t25,t26,t30,sq2,sx, &
   sy,sz,txy,tyz,tzx,one=1.0_iwp,two=2.0_iwp,d4=4.0_iwp,d6=6.0_iwp
 REAL(iwp)::denom
 INTEGER::i,j,ih
 ih=SIZE(stress)
 ALLOCATE(dfds(ih),ddfds(ih))
 sq2=SQRT(two)
 SELECT CASE(ih)
 CASE(4)
   sx=stress(1)
   sy=stress(2)
   txy=stress(3)
   sz=stress(4)
   t2=sx**2
   t6=sy**2
   t10=sz**2
   t14=txy**2
   t17=SQRT(two*t2-two*sx*sy+two*t6-two*sy*sz+two*t10-two*sz*sx+d6*t14)
   t19=one/sq2/t17
   t21=two*sy
   t22=two*sz
   t26=two*sx
   dfds(1)=t19*(d4*sx-t21-t22)/two
   dfds(2)=t19*(-t26+d4*sy-t22)/two
   dfds(3)=t19*d6*txy
   dfds(4)=t19*(-t21+d4*sz-t26)/two
 CASE(6)
   sx=stress(1)
   sy=stress(2)
   sz=stress(3)
   txy=stress(4)
   tyz=stress(5)
   tzx=stress(6)
   t2=sx**2
   t6=sy**2
   t10=sz**2
   t14=txy**2
   t16=tyz**2
   t18=tzx**2
   t21=SQRT(two*t2-two*sx*sy+two*t6-two*sy*sz+two*t10-                    &
     two*sz*sx+d6*t14+d6*t16+d6*t18)
   t23=one/sq2/t21
   t25=two*sy
   t26=two*sz
   t30=two*sx
   dfds(1)=t23*(d4*sx-t25-t26)/two
   dfds(2)=t23*(-t30+d4*sy-t26)/two
   dfds(3)=t23*(-t25+d4*sz-t30)/two
   dfds(4)=t23*d6*txy
   dfds(5)=t23*d6*tyz
   dfds(6)=t23*d6*tzx
 END SELECT
 ddfds=MATMUL(dee,dfds)
 denom=DOT_PRODUCT(ddfds,dfds)
 DO i=1,ih
   DO j=1,ih
     pl(i,j)=ddfds(i)*ddfds(j)/denom
   END DO
 END DO
 DEALLOCATE(dfds,ddfds)
RETURN
END SUBROUTINE vmdpl


subroutine outnormal_tet(x,nsd,p,outnorm,area)
! calculate the outward normal for the surface  on the solid interface
! and the area of the surface for tet element cases only
real(8) x(nsd,3)
real(8) p(nsd)
integer nsd
real(8) outnorm(nsd)
real(8) area
!---------------------------
real(8) ab(nsd)
real(8) ac(nsd)
real(8) ap(nsd)
real(8) dot_pro

ab(:)=x(:,2)-x(:,1)
ac(:)=x(:,3)-x(:,1)
ap(:)=p(:)-x(:,1)

outnorm(1)=ab(2)*ac(3)-ab(3)*ac(2)
outnorm(2)=-ab(1)*ac(3)+ab(3)*ac(1)
outnorm(3)=ab(1)*ac(2)-ab(2)*ac(1)

area=outnorm(1)**2+outnorm(2)**2+outnorm(3)**2
area=0.5*sqrt(area)

dot_pro=ap(1)*outnorm(1)+ap(2)*outnorm(2)+ap(3)*outnorm(3)

if (dot_pro > 0) then
outnorm(:)=-outnorm(:)
end if

outnorm(:)=outnorm(:)/area*0.5

continue

end


program NN
    implicit none
    real :: inputs(4),weights1(4),weights2(4),weights3(4),bias1,bias2,bias3,output(3)

    inputs   = [ 1.0 ,  2.0 ,  3.0 ,  2.5 ]
    weights1 = [ 0.2 ,  0.8 , -0.5 ,  1.0 ]
    weights2 = [ 0.5 , -0.91,  0.26, -0.5 ]
    weights3 = [-0.26, -0.27,  0.17,  0.87]
    bias1    = 2
    bias2    = 3
    bias3    = 0.5

    output = [inputs(1) * weights1(1) +&
              inputs(2) * weights1(2) +&
              inputs(3) * weights1(3) +&
              inputs(4) * weights1(4) +&
              bias1,&
              inputs(1) * weights2(1) +&
              inputs(2) * weights2(2) +&
              inputs(3) * weights2(3) +&
              inputs(4) * weights2(4) +&
              bias2,&
              inputs(1) * weights3(1) +&
              inputs(2) * weights3(2) +&
              inputs(3) * weights3(3) +&
              inputs(4) * weights3(4) +&
              bias3]

    print *,output(1),output(2),output(3)
end program NN


        function planckfn(lambda,T)

! Calculate intensity [Jy/arcsec^2] at wavelength lambda [microns] of
! optically thick source of temperature T [K].
!
! Restriction:  lambda*T > 200

        real,   intent(in)    ::  lambda        ! wavelength [microns]
        real,   intent(in)    ::  T             ! temperature [K]

        planckfn = (977.532/lambda)**3/(exp(14401.9/(lambda*T)) - 1.)

        end function planckfn


SUBROUTINE mocouf(phi,c,sigm,dsbar,theta,f)
!
! This subroutine calculates the value of the yield function
! for a Mohr-Coulomb material (phi in degrees).
!
 IMPLICIT NONE
 INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
 REAL(iwp),INTENT(IN)::phi,c,sigm,dsbar,theta
 REAL(iwp),INTENT(OUT)::f
 REAL(iwp)::phir,snph,csph,csth,snth,one=1.0_iwp,d3=3.0_iwp,d4=4.0_iwp,   &
   d180=180.0_iwp
 phir=phi*d4*ATAN(one)/d180
 snph=SIN(phir)
 csph=COS(phir)
 csth=COS(theta)
 snth=SIN(theta)
 f=snph*sigm+dsbar*(csth/SQRT(d3)-snth*snph/d3)-c*csph
RETURN
END SUBROUTINE mocouf


! Like array_constructor_6.f90, but check constructors that apply
! an elemental function to an array.
! { dg-do run }
program main
  implicit none
  call build (200)
contains
  subroutine build (order)
    integer :: order, i

    call test (order, (/ (abs ((/ i, -i, -i * 2 /)), i = 1, order) /))
    call test (order, abs ((/ ((/ -i, -i, i * 2 /), i = 1, order) /)))
    call test (order, (/ abs ((/ ((/ i, i, -i * 2 /), i = 1, order) /)) /))
  end subroutine build

  subroutine test (order, values)
    integer, dimension (3:) :: values
    integer :: order, i

    if (size (values, dim = 1) .ne. order * 3) STOP 1
    do i = 1, order
      if (values (i * 3) .ne. i) STOP 2
      if (values (i * 3 + 1) .ne. i) STOP 3
      if (values (i * 3 + 2) .ne. i * 2) STOP 4
    end do
  end subroutine test
end program main


module fqs_fake_kine

    use fqs_types,        only: wp
    use fqs_constants,    only: pi
    use fqs_euler_angle,  only: euler_t
    use fqs_utility,      only: deg2rad

    implicit none

    type, public :: fake_kine_param_t
        real(wp) :: phi_amplitude = 0.0_wp
        real(wp) :: alpha_amplitude = 0.0_wp
        real(wp) :: alpha_slope = 0.0_wp
        real(wp) :: alpha_overshoot_size = 0.0_wp
        real(wp) :: alpha_overshoot_power = 0.0_wp
        real(wp) :: theta_amplitude = 10.0_wp
        real(wp) :: theta_power = 0.0_wp
        real(wp) :: period = 0.0_wp
    end type fake_kine_param_t

    public fake_kine_period
    public fake_param_type_1
    public fake_param_type_2
    public fake_param_type_3
    public fake_param_type_4
    public fake_kine
    real(wp), parameter :: fake_kine_period  = 0.005_wp

contains

    subroutine fake_kine(t, euler, param)
        real(wp),                intent(in)            :: t(:)
        type(euler_t),           intent(out)           :: euler(:)
        type(fake_kine_param_t), intent(in), optional  :: param

        ! Local variables
        type(fake_kine_param_t) :: param_tmp
        real(wp)                :: cos_tmp(size(t))
        real(wp)                :: sin_tmp(size(t))
        real(wp)                :: attitude_erf(size(t))
        real(wp)                :: attitude_base(size(t))
        real(wp)                :: attitude_overshoot(size(t))

        if (present(param)) then
            param_tmp = param
        else
            param_tmp = fake_param_type_4()
        end if

        cos_tmp = cos( 2.0_wp*pi*t/(param_tmp % period) )
        sin_tmp = sin( 2.0_wp*pi*t/(param_tmp % period) )

        attitude_base = (param_tmp % alpha_amplitude) *  erf( (param_tmp % alpha_slope) * sin_tmp )
        attitude_overshoot = sign(1.0_wp,cos_tmp)*(abs(cos_tmp) ** (param_tmp % alpha_overshoot_power))
        attitude_overshoot = (param_tmp % alpha_overshoot_size)*attitude_overshoot

        euler % heading  = (param_tmp % phi_amplitude) * cos_tmp
        euler % attitude = attitude_base  + attitude_overshoot
        euler % bank = (param_tmp % theta_amplitude) * abs( cos_tmp ) ** (param_tmp % theta_power )

    end subroutine fake_kine

    function fake_param_type_1() result(param)
        type(fake_kine_param_t) :: param
        param % phi_amplitude = deg2rad(70.0_wp)
        param % alpha_amplitude = deg2rad(50.0_wp)
        param % alpha_slope = 3.0_wp
        param % alpha_overshoot_size = deg2rad(0.0_wp)
        param % alpha_overshoot_power = 12.0_wp
        param % theta_amplitude = deg2rad(0.0_wp)
        param % theta_power = 10.0_wp
        param % period  = fake_kine_period
    end function fake_param_type_1

    function fake_param_type_2() result(param)
        type(fake_kine_param_t) :: param
        param % phi_amplitude = deg2rad(70.0_wp)
        param % alpha_amplitude = deg2rad(50.0_wp)
        param % alpha_slope = 3.0_wp
        param % alpha_overshoot_size = deg2rad(0.0_wp)
        param % alpha_overshoot_power = 12.0_wp
        param % theta_amplitude = deg2rad(10.0_wp)
        param % theta_power = 10.0_wp
        param % period  = fake_kine_period
    end function fake_param_type_2

    function fake_param_type_3() result(param)
        type(fake_kine_param_t) :: param
        param % phi_amplitude = deg2rad(70.0_wp)
        param % alpha_amplitude = deg2rad(50.0_wp)
        param % alpha_slope = 3.0_wp
        param % alpha_overshoot_size = deg2rad(30.0_wp)
        param % alpha_overshoot_power = 12.0_wp
        param % theta_amplitude = deg2rad(0.0_wp)
        param % theta_power = 10.0_wp
        param % period  = fake_kine_period
    end function fake_param_type_3

    function fake_param_type_4() result(param)
        type(fake_kine_param_t) :: param
        param % phi_amplitude = deg2rad(70.0_wp)
        param % alpha_amplitude = deg2rad(50.0_wp)
        param % alpha_slope = 3.0_wp
        param % alpha_overshoot_size = deg2rad(30.0_wp)
        param % alpha_overshoot_power = 12.0_wp
        param % theta_amplitude = deg2rad(10.0_wp)
        param % theta_power = 10.0_wp
        param % period  = fake_kine_period
    end function fake_param_type_4

end module fqs_fake_kine


c=======================================================================
c
c     subroutine RISKBUDGETIT Index Tracking
c
c     Risk Budgeting allocation strategy (Index Tracking constraints)
c
c     Max[ w'*rho -kappa(1/2*w'*Gamma*w-Covb*w]
c      s.t.
c     sqrt[(w-w_b)'*Gamma_i*(w-w_b)] <= sigma(i)**2-var_bench+w_b'*Gamma_i*w_b  (volatility constraint)
c     C*w <= b                     (linear constraints)
c     Cinf <= w <= Csup            (lower/upper bounds)
c
c        w   : portfolio weights
c        Q   : covariance matrix
c        rho : assets performance
c
c-----------------------------------------------------------------------
      SUBROUTINE riskbudgetit ( n, cov, kappa, rho, covb, varb, neq,
     &                          nin, ccst, bcst, cinf, csup,
     &                          nbc, class, sigma,
     &                          iwork, dwork,
     &                          wopt, lambda, info)
c-----------------------------------------------------------------------
c
c     INPUT
c       n      : portfolio size                                  integer
c       cov    : covariance matrix (n*n)                          double
c       rho    : expected returns vector (n)                      double
c       covb   : covariance assets-index (n)                      double
c       varb   : variance of the benchmark                        double
c       neq    : number equality constraints                     integer
c       nin    : number inequality constraints                   integer
c       ccst   : matrix of constraints (nasset*(neq+nin))         double
c       bcst   : vector initial of constraints (neq+nin)          double
c       cinf   : lower bound (n)                                  double
c       csup   : upper bound (n)                                  double
c       nbc    : number of risk budgeting constraints (>=1)      integer
c       class  : block definition (n)                            integer
c       sigma  : volatilities budgets constraints (nbc)           double
c
c     WORKSPACE
c       iwork  : 3*nbc + 17*n + neq + 2*nin + 12                 integer
c       dwork  : nbc*(nbc+25)/2+n*(13*n+53+2*neq
c                   +2*nin)+6*neq+8*nin+14                        double
c
c     OUTPUT
c       wopt   : optimal portfolio (n)                            double
c       lambda : optimal lambda (nbc)
c       info   : diagnostic argument                             integer
c
c     CALL
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
      EXTERNAL simmvol, gesterr
c
c     arguments i/o
      INTEGER n, info, neq, nin, nbc
      INTEGER class(*)
      DOUBLE PRECISION cov(*), rho(*), covb(*), varb, cinf(*), csup(*)
      DOUBLE PRECISION ccst(n,*), bcst(*), wopt(*), sigma(*), lambda(*)
      DOUBLE PRECISION  kappa
c
c     workspaces
      INTEGER iwork(*)
      DOUBLE PRECISION dwork(*)
c
c     Local Variables
c
      double precision kappa2, scal, scal2, dzero, epsil

      integer pdQ, pdrhoIT, pdbcstIT, pdcinfIT, pdcsupIT, pdsigmaIT
      integer pdomegab, pdw, pdwalloc, pdwoptIT, pdcov, pdcov1

      integer piw, piwalloc

      integer iosort, infot, i, j, npk
      iosort = 6

      pdQ = 1
c     needs n*n
c

      pdomegab = pdQ + n*n
c     needs n
c
      pdrhoIT = pdomegab + n
c     needs n
c      so n*n+2*n
      pdbcstIT = pdrhoIT + n
c     needs neq + nineq
c      so n*n+2*n + neq + nineq
      pdcinfIT = pdbcstIT + neq + nin
c     needs n
c      so n*n+3*n + neq + nineq
      pdcsupIT = pdcinfIT + n
c     needs n
c      so n*n+4*n + neq + nineq
      pdsigmaIT = pdcsupIT + n
c     needs nbc
c      so n*n+4*n + neq + nineq + nbc
      pdwoptIT = pdsigmaIT + nbc
c     needs n
c      so n*n+5*n + neq + nineq + nbc
      pdw = pdwoptIT + n
c     needs n*(4*n+28) + nbc + nineq + neq
c      so    n*n+n*(5*n+33) + 2*(nbc + nineq + neq)
      pdwalloc = pdw + n*(4*n+28) + nbc + nin + neq
c     needs  nbc*(nbc+21)/2 +
c           n*n+n*(8*n+20+2*neq+2*nin)+4*neq+6*nin+14
c
c     so nbc*(nbc+25)/2+n*(14*n+53+2*neq+2*nin)+6*neq+8*nin+14
      pdcov = pdwalloc
c     needs n*n
c
      pdcov1 = pdcov + n*n
c     needs n*n
c

      piw = 1
c     needs 12*n + nbc
c
      piwalloc = piw + 12*n + nbc
c     needs 2*nbc + 5*n + neq + 2*nin + 12
c          so 3*nbc + 17*n + neq + 2*nin + 12
      call buildrbit(n, cov, kappa, rho, covb, varb,
     &   neq, nin, ccst, bcst, cinf, csup, nbc, class, sigma,
     &   iwork(piw), dwork(pdw), dwork(pdomegab), dwork(pdrhoIT),
     &   dwork(pdbcstIT), dwork(pdcinfIT), dwork(pdcsupIT),
     &   dwork(pdsigmaIT), info)
      if (info .LT. 0) then
        return
      endif
      kappa2 = kappa/2

      call ym(n, n, cov, dwork(pdQ))
      call allocrb ( n, cov, dwork(pdQ), kappa2, dwork(pdrhoIT),
     &               neq, nin,
     &               ccst, dwork(pdbcstIT),
     &               dwork(pdcinfIT), dwork(pdcsupIT),
     &               nbc, class, dwork(pdsigmaIT),
     &               iwork(piwalloc), dwork(pdwalloc),
     &               dwork(pdwoptIT), lambda, info)
      call SV(n, dwork(pdwoptIT), dwork(pdomegab), wopt)
      if (info .eq. 1001) then
        info = -101
        return
      endif
c
c     building cov. matrix with optimal dual solution
      DO i = 1,nbc
c
         DO j = 1,n
            iwork(piwalloc + i - 1) = 0
         ENDDO
c         CALL IVX ( n, iwork(piwbf), ZERO ) ! initialize piwbf
c
c        dimension of block i
         epsil = 1.e-8
         dzero = 0.
         npk = 0
         DO j = 1,n
            IF (class(j) .EQ. i) THEN
               npk = npk + 1
               iwork(piwalloc + npk - 1) = j
            ENDIF
         ENDDO
         IF (npk .GT. 1) THEN
c
c           initialize temporary cov. matrix
            CALL IMX ( npk, npk, dwork(pdcov1), dzero)
            CALL IMX ( n, n, dwork(pdcov), dzero)
c
c           extract block i
            CALL YMCPI(n, cov, npk, iwork(piwalloc), dwork(pdcov1)
     &     , infot)
c
c           sub-block -> cov. matrix
            CALL YMCPIR (npk, dwork(pdcov1), n, iwork(piwalloc),
     &                   dwork(pdcov), infot)
c
c           optimal portfolio variance: w'*Cov*w
            CALL OVTMCV(n, cov,  dwork(pdwoptIT), scal)
            IF (scal .GT. (dwork(pdsigmaIT+i-1)**2+epsil)) THEN
                info = -120
                RETURN
            ENDIF
            CALL OVTMCV(n, cov,  wopt, scal)
            CALL XV(n, wopt, covb, scal2)
            IF (scal .gt. (sigma(i)**2+2*scal2-varb+epsil)) THEN
                info = -120
                RETURN
            ENDIF
         ENDIF
      ENDDO
      RETURN
      END


       subroutine dery_1(q0, q1)

!----------------------------------------------------------- !
!               F. Califano, 2006                            !
!  First y-derivative by FFT (periodic boundary conditions)  !
!----------------------------------------------------------- !

!**************************************************
!  MPI PARALLEL VERSION: VALENTINI-FAGANELLO 2009
!**************************************************

use parameter_mod
use box_mod
use deriv_mod

IMPLICIT NONE

integer :: ik, il

REAL(dp),DIMENSION (ny) :: q0, q1, qq

    qq = q0

    call drfftf(ny, qq, savey)

       il = 1
          do ik = 2, ny1, 2
            q1(ik)   = - qq(ik+1) * ky_1(il)
            q1(ik+1) =   qq(ik)   * ky_1(il)
           il = il + 1
          enddo

            q1(1)  = 0.0
            q1(ny) = 0.0

    call drfftb(ny, q1, savey)

end subroutine


!-------------------------------------------------------------------------------------------------
! Fortran source code for module et_ppmd.coref90
!-------------------------------------------------------------------------------------------------
! Remarks:
!   . Enter Python documentation for this module in ``./coref90.rst``.
!     You might want to check the f2py output for the interfaces of the C-wrapper functions.
!     It will be autmatically included in the et_ppmd documentation.
!   . Documument the Fortran routines in this file. This documentation will not be included
!     in the et_ppmd documentation (because there is no recent sphinx
!     extension for modern fortran.

! VERBOSE is for debugging purposes
!#define VERBOSE
! preprocessor directives must start at the beginning of the line in Fortran code

!-------------------------------------------------------------------------------------------------
function force_factor(rij2)
  ! Compute the force factor. Multiply with the interatomic vector to obtain the force vector.
    implicit none
  !-------------------------------------------------------------------------------------------------
  ! subprogram parameters
    real*8 ,intent(in) ::rij2           ! interatomic distance squqred
    real*8             ::force_factor   ! result
  !-------------------------------------------------------------------------------------------------
  ! local variables
    real*8 :: rm2, rm6
  !-------------------------------------------------------------------------------------------------
    rm2 = 1.0/rij2
    rm6 = (rm2*rm2*rm2)
    force_factor = (1.0 - 2.0*rm6)*rm6*rm2*6.0
endfunction

!-------------------------------------------------------------------------------------------------
subroutine computeForces(x,y,vlsz,vlst,fx,fy,n,m)
  ! Compute the forces from the atom positions (x,y) and the Verlet list vl.
  ! See the VerletList class in et_ppmd/verlet.py for an explanation of the
  ! data structure (a 2D integer array.
  !
    implicit none
  !-------------------------------------------------------------------------------------------------
  ! subprogram parameters
    integer*4              , intent(in)    :: n     ! number of atoms
    integer*4              , intent(in)    :: m     ! length of the linearized verlet list = total number of pairs
    real*8   , dimension(n), intent(in)    :: x,y   ! atom positions
    integer*8, dimension(n), intent(in)    :: vlsz  ! size of the individual Verlet lists
    integer*8, dimension(m), intent(in)    :: vlst  ! linearized Verlet lists
    real*8   , dimension(n), intent(inout) :: fx,fy ! atom forces
    ! intent is inout because we do not want to return an array to avoid needless copying
  !-------------------------------------------------------------------------------------------------
  ! local variables
    integer*4 :: i,nb0,nb,j
    integer*4 :: n_neighbours_i
    real*8    :: xij,yij,rij2,ff,ffx,ffy,force_factor
  !------------------------------------------------------------------------------------------------
  ! Zero the forces
    do i=1,n
        fx(i) = 0.0
    end do
    do i=1,n
        fy(i) = 0.0
    end do
#if defined VERBOSE
    do i=1,m
        write (*,*) vlst(i)
    end do
#endif
  ! Compute the interatomic forces and add them to fx and fy
    nb0 = 0 ! offset of verlet list of atom i in the linearized verlet list
  ! Note that Fortran indexes arrays counting from 1 to n, whereas Python, C and C++
  ! count from 0 to n-1.
    do i=1,n
#if defined VERBOSE
        write (*,*) 'coref90:','nb0=',nb0
#endif
        n_neighbours_i = vlsz(i)
        do nb = nb0+1, nb0+n_neighbours_i
#if defined VERBOSE
            write (*,*) 'coref90:','nb=',nb
#endif
            j = vlst(nb) + 1 ! + 1 since Fortran starts counting at 1, and the python atom indices are 0-based
#if defined VERBOSE
            write (*,*) 'coref90:',i,nb,j
#endif
            xij = x(j) - x(i)
            yij = y(j) - y(i)
            rij2 = xij*xij + yij*yij
            ff = force_factor(rij2)
            ffx = ff*xij
            ffy = ff*yij
            fx(i) = fx(i) + ffx
            fy(i) = fy(i) + ffy
            fx(j) = fx(j) - ffx
            fy(j) = fy(j) - ffy
        end do
        nb0 = nb0 + n_neighbours_i
    end do
end subroutine computeForces

!-------------------------------------------------------------------------------------------------
subroutine velocity_verlet_12(dt,rx,ry,vx,vy,ax,ay,vx_midstep,vy_midstep,n)
  ! Compute the forces from the atom positions (x,y) and the Verlet list vl.
  ! See the VerletList class in et_ppmd/verlet.py for an explanation of the
  ! data structure (a 2D integer array.
  !
    implicit none
  !-------------------------------------------------------------------------------------------------
  ! subprogram parameters
    real*8                 , intent(in)    :: dt    ! timestep
    integer*4              , intent(in)    :: n     ! number of atoms
    real*8   , dimension(n), intent(inout) :: rx,ry ! atom positions
    real*8   , dimension(n), intent(in)    :: vx,vy ! atom velocities
    real*8   , dimension(n), intent(in)    :: ax,ay ! atom accelerations
    real*8   , dimension(n), intent(inout) :: vx_midstep,vy_midstep ! midstep atom velocities
  !-------------------------------------------------------------------------------------------------
  ! local variables
    integer*4 :: i
    real*8    :: halfstep
  !------------------------------------------------------------------------------------------------
  ! Step 1: compute velocities at midstep (t+dt/2) using the current accelerations:
  !    self.vx_midstep = self.vx + (0.5*dt)*self.ax
  !    self.vy_midstep = self.vy + (0.5*dt)*self.ay
  ! Step 2: compute positions at next step (t+dt) using the midstep velocities:
  !     self.rx += self.vx_midstep*dt
  !     self.ry += self.vy_midstep*dt
  ! first all work on x components, then on y-components

    halfstep = 0.5*dt;

    do i=1,n
        vx_midstep(i) = vx(i) + halfstep*ax(i)
        rx(i) = rx(i)+ vx_midstep(i) * dt
    end do

    do i=1,n
        vy_midstep(i) = vy(i) + halfstep*ay(i)
        ry(i) = ry(i) + vy_midstep(i) * dt
    end do

end subroutine velocity_verlet_12

!-------------------------------------------------------------------------------------------------
subroutine velocity_verlet_4(dt,rx,ry,vx,vy,ax,ay,vx_midstep,vy_midstep,n)
  ! Compute the forces from the atom positions (x,y) and the Verlet list vl.
  ! See the VerletList class in et_ppmd/verlet.py for an explanation of the
  ! data structure (a 2D integer array.
  !
    implicit none
  !-------------------------------------------------------------------------------------------------
  ! subprogram parameters
    real*8                 , intent(in)    :: dt    ! timestep
    integer*4              , intent(in)    :: n     ! number of atoms
    real*8   , dimension(n), intent(in)    :: rx,ry ! atom positions
    real*8   , dimension(n), intent(inout) :: vx,vy ! atom velocities
    real*8   , dimension(n), intent(in)    :: ax,ay ! atom accelerations
    real*8   , dimension(n), intent(in)    :: vx_midstep,vy_midstep ! midstep atom velocities
  !-------------------------------------------------------------------------------------------------
  ! local variables
    integer*4 :: i
    real*8    :: halfstep
  !------------------------------------------------------------------------------------------------
  ! Step 4: compute velocities at next step (t+dt)
  ! 	self.vx = self.vx_midstep + self.ax * (0.5*dt)
  ! 	self.vy = self.vy_midstep + self.ay * (0.5*dt)

    halfstep = 0.5*dt

    do i=1,n
        vx(i) = vx_midstep(i) + halfstep*ax(i)
    end do

    do i=1,n
        vy(i) = vy_midstep(i) + halfstep*ay(i);
    end do

end subroutine velocity_verlet_4


! MODULE PRANA
! Generate a random proper rotation (orthogonal) matrix.
!
! The procedure is to fill a square matrix of desired order with independent
! Gaussian random numbers, then to apply Gram-Schmidt orthonormalization.
! The "4.... goto 4" loop allows for reiteration in the very rare case that
! the initial random matrix is (to within round-off) essentially singular.
! The final `coin toss' randomization of orientation of each column along
! the line it lies along overcomes what would otherwise be a bias that
! comes from the way the gram-schmidt code works. The even number of reversals
! ensures the preservation of the determinant.
!=============================================================================
module prana
!=============================================================================
use pran; use peuc
implicit none
private
public:: ranrot
interface ranrot; module procedure ranrot_s,ranrot_d; end interface

contains

!=============================================================================
subroutine ranrot_s(a)
!=============================================================================
real(4),dimension(:,:),intent(OUT)    :: a
!-----------------------------------------------------------------------------
integer                            :: n,i,j,nrank,flip
real(4),dimension(size(a,1),size(a,1)):: as
real(4)                               :: deter,xran
!=============================================================================
n=size(a,1); if(size(a,2)/=n)stop 'In ranrot; incompatible dimensions'
4 do j=1,n; do i=1,n; call gauss(as(i,j)); enddo; enddo
print'(t5,10f7.4)',as
call gram(as,a,nrank,deter); if(nrank/=n)goto 4
print *, 'nrank=',nrank
print'(t5,10f7.4)',a
flip=1; do j=1,n-1
call uniform(xran);if(xran>.5)then; flip=-flip; a(:,j)=-a(:,j); endif
enddo; a(:,n)=flip*a(:,n)
print'(t5,10f7.4)',a
end subroutine ranrot_s

!=============================================================================
subroutine ranrot_d(a)
!=============================================================================
real(8),dimension(:,:),intent(OUT)    :: a
!-----------------------------------------------------------------------------
integer                               :: n,i,j,nrank,flip,k
real(8),dimension(size(a,1),size(a,1)):: as
real(8)                               :: deter,xran
!=============================================================================
n=size(a,1); if(size(a,2)/=n)stop 'In ranrot; incompatible dimensions'
4 do j=1,n; do i=1,n; call gauss(as(i,j)); enddo; enddo
!a=0.0
!do j=1,n; do i=1,n; do k=1,n; a(i,j)=a(i,j)+as(k,i)*as(k,j); enddo; enddo; enddo
!print *,'raw'
!print'(t5,10f7.4)',as
!print *,'raw variance)'
!print'(t5,10f7.4)',a
!a=0.0
call gram(as,a,nrank,deter); if(nrank/=n)goto 4
!as=0.0
!do j=1,n; do i=1,n; do k=1,n; as(i,j)=as(i,j)+a(k,i)*a(k,j); enddo; enddo; enddo
!print *, 'nrank=',nrank
!print *,'after gram'
!print'(t5,10f7.4)',a
!print *,'after gram variance)'
!print'(t5,10f7.4)',as
!flip=1; do j=1,n-1
!call uniform(xran);if(xran>.5d0)then; flip=-flip; a(:,j)=-a(:,j); endif
!enddo; a(:,n)=flip*a(:,n)
!as=0.0
!do j=1,n; do i=1,n; do k=1,n; as(i,j)=as(i,j)+a(k,i)*a(k,j); enddo; enddo; enddo
!print *,'after flipping'
!print'(t5,10f7.4)',a
!print *,'after gram variance)'
!print'(t5,10f7.4)',as
end subroutine ranrot_d

end module prana


subroutine calc_distance (np, nd, pos, i, j, rij, d, d2)
!
!  Calculate Distance vector, scalar distance and trucated distance
!  between atoms i and j.
!  The distance is truncated at pi/2
!
    implicit none
    integer(kind=4),intent(in) :: np                ! number of particles
    integer(kind=4),intent(in) :: nd                ! number of dimensions
    real(kind=8),intent(in),dimension(nd,np) :: pos ! positions
    integer(kind=4),intent(in) :: i                 ! index particle I
    integer(kind=4),intent(in) :: j                 ! index particle J
    real(kind=8),intent(out),dimension(nd) :: rij   ! distance vector
    real(kind=8),intent(out) :: d                   ! distance
    real(kind=8),intent(out) :: d2                  ! trucated distance

    real ( kind = 8 ), parameter :: PI2 = 3.141592653589793D+00 / 2.0D+00

    rij(1:nd) = pos(1:nd,i) - pos(1:nd,j)

    d = sqrt ( sum ( rij(1:nd)**2 ) )

    !  Truncate the distance:
    d2 = min ( d, PI2 )
end subroutine calc_distance


C     # This returns a unit square map in [-1,1]x[-1,1]
      subroutine mapc2m_fivepatch(blockno,xc,yc,xp,yp,zp,alpha)
      implicit none

      integer blockno
      double precision xc,yc,xp,yp,zp,alpha, xc_in, yc_in
      double precision xc1, yc1

      if (blockno .eq. 2) then
         xp = (2*xc-1)*alpha
         yp = (2*yc-1)*alpha
      else
         if (blockno .eq. 0) then
            xc1 = xc
            yc1 = 1-yc
            call bilinear_help(alpha,xc1,yc1,xp,yp)
            yp = -yp
         elseif (blockno .eq. 1) then
            xc1 = yc
            yc1 = 1 -xc
            call bilinear_help(alpha,xc1,yc1,yp,xp)
            xp = -xp
         elseif (blockno .eq. 3) then
            xc1 = yc
            yc1 = xc
            call bilinear_help(alpha,xc1,yc1,yp,xp)
         elseif (blockno .eq. 4) then
            xc1 = xc
            yc1 = yc
            call bilinear_help(alpha,xc1,yc1,xp,yp)
         else
            write(6,'(A,I5)') 'mapc2m_fivepatch.f : blockno = ', blockno
            stop
         endif
      endif
      zp = 0
      end

      subroutine bilinear_help(alpha,xi,eta,xp,yp)
      implicit none

      double precision alpha, xi, eta, xp,yp

      double precision corner(4,2)
      double precision a(2),u1(2),v1(2),v2(2), pt(2)
      integer m

      corner(1,1) = -alpha
      corner(4,1) = alpha
      corner(3,1) = 1
      corner(2,1) = -1

      corner(1,2) = alpha
      corner(4,2) = alpha
      corner(3,2) = 1
      corner(2,2) = 1

      do m = 1,2
         a(m)  = corner(1,m)
         u1(m) = corner(4,m) - corner(1,m)
         v1(m) = corner(2,m) - corner(1,m)
         v2(m) = corner(3,m) - corner(4,m)
      enddo

      do m = 1,2
         pt(m) = a(m) + u1(m)*xi + v1(m)*eta + (v2(m)-v1(m))*xi*eta
      enddo

      xp = pt(1)
      yp = pt(2)

      end