InvertMatrix Subroutine

  subroutine InvertMatrix(A,Ainv,n)
    !! Invert the n-by-n matrix A by Gauss-Jordan elimination with partial pivoting, in double
    !! precision. Used once at initialization for the small (N+1) Legendre Vandermonde; A is a
    !! well-conditioned Vandermonde in an orthonormal basis, so a direct solve is appropriate.
    implicit none
    integer,intent(in) :: n
    real(real64),intent(in) :: A(1:n,1:n)
    real(real64),intent(out) :: Ainv(1:n,1:n)
    ! Local
    real(real64) :: M(1:n,1:n)
    integer :: i,j,k,piv
    real(real64) :: pmax,factor,tmp

    M = A
    Ainv = 0.0_real64
    do i = 1,n
      Ainv(i,i) = 1.0_real64
    enddo

    do k = 1,n
      ! Partial pivot: find the largest-magnitude entry in column k at or below the diagonal.
      piv = k
      pmax = abs(M(k,k))
      do i = k+1,n
        if(abs(M(i,k)) > pmax) then
          pmax = abs(M(i,k))
          piv = i
        endif
      enddo
      if(pmax <= 0.0_real64) then
        print*,__FILE__,':',__LINE__,' : Error : singular Vandermonde in modal transform.'
        stop 1
      endif
      if(piv /= k) then
        do j = 1,n
          tmp = M(k,j); M(k,j) = M(piv,j); M(piv,j) = tmp
          tmp = Ainv(k,j); Ainv(k,j) = Ainv(piv,j); Ainv(piv,j) = tmp
        enddo
      endif

      ! Normalize the pivot row.
      factor = M(k,k)
      do j = 1,n
        M(k,j) = M(k,j)/factor
        Ainv(k,j) = Ainv(k,j)/factor
      enddo

      ! Eliminate column k from every other row.
      do i = 1,n
        if(i /= k) then
          factor = M(i,k)
          do j = 1,n
            M(i,j) = M(i,j)-factor*M(k,j)
            Ainv(i,j) = Ainv(i,j)-factor*Ainv(k,j)
          enddo
        endif
      enddo
    enddo

  endsubroutine InvertMatrix