!     Generate .gr3 (e.g. shapiro.gr3)  using bottom slope as a criterion.
!     Works for mixed tri/quads
!     Inputs: hgrid.gr3 (not in lon/lat!); consts below
!     Output: slope_filter.gr3

!     ifort -O2 -CB -o gen_slope_filter gen_slope_filter.f90

      implicit real*8(a-h,o-z)
      !parameter(mnp=1000000)
      !parameter(mne=2000000)
      parameter(mnei=50)

      integer,allocatable :: i34(:),elnode(:,:),nwild(:)
      real*8,allocatable :: x(:),y(:),dp(:),area(:),dldxy(:,:)
      real*8,allocatable :: slope(:),hdif(:),hdif_e(:),rlh(:)
      integer,allocatable :: nne(:),indel(:,:)

      !Formula: depth=hdif_max*tanh(2*gam/threshold_slope), where gam is
      !slope
      hdif_max=0.5 !max

      print*, 'Input ref slope:'
      read*, threshold_slope

      open(14,file='hgrid.gr3',status='old')
      open(13,file='slope_filter.gr3',status='replace')
      read(14,*)
      read(14,*) ne,np
      !if(ne>mne.or.np>mnp) then
      !  write(*,*)'Increase mne/mnp',mne,mnp,ne,np
      !  stop
      !endif
      allocate(i34(ne),elnode(4,ne),nwild(3), &
             & x(np),y(np),dp(np),area(ne),dldxy(2,3), &
             & slope(ne),hdif(np),hdif_e(ne),rlh(4), &
             & nne(np),indel(mnei,np),stat=istat)

      do i=1,np
        read(14,*) j,x(i),y(i),dp(i)
      enddo !i=1,np

      slope=0 !init
      do i=1,ne
        read(14,*) j,i34(i),elnode(1:i34(i),i)

        do m=1,i34(i)-2 !split into tri
          if(m==1) then
            nwild(1:3)=(/1,2,3/)
          else !quad
            nwild(1:3)=(/1,3,4/)
          endif !m

          n1=elnode(nwild(1),i)
          n2=elnode(nwild(2),i)
          n3=elnode(nwild(3),i)
          area(i)=signa(x(n1),x(n2),x(n3),y(n1),y(n2),y(n3))
          if(area(i)<=0) then
            write(*,*)'Negative area at',i
            stop
          endif

!        if(i34(i)==4) then
!          n4=elnode(4,i)
!          area(i)=area(i)+signa(x(n1),x(n3),x(n4),y(n1),y(n3),y(n4))
!        endif

          do j=1,3
            nj1=j+1
            nj2=j+2
            if(nj1>3) nj1=nj1-3
            if(nj2>3) nj2=nj2-3
            nd1=elnode(nwild(nj1),i)
            nd2=elnode(nwild(nj2),i)
            dldxy(1,j)=(y(nd1)-y(nd2))/2/area(i)
            dldxy(2,j)=(x(nd2)-x(nd1))/2/area(i)
          enddo !j
          slx=dot_product(dp(elnode(nwild(1:3),i)),dldxy(1,:))
          sly=dot_product(dp(elnode(nwild(1:3),i)),dldxy(2,:))
          slope(i)=max(slope(i),sqrt(slx**2+sly**2))
        enddo !m

        write(99,*)i,slope(i)
      enddo !i=1,ne

!     Neighborhood
      nne=0
      do i=1,ne
        do j=1,i34(i)
          nd=elnode(j,i)
          nne(nd)=nne(nd)+1
          if(nne(nd)>mnei) then
            write(*,*)'Too many neighbors',nd
            stop
          endif
          indel(nne(nd),nd)=i
        enddo
      enddo

      hdif=0
      do i=1,np
        slopemax=0
        do j=1,nne(i)
          ie=indel(j,i)
          slopemax=max(slopemax,slope(ie))
        enddo !j
        hdif(i)=hdif_max*tanh(2*slopemax/threshold_slope)
      enddo !i

      write(13,*)'threshold_slope=',threshold_slope
      write(13,*)ne,np
      do i=1,np
        write(13,*)i,real(x(i)),real(y(i)),real(hdif(i))
      enddo !i
      do i=1,ne
        write(13,*) i,i34(i),elnode(1:i34(i),i)
      enddo !i

      print*, 'Max bottom slope=',maxval(slope(1:ne))
      print*, 'Min/max = ',minval(hdif(1:np)),maxval(hdif(1:np))

      stop
      end

      function signa(x1,x2,x3,y1,y2,y3)
!...  Compute signed area formed by pts 1,2,3
      implicit real*8(a-h,o-z)

      signa=((x1-x3)*(y2-y3)-(x2-x3)*(y1-y3))/2

      return
      end


!file: coord_desc.f90
subroutine coord_desc(converged, prec, corr, latent, weight, & !outputs
                      observation, dims_pop, & !inputs :: data
                      lambda_ridge, lambda_glasso, & !inputs :: parameters
                      ths, max_iter, & !inputs, optional :: opt. parameters
                      prec_init, weight_init,& !inputs, optional :: initial parameters
                      verbose, verb_period, & !inputs, optional :: etc.
                      num_pop, dim_obs, num_trial) !inputs :: dimensions

    implicit none

    !inputs :: dimensions
    integer, intent(in) :: num_pop, dim_obs, num_trial

    !inputs :: data
    real(8), dimension(dim_obs, num_trial), intent(in) :: observation
    integer, dimension(num_pop), intent(in) :: dims_pop

    !inputs :: parameters
    real(8), dimension(num_pop, num_pop), intent(in) :: lambda_glasso
    real(8), dimension(num_pop), intent(in) :: lambda_ridge

    !inputs, optional :: opt.parameters
    integer, intent(in) :: max_iter
    real(8), intent(in) :: ths

!f2py integer optional, intent(in) :: max_iter = 10000
!f2py real(8) optional, intent(in) :: ths = 0.0001

    !inputs, optional :: initial parameters
    real(8), dimension(num_pop, num_pop), intent(in) :: prec_init
    real(8), dimension(dim_obs), intent(in) :: weight_init

!f2py real(8) optional, dimension(num_pop, num_pop), intent(in) :: prec_init = 0
!f2py real(8) optional, dimension(dim_obs), intent(in) :: weight_init = 1

    !inputs, optional :: etc.
    logical, intent(in) :: verbose
    integer, intent(in) :: verb_period

!f2py integer optional, intent(in) :: verbose = 0
!f2py integer optional, intent(in) :: verb_period = 10

    !outputs
    logical, intent(out) :: converged
    real(8), dimension(num_pop, num_pop), intent(out) :: prec, corr
    real(8), dimension(num_pop, num_trial), intent(out) :: latent
    real(8), dimension(dim_obs), intent(out) :: weight

    !local variables
    integer :: i, info
    real(8) :: cost, cost_old, diff
    real(8), dimension(num_pop) :: w
    real(8), dimension(num_pop, num_pop) :: prec_copy
    real(8), dimension(max(1, 3*num_pop-1)) :: work_eigen

    external DSYEV

    if (sum(dims_pop) /= dim_obs) then
        stop 'Dimensions of population does not match with observation'
    end if

    call coord_desc_init(prec, corr, latent, weight, & !outputs
                         observation, dims_pop, & !inputs :: data
                         lambda_ridge, prec_init, weight_init, & !inputs :: parameters
                         num_pop, dim_obs, num_trial)

    !initial setting
    cost_old = huge(1.)
    converged = .false.

    !iteration
    do i = 1, max_iter
        call coord_desc_iter(prec, corr, latent, weight, &
                             observation, dims_pop, lambda_ridge, lambda_glasso, &
                             num_pop, dim_obs, num_trial)

        ! calculate cost
        prec_copy = prec

        call DSYEV('N', 'U', num_pop, prec_copy, num_pop, w, work_eigen, &
                    max(3*num_pop-1, 1), info)

        if ( w(1) <= 0 .or. product(w) <= 0 ) then
            if (verbose .and. mod(i, verb_period) == 0) then
                print *, 'Iteration ', i, ', Precision is not positive definite!'
            end if
            cost_old = huge(1.)
        else
            cost = - log(product(w)) + sum(prec * corr) + &
                   sum(max(lambda_glasso, 0.0) * abs(prec))
            diff = cost_old - cost

            if (verbose .and. mod(i, verb_period) == 0) then
                print *, 'Iteration ', i, ', cost: ', cost, ', difference: ', diff
            end if

            if (abs(diff) < ths) then
                converged = .true.
                exit
            end if

            cost_old = cost
        end if
    end do

end subroutine coord_desc


module matrixMath

USE IntrinsicTypesModule, RK=>REAL_KIND
USE units_mod

IMPLICIT NONE

CONTAINS

SUBROUTINE mat_mult_const_mat(a, b, c)

    IMPLICIT NONE

    REAL(RK), INTENT(IN) :: a(:, :, :), b(:, :)
    REAL(RK), INTENT(OUT):: c(:, :, :)

    INTEGER :: i, j, k

    INTEGER lda, ldb, ldc, lta, ltb, n
!
!----------------------------------------------------------------------

    lda = ubound(a, 2)
    ldb = ubound(b, 1)
    lta = ubound(a, 3)
    ltb = ubound(b, 2)

    c = 0.0_RK

    do k = 1,ltb
      do j = 1,ldb
         do i = 1,lda
            c(:, i, k) = c(:, i, k) + a(:, i, j)*b(j, k)
         end do
      end do
    end do

END SUBROUTINE mat_mult_const_mat

SUBROUTINE mat_const_mult_mat(a, b, c)

    IMPLICIT NONE

    REAL(RK), INTENT(IN) :: a(:, :), b(:, :, :)
    REAL(RK), INTENT(OUT):: c(:, :, :)

    INTEGER :: i, j, k

    INTEGER lda, ldb, lta, ltb
!
!----------------------------------------------------------------------

    lda = ubound(a, 1)
    ldb = ubound(b, 2)
    lta = ubound(a, 2)
    ltb = ubound(b, 3)

    c = 0.0_RK

    do k = 1,ltb
      do j = 1,ldb
         do i = 1,lda
            c(:, i, k) = c(:, i, k) + a(i, j)*b(:, j, k)
         end do
      end do
    end do

END SUBROUTINE mat_const_mult_mat

SUBROUTINE matrix_mult(a, b, c)

    IMPLICIT NONE

    REAL(RK), INTENT(IN) :: a(:, :, :), b(:, :, :)
    REAL(RK), INTENT(OUT):: c(:, :, :)

    INTEGER :: i, j, k

    INTEGER lda, ldb, lta, ltb
!
!----------------------------------------------------------------------

    lda = ubound(a, 2)
    ldb = ubound(b, 2)
    lta = ubound(a, 3)
    ltb = ubound(b, 3)

    c = 0.0_RK

    do k = 1,ltb
      do j = 1,ldb
         do i = 1,lda
            c(:, i, k) = c(:, i, k) + a(:, i, j)*b(:, j, k)
         end do
      end do
    end do

END SUBROUTINE matrix_mult

SUBROUTINE mat_const_mult_vec(a, b, c)

    IMPLICIT NONE

    REAL(RK), INTENT(IN) :: a(:, :), b(:, :)
    REAL(RK), INTENT(OUT):: c(:, :)

    INTEGER :: j, k

    INTEGER lda, ldb , lta, ltb
!
!----------------------------------------------------------------------

    lda = ubound(a, 1)
    ldb = ubound(b, 1)
    lta = ubound(a, 2)
    ltb = ubound(b, 2)

    c = 0.0_RK

    do k = 1,ltb
      do j = 1,lda
        c(:, j) = c(:, j) + a(j, k)*b(:, k)
      end do
    end do

END SUBROUTINE mat_const_mult_vec

SUBROUTINE vec_dot_prod(a, b, c)

    IMPLICIT NONE
    REAL(RK), INTENT(IN) :: a(:, :), b(:, :)
    REAL(RK), INTENT(OUT) :: c(:)

    INTEGER :: i, lta
    lta = ubound(a, 2)

    c = 0.0_RK

    do i = 1,lta
        c(:) = c(:) + a(:, i)*b(:, i)
    end do

END SUBROUTINE vec_dot_prod

SUBROUTINE ltwonorm(vec, norm)

    IMPLICIT NONE
    REAL(RK), INTENT(IN) :: vec(:)
    REAL(RK), INTENT(OUT) :: norm

    REAL(RK), ALLOCATABLE :: svec(:)
    INTEGER :: i, lta

    lta = ubound(vec,1)
    ALLOCATE(svec(1:lta))

    svec = vec*vec
    norm = 0.0_RK

    do i = 1, lta
        norm = norm + sqrt(svec(i))
    end do

END SUBROUTINE ltwonorm

SUBROUTINE getskewmat(mat, skew, dim)

    IMPLICIT NONE
    INTEGER, INTENT(IN) :: dim
    REAL(RK), INTENT(IN) :: mat(0:dim, 0:DIMS1, 0:DIMS1)
    REAL(RK), INTENT(OUT) :: skew(0:dim, 0:DIMS1, 0:DIMS1)

    skew = 0.0_RK

    skew(:, 0, 1) = 0.5_RK * (mat(:, 0, 1) - mat(:, 1, 0))
    skew(:, 0, 2) = 0.5_RK * (mat(:, 0, 2) - mat(:, 2, 0))
    skew(:, 1, 2) = 0.5_RK * (mat(:, 1, 2) - mat(:, 2, 1))

    skew(:, 1, 0) = - skew(:, 0, 1)
    skew(:, 2, 0) = - skew(:, 0, 2)
    skew(:, 2, 1) = - skew(:, 1, 2)

END SUBROUTINE getskewmat

SUBROUTINE getsymmat(mat, sym, dim)

    IMPLICIT NONE
    INTEGER, INTENT(IN) :: dim
    REAL(RK), INTENT(IN) :: mat(0:dim, 0:DIMS1, 0:DIMS1)
    REAL(RK), INTENT(OUT) :: sym(0:dim, 0:DIMS1, 0:DIMS1)

    sym = 0.0_RK

    sym(:, 0, 0) = mat(:, 0, 0)
    sym(:, 1, 1) = mat(:, 1, 1)
    sym(:, 2, 2) = mat(:, 2, 2)

    sym(:, 0, 1) = 0.5_RK * (mat(:, 0, 1) + mat(:, 1, 0))
    sym(:, 0, 2) = 0.5_RK * (mat(:, 0, 2) + mat(:, 2, 0))
    sym(:, 1, 2) = 0.5_RK * (mat(:, 1, 2) + mat(:, 2, 1))

    sym(:, 1, 0) = sym(:, 0, 1)
    sym(:, 2, 0) = sym(:, 0, 2)
    sym(:, 2, 1) = sym(:, 1, 2)

END SUBROUTINE getsymmat

SUBROUTINE getDevMat(mat, dev)

    IMPLICIT NONE

    REAL(RK), INTENT(IN) :: mat(0:nelem1, 0:DIMS1, 0:DIMS1)
    REAL(RK), INTENT(OUT) :: dev(0:nelem1, 0:DIMS1, 0:DIMS1)

    REAL(RK) :: trVal(0:nelem1)

    trVal = 1.0_RK/3.0_RK * (mat(:, 0, 0)+mat(:, 1, 1)+mat(:, 2, 2))

    dev(:, 0, 0) = mat(:, 0, 0) - trVal
    dev(:, 1, 1) = mat(:, 1, 1) - trVal
    dev(:, 2, 2) = mat(:, 2, 2) - trVal

    dev(:, 0, 1) = mat(:, 0, 1)
    dev(:, 0, 2) = mat(:, 0, 2)
    dev(:, 1, 2) = mat(:, 1, 2)

    dev(:, 1, 0) = mat(:, 1, 0)
    dev(:, 2, 0) = mat(:, 2, 0)
    dev(:, 2, 1) = mat(:, 2, 1)

END SUBROUTINE getDevMat

SUBROUTINE invert4x4(m, invOut)
    !Took this from a stackoverflow topic on how to code a inverted 4x4 matrix
    IMPLICIT NONE

    REAL(RK), INTENT(IN)  :: m(0:15)
    REAL(RK), INTENT(OUT) :: invOut(0:15)
    REAL(RK) :: inv(0:15)
    REAL(RK) :: det, idet

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

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

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

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

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

    inv(5) =  m(0) * m(10) * m(15) - m(0) * m(11) * m(14) - m(8)&
            &* m(2) * m(15) + m(8) * m(3) * m(14) + m(12) * m(2) * m(11) - m(12) * m(3) * m(10)

    inv(9) = -m(0) * m(9) * m(15) + m(0) * m(11) * m(13) + m(8)&
            &* m(1) * m(15) - m(8) * m(3) * m(13) - m(12) * m(1) * m(11) + m(12) * m(3) * m(9)

    inv(13) =  m(0) * m(9) * m(14) - m(0) * m(10) * m(13) - m(8)&
            &* m(1) * m(14) + m(8) * m(2) * m(13) + m(12) * m(1) * m(10) - m(12) * m(2) * m(9)

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

    inv(6) = -m(0) * m(6) * m(15) + m(0) * m(7) * m(14) + m(4)&
            &* m(2) * m(15) - m(4) * m(3) * m(14) - m(12) * m(2) * m(7) + m(12) * m(3) * m(6)

    inv(10) =  m(0) * m(5) * m(15) - m(0) * m(7) * m(13) - m(4)&
            &* m(1) * m(15) + m(4) * m(3) * m(13) + m(12) * m(1) * m(7) - m(12) * m(3) * m(5)

    inv(14) = -m(0) * m(5) * m(14) + m(0) * m(6) * m(13) + m(4)&
            &* m(1) * m(14) - m(4) * m(2) * m(13) - m(12) * m(1) * m(6) + m(12) * m(2) * m(5)

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

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

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

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

    det = m(0) * inv(0) + m(1) * inv(4) + m(2) * inv(8) + m(3) * inv(12);

    idet = 1.0_RK/det;

    invOut(:) = inv(:) * idet;

END SUBROUTINE invert4x4

SUBROUTINE solve4x4(m, v, x)

    !We are going to be using this for finding the barycentric coordinates of a tetrahedron
    !This can then be used to tell us if a point is inside a tetrahedron or not. It can also
    !be used to linearly interpolate scalar values out to the coords of the tetrahedron.

    IMPLICIT NONE

    REAL(RK), INTENT(IN)  :: m(4,4), v(4)
    REAL(RK), INTENT(OUT) :: x(4)

    REAL(RK) :: inv_m(4,4)
    !Just giving a rough parameter for machine precision
    REAL(RK), PARAMETER :: mach_eps = 2.2e-16

    call invert4x4(m, inv_m)

    x = matmul(inv_m, v)

    !If under machine precision we're just going to set the value to zero
    !This should also help us deal with points that lie on an edge or point of the
    !tetrahedron
    where(abs(x) .LE. mach_eps) x = 0.0_RK

END SUBROUTINE solve4x4

end module matrixMath


      subroutine sfncge(delxi,xval0,xtargv,dxval0,nord,nrd1mx,resx)
c
c     This subroutine computes a general search function for
c     EQ6/search.f. This search function is a residual defined as the
c     difference between the calculated value of a function (xvalc)
c     described by a truncated Taylor's series and a target value
c     (xtargv).
c
c     This subroutine is called by:
c
c       EQ6/search.f
c
c-----------------------------------------------------------------------
c
c     Principal input:
c
c       delxi  = step size
c       xtargv = target value
c       xval0  = the function value at delxi = 0
c       dxval0 = the derivatives of this function at delxi = 0
c       nord   = the order of the truncated Taylor's series
c
c     Principal output:
c
c       resx   = value of residual function
c
c-----------------------------------------------------------------------
c
      implicit none
c
c-----------------------------------------------------------------------
c
c     Calling sequence variable declarations.
c
      integer nrd1mx
c
      integer nord
c
      real*8 dxval0(nrd1mx)
c
      real*8 delxi,resx,xtargv,xval0
c
      real*8 fctrl
c
c-----------------------------------------------------------------------
c
c     Local variable declarations.
c
      integer n
c
      real*8 dxp,xvalc
c
c-----------------------------------------------------------------------
c
      xvalc = xval0
      if (nord .gt. 0) then
        dxp = 1.
        do n = 1,nord
          dxp = dxp*delxi
          xvalc = xvalc + ( dxval0(n)/fctrl(n) )*dxp
        enddo
      endif
      resx = xvalc - xtargv
c
      end


c     program DRDEVVUN
c>> 1996-05-28 DRDEVVUN  Krogh Added external statement.
c>> 1994-10-19 DRDEVVUN  Krogh  Changes to use M77CON
c>> 1994-09-22 DRDEVVUN  CLL
c>> 1992-04-23 CLL
c>> 1992-03-04 DRDEVVUN  Krogh Initial version.
c     Demonstrate unsymmetric eigenvalue/eigenvector subroutine DEVVUN.
c     ------------------------------------------------------------------
c--D replaces "?": DR?EVVUN, ?EVVUN, ?VECP, ?MATP, ?DOT
c     ------------------------------------------------------------------
      integer I, J, LDA, N
      parameter (LDA = 3)
      integer IFLAG(LDA)
      double precision A(LDA,LDA), VR(LDA), VI(LDA), VEC(LDA, LDA)
      double precision WORK(LDA), ASAV(LDA, LDA), D(LDA, LDA), ANORM
      external DDOT
      double precision DDOT
      data (A(1,I), I=1,3) /  8.0d0, -1.0d0, -5.0d0 /
      data (A(2,I), I=1,3) / -4.0d0,  4.0d0, -2.0d0 /
      data (A(3,I), I=1,3) / 18.0d0, -5.0d0, -7.0d0 /
      data ANORM / 30.0d0 /
      data N / LDA /
c     ------------------------------------------------------------------
      print*,'DRDEVVUN..  Demo driver for DEVVUN.'
c
c     First copy A() to ASAV() for later residual check.
c
         do 20 J = 1, N
            do 10 I = 1, N
               ASAV(I, J) = A(I, J)
   10       continue
   20    continue

         call DEVVUN(A(1, 1), LDA, N, VR, VI, VEC, IFLAG, WORK)

         print '(a, I2)',' IFLAG(1) =',IFLAG(1)
         if (IFLAG(1) .le. 2) then
            call DVECP(VR, N, ' Real part of the eigenvalues')
            call DVECP(VI, N, ' Imaginary part of the eigenvalues')
            call DMATP(VEC, LDA, N, N,
     *         '0 Eigenvectors as columns or pairs of columns')
c
c           As a check compute D = (ASAV*VEC - VEC*EVAL) / ANORM.
c           Expect D to be close to the machine precision.
c
            do 50 J = 1, N
               if (VI(J) .eq. 0.0d0) then
c                 Compute residual for a real eigenvalue and eigenvector
                  do 30 I = 1, N
                     D(I, J) = (DDOT(N, ASAV(I,1), LDA, VEC(1,J), 1) -
     *                  VEC(I,J) * VR(J)) / ANORM
   30             continue
               else if (VI(J) .gt. 0.0d0) then
                  do 40 I = 1, N
                     D(I, J) = (DDOT(N,ASAV(I,1),LDA,VEC(1,J),1) -
     *                  VEC(I,J)*VR(J)+VEC(I,J+1)*VI(J))/ANORM
                     D(I, J+1) = (DDOT(N,ASAV(I,1),LDA,VEC(1,J+1),1) -
     *                  VEC(I,J)*VI(J)-VEC(I,J+1)*VR(J))/ANORM
   40             continue
               end if
   50       continue
            call DMATP(D, LDA, N, N,
     *      '0 Packed residual matrix D = (A*EVEC - EVEC*EVAL) / ANORM')
         else
            print'(/a)', ' Failure in DEVVUN. '
         end if
      stop
      end


!----------------------------------------
      subroutine computeorthogradient(msk,psi,dx,dy,nh,n,m,u,v)

! compute u,v= (-dpsi/dy,dpsi/dx)
!

      implicit none

      integer,intent(in):: n,m,nh
      integer*1,dimension(m,n) :: msk
      real*8,dimension(m,n) :: psi,u,v
      real*8::dx,dy

!f2py intent(inplace)::psi,msk,u,v

      real*8::zdx,zdy
      integer:: i,j
      integer*1:: mm

      zdx = 1./dx
      zdy = 1./dy
      do j=2,m-1!nh-1,m-nh+1
         do i=2,n-1!nh-1,n-nh+1
            mm=msk(j,i)+msk(j,i+1)
            if(mm.eq.2)then
               u(j,i) = zdy*(psi(j-1,i)-psi(j,i))
            else
               u(j,i) = 0.
            endif
            mm=msk(j,i)+msk(j+1,i)
            if(mm.eq.2)then
               v(j,i) = zdx*(psi(j,i)-psi(j,i-1))
            else
               v(j,i) = 0.
            endif
         enddo
      enddo

      end subroutine

!----------------------------------------
      subroutine celltocorner(xr,xp,n,m)

! average 4 cell centers at upper right cell corner
!

      implicit none

      integer,intent(in):: n,m
      real*8,dimension(m,n) :: xr,xp

!f2py intent(inplace)::xr,xp

      integer:: i,j

      do j=1,m-1
         do i=1,n-1
            xp(j,i)=0.25*(xr(j,i)+xr(j,i+1)+xr(j+1,i)+xr(j+1,i+1))
         enddo
      enddo

      end subroutine

!----------------------------------------
      subroutine celltocornerbicubic(xr,xp,n,m)

! average 4x4 cell centers at upper right cell corner
! using the finite difference interpolation [-1,9,9,1]/16 in each direction

      implicit none

      integer,intent(in):: n,m
      real*8,dimension(m,n) :: xr,xp

!f2py intent(inplace)::xr,xp
      real:: cff
      integer:: i,j,jj,k1,k2,k3,k4
      real*8,dimension(n,4)::z

      cff = 1./12.
      do j=1,m
         k1 = mod(j-1,4)+1
         do i=2,n-2
            z(i,k1)=cff*(-xr(j,i-1)+7*(xr(j,i)+xr(j,i+1))-xr(j,i+2))
         enddo
         if (j.ge.4)then
            k2 = mod(j-2,4)+1
            k3 = mod(j-3,4)+1
            k4 = mod(j-4,4)+1
            jj = j-2
            do i=2,n-2
               xp(jj,i)=cff*(-z(i,k1)+7*(z(i,k2)+z(i,k3))-z(i,k4))
            enddo
         endif
      enddo

      end subroutine

!----------------------------------------
      subroutine cornertocell(xp,xr,n,m)

! average 4 cell centers at upper right cell corner
!

      implicit none

      integer,intent(in):: n,m
      real*8,dimension(m,n) :: xr,xp

!f2py intent(inplace)::xr,xp

      integer:: i,j

      do j=2,m
         do i=2,n
            xr(j,i)=0.25*(xp(j,i)+xp(j,i-1)+xp(j-1,i)+xp(j-1,i-1))
         enddo
      enddo

      end subroutine

!----------------------------------------
      subroutine add_diffusion(msk,trac,dx,nh,Kdiff,dtrac,n,m)

! add a diffusion term (with homogenous Neumann BC) on trac
!

      implicit none

      integer,intent(in):: n,m,nh
      integer*1,dimension(m,n) :: msk
      real*8,dimension(m,n) :: trac,dtrac
      real*8,intent(in)::dx,Kdiff

!f2py intent(inplace)::msk,trac,dtrac

      real*8::coef,dbl,dbr
      integer:: i,j
      integer*1:: mm,ml,mr

      coef = Kdiff/(dx*dx)
      do j=2,m-1
         do i=2,n-1
            if (msk(j,i).eq.1) then
               dtrac(j,i) = dtrac(j,i) + coef*( &
                    + msk(j,i-1) * ( trac(j,i-1)-trac(j,i) ) &
                    + msk(j,i+1) * ( trac(j,i+1)-trac(j,i) ) &
                    + msk(j-1,i) * ( trac(j-1,i)-trac(j,i) ) &
                    + msk(j+1,i) * ( trac(j+1,i)-trac(j,i) ) )
            endif
         enddo
      enddo

      end subroutine

!----------------------------------------
      subroutine computewallshear(msk,x,y,total,dx,nh,m,n)

! compute y=b-A*x

      implicit none

      integer,intent(in):: n,m,nh
      integer*1,dimension(m,n) :: msk
      real*8,dimension(m,n) :: x
      real*8,dimension(m,n) :: y
      real*8::dx,total

!f2py intent(inplace)::msk,x
!f2py intent(inplace)::y
!f2py intent(out)::total

!      real*8,dimension(n,3) :: xo
      integer:: i,j
      integer*1::msku,mskv
      real*8::cff,u,v

      cff=1./(2*dx*dx)

!      y(:,:)=0.
      total=0.
      y(1:nh,:)=0.
      do j=nh+1,m-nh+1
         y(j,1:nh)=0.
         do i=nh+1,n-nh+1
            y(j,i)=0.
            msku=msk(j,i-1)+msk(j,i)
            if (msku.eq.1)then
               if(msk(j,i).ne.0)then
                  v=(x(j,i+1)+x(j-1,i+1))*cff
                  y(j,i)=y(j,i)-v
                  total=total-v
               else
                  v=(x(j,i-3)+x(j-1,i-3))*cff
                  y(j,i-1)=y(j,i-1)+v
                  total=total+v
               endif
            endif
            mskv=msk(j-1,i)+msk(j,i)
            if (mskv.eq.1)then
               if(msk(j,i).ne.0)then
                  u=(x(j+1,i)+x(j+1,i-1))*cff
                  y(j,i)=y(j,i)+u
                  total=total+u
               else
                  u=(x(j-3,i)+x(j-3,i-1))*cff
                  y(j-1,i)=y(j-1,i)-u
                  total=total-u
               endif
            endif
         enddo
      enddo

      end subroutine

!----------------------------------------
      subroutine computenoslipsourceterm(msk,x,y,total,dx,dy,nh,m,n)

! compute y=b-A*x

      implicit none

      integer,intent(in):: n,m,nh
      integer*1,dimension(m,n) :: msk
      real*8,dimension(m,n) :: x
      real*8,dimension(m,n) :: y
      real*8::dx,dy,total

!f2py intent(inplace)::msk,x
!f2py intent(inplace)::y
!f2py intent(out)::total

!      real*8,dimension(n,3) :: xo
      integer:: i,j
      integer*1::msku,mskv
      real*8::cff,u,v

      cff=1./(2*dx*dy)

!      y(:,:)=0.
      total=0.
      y(1:nh,:)=0.
      do j=nh+1,m-nh+1
         y(j,1:nh)=0.
         do i=nh+1,n-nh+1
            y(j,i)=0.
            msku=msk(j,i-1)+msk(j,i)
            if (msku.eq.1)then
               v=(x(j,i)+x(j-1,i)-x(j,i-2)-x(j-1,i-2))*cff
               if(msk(j,i).ne.0)then
                  y(j,i)=y(j,i)-v
                  total=total-v
               else
                  y(j,i-1)=y(j,i-1)+v
                  total=total+v
               endif
            endif
            mskv=msk(j-1,i)+msk(j,i)
            if (mskv.eq.1)then
               u=-(x(j,i)+x(j,i-1)-x(j-2,i)-x(j-2,i-1))*cff
               if(msk(j,i).ne.0)then
                  y(j,i)=y(j,i)+u
                  total=total+u
               else
                  y(j-1,i)=y(j-1,i)-u
                  total=total-u
               endif
            endif
         enddo
      enddo

      end subroutine

!----------------------------------------
      subroutine computenoslipsourceterm_regularized(msk,x,y,cu,cv,total,dx,nh,m,n)

! compute y=b-A*x

      implicit none

      integer,intent(in):: n,m,nh
      integer*1,dimension(m,n) :: msk
      real*8,dimension(m,n) :: x,cu,cv
      real*8,dimension(m,n) :: y
      real*8::dx,total

!f2py intent(inplace)::msk,x,cu,cv
!f2py intent(inplace)::y
!f2py intent(out)::total

!      real*8,dimension(n,3) :: xo
      integer:: i,j
      integer*1::msku,mskv
      real*8::cff,ul,ur,vl,vr,z

      cff=2./(dx*dx)

!      y(:,:)=0.
      total=0.
      y(1:nh,:)=0.
      do j=nh+1,m-nh+1
         y(j,1:nh)=0.
         do i=nh+1,n-nh+1
            if(msk(j,i).ne.0)then
               vl = (x(j,i)-x(j,i-1))*cv(j,i-1)/(msk(j,i)+msk(j,i-1))
               vr = (x(j,i)-x(j,i+1))*cv(j,i)/(msk(j,i)+msk(j,i+1))

               ul = (x(j,i)-x(j-1,i))*cu(j-1,i)/(msk(j,i)+msk(j-1,i))
               ur = (x(j,i)-x(j+1,i))*cu(j,i)/(msk(j,i)+msk(j+1,i))

               z = -(vl+ur+vr+ul)*cff
               y(j,i) = z
               total=total+z
            else
               y(j,i)=0.
            endif
         enddo
      enddo

      end subroutine

!----------------------------------------
      subroutine add_torque(msk,buoy,dx,nh,gravity,domega,n,m)

! add torque db/dx to domega
!

      implicit none

      integer,intent(in):: n,m,nh
      integer*1,dimension(m,n) :: msk
      real*8,dimension(m,n) :: buoy,domega
      real*8,intent(in)::dx,gravity

!f2py intent(inplace)::msk,buoy,domega

      real*8::coef,dbl,dbr,sum,z
      integer:: i,j,ii
      integer*1:: ml,mr,mm,one

      coef = 0.5*gravity/(dx)

      one = 1
      do j=1+nh,m-nh
!         sum=0.
         i=1+nh
         ml  =  max(one,msk(j,i)+ msk(j,i-1))
         dbl = (buoy(j,i)*msk(j,i)+buoy(j,i-1)*msk(j,i-1))/ml
         do i=1+nh,n-nh

            mr = max(one,msk(j,i+1)+ msk(j,i))
            dbr=(buoy(j,i+1)*msk(j,i+1)+buoy(j,i)*msk(j,i))/mr

            mm=(ml+mr)
!            if(mm.gt.0) then
!               z = (dbr-dbl)*(coef)
!            else
!               z=0.
!            endif
            if (mm.eq.4) then
!                 domega(j,i) = domega(j,i) + (dbr-dbl)*(coef)*msk(j,i)
                 domega(j,i) = domega(j,i) + (buoy(j,i+1)-buoy(j,i-1))*(coef)*msk(j,i)
            endif
!            sum=sum+z
!            if(mm.gt.0) domega(j,i) =  domega(j,i)+(dbr-dbl)*(coef/mm)
            ml=mr
            dbl=dbr
!            ii=ii+1
         enddo
!         write(*,*)"sum=",j,ii,sum
      enddo

      end subroutine