c----------------------------------------------------------------------c
c                          S P A R S K I T                             c
c----------------------------------------------------------------------c
c      MATRIX GENERATION ROUTINES - FINITE ELEMENT MATRICES            c
c----------------------------------------------------------------------c
c contents:                                                            c
c----------                                                            c
c genfea       : generates finite element matrices in assembled form   c
c genfea_wbc   : generates finite element matrices in assembled form   c
c                without applying the boundary conditions              c
c genfeu       : generates finite element matrices in unassembled form c
c genfeu_wbc   : generates finite element matrices in unassembled form c
c                without applying the boundary conditions              c
c genfeu_lstif : generates finite element matrices in unassembled form c
c                using the lstif problem appearing in elmtlib2.f       c
c assmb1       : assembles an unassembled matrix (produced by genfeu)  c
c----------------------------------------------------------------------c
      subroutine genfea (nx,nelx,node,job,x,y,ijk,nodcode,fs,nint,
     *     a,ja,ia,f,iwk,jwk,ierr,xyk)
c-----------------------------------------------------------------------
c this subroutine generates a finite element matrix in assembled form.
c the matrix is assembled in compressed sparse row format. See genfeu
c for matrices in unassembled form. The user must provide the grid,
c (coordinates x, y and connectivity matrix ijk) as well as some
c information on the nodes (nodcode) and the material properties
c (the function K(x,y) above) in the form of a subroutine xyk.
c----------------------------------------------------------------------
c
c on entry:
c ---------
c
c nx	    = integer . the number of nodes in the grid .
c nelx	    = integer . the number of elements in the grid.
c node      = integer = the number of nodes per element (should be
c             set to three in this version). also the first dimension
c             of ijk
c job	    = integer. If job=0, it is assumed that there is no heat
c             source (i.e. fs = 0) and the right hand side
c             produced will therefore be a zero vector.
c             If job = 1 on entry then the contributions from the
c             heat source in each element are taken into account.
c
c x, y      = two real arrays containing the coordinates of the nodes.
c
c ijk       =  an integer array containing the connectivity matrix.
c              ijk(i,nel), i=1,2,..node, is the list of the nodes
c              constituting the element nel, ans listed in
c              counter clockwise order.
c
c nodcode   = an integer array containing the boundary information for
c             each node with the following meaning.
c	nodcode(i) = 0 -->  node i is internal
c	nodcode(i) = 1 -->  node i is a boundary but not a corner point
c	nodcode(i) = 2 -->  node i is a corner node. [This node and the
c             corresponmding element are discarded.]
c
c fs	    = real array of length nelx on entry containing the heat
c             source for each element (job = 1 only)
c
c xyk	    = subroutine defining the material properties at each
c	      element. Form:
c 	      call xyk(nel,xyke,x,y,ijk,node) with on return
c             xyke =  material constant matrices.
c	      for each element nel, xyke(1,nel),xyke(2,nel)
c             and xyke(3,nel) represent the constants
c             K11, K22, and K12 at that element.
c
c on return
c ---------
c nint	    = integer. The number of active (nonboundary) nodes. Also
c             equal to the dimension of the assembled matrix.
c
c a, ja, ia = assembled matrix in compressed sparse row format.
c
c f	    = real array containing the right hand for the linears
c             system to solve.
c
c ierr	    = integer. Error message. If (ierr .ne. 0) on return
c             it means that one of the elements has a negative or zero
c             area probably because of a bad ordering of the nodes
c             (see ijk above). Use the subroutine chkelmt to reorder
c             the nodes properly if necessary.
c iwk, jwk  = two integer work arrays of length nx each.
c
c-----------------------------------------------------------------------
      real*8 a(*),x(*),y(*),f(*),fs(*)
      integer ijk(node,*), nodcode(*),ia(*),ja(*),iwk(*),jwk(*)
      external xyk, funb, func, fung
c
      ierr = 0
c
c     take into boundary conditions to remove boundary nodes.
c
      call bound (nx,nelx,ijk,nodcode,node,nint,jwk,
     *     x,y,f,iwk)
c
c     assemble the matrix
c
       call assmbo (nx,nelx,node,ijk,nodcode,x,y,
     *     a,ja,ia,f,iwk,jwk,ierr,xyk, funb, func, fung)
c
c     if applicable (job .eq. 1) get heat source function
c
      indic = 1
      if (job .eq. 1)
     *     call hsourc (indic,nelx,node,x,y,ijk,fs,f)
c
c     call diric for Dirichlet conditions
c
      call diric(nint,a,ja,ia)
c     done
      return
c------end of genfea ---------------------------------------------------
c-----------------------------------------------------------------------
      end
      subroutine genfea_wbc (nx,nelx,node,job,x,y,ijk,nodcode,fs,
     *     a,ja,ia,f,iwk,jwk,ierr,xyk)
c-----------------------------------------------------------------------
c this subroutine generates a finite element matrix in assembled form.
c the matrix is assembled in compressed sparse row format. See genfeu
c for matrices in unassembled form. The user must provide the grid,
c (coordinates x, y and connectivity matrix ijk) as well as some
c information on the nodes (nodcode) and the material properties
c (the function K(x,y) above) in the form of a subroutine xyk.
c----------------------------------------------------------------------
c Irene Moulitsas, moulitsa@cs.umn.edu  : It does not apply boundary
c                 conditions; variable nint is eliminated
c----------------------------------------------------------------------
c
c on entry:
c ---------
c
c nx	    = integer . the number of nodes in the grid .
c nelx	    = integer . the number of elements in the grid.
c node      = integer = the number of nodes per element (should be
c             set to three in this version). also the first dimension
c             of ijk
c job	    = integer. If job=0, it is assumed that there is no heat
c             source (i.e. fs = 0) and the right hand side
c             produced will therefore be a zero vector.
c             If job = 1 on entry then the contributions from the
c             heat source in each element are taken into account.
c
c x, y      = two real arrays containing the coordinates of the nodes.
c
c ijk       =  an integer array containing the connectivity matrix.
c              ijk(i,nel), i=1,2,..node, is the list of the nodes
c              constituting the element nel, ans listed in
c              counter clockwise order.
c
c nodcode   = an integer array containing the boundary information for
c             each node with the following meaning.
c	nodcode(i) = 0 -->  node i is internal
c	nodcode(i) = 1 -->  node i is a boundary but not a corner point
c	nodcode(i) = 2 -->  node i is a corner node. [This node and the
c             corresponmding element are discarded.]
c
c fs	    = real array of length nelx on entry containing the heat
c             source for each element (job = 1 only)
c
c xyk	    = subroutine defining the material properties at each
c	      element. Form:
c 	      call xyk(nel,xyke,x,y,ijk,node) with on return
c             xyke =  material constant matrices.
c	      for each element nel, xyke(1,nel),xyke(2,nel)
c             and xyke(3,nel) represent the constants
c             K11, K22, and K12 at that element.
c
c on return
c ---------
c a, ja, ia = assembled matrix in compressed sparse row format.
c
c f	    = real array containing the right hand for the linears
c             system to solve.
c
c ierr	    = integer. Error message. If (ierr .ne. 0) on return
c             it means that one of the elements has a negative or zero
c             area probably because of a bad ordering of the nodes
c             (see ijk above). Use the subroutine chkelmt to reorder
c             the nodes properly if necessary.
c iwk, jwk  = two integer work arrays of length nx each.
c
c-----------------------------------------------------------------------
      real*8 a(*),x(*),y(*),f(*),fs(*)
      integer ijk(node,*), nodcode(*),ia(*),ja(*),iwk(*),jwk(*)
      external xyk, funb, func, fung
c
      ierr = 0
c
c     assemble the matrix
c
       call assmbo (nx,nelx,node,ijk,nodcode,x,y,
     *     a,ja,ia,f,iwk,jwk,ierr,xyk, funb, func, fung)
c
c     if applicable (job .eq. 1) get heat source function
c
      indic = 1
      if (job .eq. 1)
     *     call hsourc (indic,nelx,node,x,y,ijk,fs,f)
c
c     done
      return
c------end of genfea_wbc -----------------------------------------------
c-----------------------------------------------------------------------
       end
c-----------------------------------------------------------------------
      subroutine genfeu (nx,nelx,node,job,x,y,ijk,nodcode,fs,
     *     nint,a,na,f,iwk,jwk,ierr,xyk)
c-----------------------------------------------------------------------
c this subroutine generates finite element matrices for heat
c condution problem
c
c                  - Div ( K(x,y) Grad u ) = f
c                    u = 0 on boundary
c
c (with Dirichlet boundary conditions). The matrix is returned
c in unassembled form. The user must provide the grid,
c (coordinates x, y and connectivity matrix ijk) as well as some
c information on the nodes (nodcode) and the material properties
c (the function K(x,y) above) in the form of a subroutine xyk.
c
c----------------------------------------------------------------------
c
c on entry:
c ---------
c
c nx	    = integer . the number of nodes in the grid .
c nelx	    = integer . the number of elements in the grid.
c node      = integer = the number of nodes per element (should be
c             set to three in this version). also the first dimension
c             of ijk
c job	    = integer. If job=0, it is assumed that there is no heat
c             source (i.e. fs = 0) and the right hand side
c             produced will therefore be a zero vector.
c             If job = 1 on entry then the contributions from the
c             heat source in each element are taken into account.
c
c na	    = integer. The first dimension of the array a.
c             a is declared as an array of dimension a(na,node,node).
c
c x, y      = two real arrays containing the coordinates of the nodes.
c
c ijk       =  an integer array containing the connectivity matrix.
c              ijk(i,nel), i=1,2,..node, is the list of the nodes
c              constituting the element nel, ans listed in
c              counter clockwise order.
c
c xyk	    = subroutine defining the material properties at each
c	      element. Form:
c 	      call xyk(nel,xyke,x,y,ijk,node) with on return
c             xyke =  material constant matrices.
c	      for each element nel, xyke(1,nel),xyke(2,nel)
c             and xyke(3,nel) represent the constants
c             K11, K22, and K12 at that element.
c
c nodcode   = an integer array containing the boundary information for
c             each node with the following meaning.
c	nodcode(i) = 0 -->  node i is internal
c	nodcode(i) = 1 -->  node i is a boundary but not a corner point
c	nodcode(i) = 2 -->  node i is a corner node. [This node and the
c             corresponmding element are discarded.]
c
c fs	    = real array of length nelx on entry containing the heat
c             source for each element (job = 1 only)
c
c on return
c ---------
c nint	    = integer. The number of active (nonboundary) nodes. Also
c             equal to the dimension of the assembled matrix.
c
c a         = matrix in unassembled form. a(nel,*,*) contains the
c             element matrix for element nel.
c
c f	    = real array containing the right hand for the linears
c             system to solve, in assembled form.
c
c ierr	    = integer. Error message. If (ierr .ne. 0) on return
c             it means that one of the elements has a negative or zero
c             area probably because of a bad ordering of the nodes
c             (see ijk above). Use the subroutine chkelmt to reorder
c             the nodes properly if necessary.
c iwk, jwk  = two integer work arrays of length nx each.
c
c-----------------------------------------------------------------------
      real*8 a(na,node,node),x(*),y(*),f(*), fs(*)
      integer ijk(node,*), nodcode(*),iwk(*),jwk(*)
      external xyk
c
      ierr = 0
c
c     take boundary conditions into account to move boundary nodes to
c     the end..
c
      call bound (nx,nelx,ijk,nodcode,node,nint,jwk,
     *     x,y,f,iwk)
c
c     assemble the matrix
c
      call unassbl (a,na,f,nx,nelx,ijk,node,x,y,ierr,xyk)
c
c     if applicable (job .eq. 1) get heat source function
c
      indic = 0
      if (job .eq. 1)
     *     call hsourc (indic,nelx,node,x,y,ijk,fs,f)
c
c     done
c
      return
      end
c----- end of genfeu ----------------------------------------------------
      subroutine genfeu_wbc (nx,nelx,node,job,x,y,ijk,fs,
     *          a,na,f,ierr,xyk)
c-----------------------------------------------------------------------
c this subroutine generates finite element matrices for heat
c condution problem
c
c                  - Div ( K(x,y) Grad u ) = f
c                    u = 0 on boundary
c
c (with Dirichlet boundary conditions). The matrix is returned
c in unassembled form. The user must provide the grid,
c (coordinates x, y and connectivity matrix ijk) as well as some
c information on the nodes (nodcode) and the material properties
c (the function K(x,y) above) in the form of a subroutine xyk.
c
c----------------------------------------------------------------------
c moulitsa@cs   : It does not apply boundary conditions
c                 variable nint is eliminated
c----------------------------------------------------------------------
c
c on entry:
c ---------
c
c nx	    = integer . the number of nodes in the grid .
c nelx	    = integer . the number of elements in the grid.
c node      = integer = the number of nodes per element (should be
c             set to three in this version). also the first dimension
c             of ijk
c job	    = integer. If job=0, it is assumed that there is no heat
c             source (i.e. fs = 0) and the right hand side
c             produced will therefore be a zero vector.
c             If job = 1 on entry then the contributions from the
c             heat source in each element are taken into account.
c
c na	    = integer. The first dimension of the array a.
c             a is declared as an array of dimension a(na,node,node).
c
c x, y      = two real arrays containing the coordinates of the nodes.
c
c ijk       =  an integer array containing the connectivity matrix.
c              ijk(i,nel), i=1,2,..node, is the list of the nodes
c              constituting the element nel, ans listed in
c              counter clockwise order.
c
c xyk	    = subroutine defining the material properties at each
c	      element. Form:
c 	      call xyk(nel,xyke,x,y,ijk,node) with on return
c             xyke =  material constant matrices.
c	      for each element nel, xyke(1,nel),xyke(2,nel)
c             and xyke(3,nel) represent the constants
c             K11, K22, and K12 at that element.
c
c nodcode   = an integer array containing the boundary information for
c             each node with the following meaning.
c	nodcode(i) = 0 -->  node i is internal
c	nodcode(i) = 1 -->  node i is a boundary but not a corner point
c	nodcode(i) = 2 -->  node i is a corner node. [This node and the
c             corresponmding element are discarded.]
c
c fs	    = real array of length nelx on entry containing the heat
c             source for each element (job = 1 only)
c
c on return
c ---------
c a         = matrix in unassembled form. a(nel,*,*) contains the
c             element matrix for element nel.
c
c f	    = real array containing the right hand for the linears
c             system to solve, in assembled form.
c
c ierr	    = integer. Error message. If (ierr .ne. 0) on return
c             it means that one of the elements has a negative or zero
c             area probably because of a bad ordering of the nodes
c             (see ijk above). Use the subroutine chkelmt to reorder
c             the nodes properly if necessary.
c
c-----------------------------------------------------------------------
      real*8 a(na,node,node),x(*),y(*),f(*), fs(*)
      integer ijk(node,*)
      external xyk
c
      ierr = 0
c
c     assemble the matrix
c
      call unassbl (a,na,f,nx,nelx,ijk,node,x,y,ierr,xyk)
c
c     if applicable (job .eq. 1) get heat source function
c
      indic = 0
      if (job .eq. 1)
     *     call hsourc (indic,nelx,node,x,y,ijk,fs,f)
c
c     done
c
      return
      end
c----- end of genfeu_wbc -----------------------------------------------
      subroutine genfeu_lstif (nx,nelx,node,job,x,y,ijk,fs,
     *          a,na,f,ierr,xyk)
c-----------------------------------------------------------------------
c this subroutine generates finite element matrices using unassmbl_lstif.
c The matrix is returned in unassembled form.
c The user must provide the grid, coordinates x, y and connectivity matrix
c ijk) as well as some information on the nodes (nodcode) and the material
c properties (the function K(x,y) above) in the form of a subroutine xyk.
c
c----------------------------------------------------------------------
c moulitsa@cs.umn.edu   : It does not apply boundary conditions
c                         variable nint is eliminated
c----------------------------------------------------------------------
c
c on entry:
c ---------
c
c nx	    = integer . the number of nodes in the grid .
c nelx	    = integer . the number of elements in the grid.
c node      = integer = the number of nodes per element (should be
c             set to three in this version). also the first dimension
c             of ijk
c job	    = integer. If job=0, it is assumed that there is no heat
c             source (i.e. fs = 0) and the right hand side
c             produced will therefore be a zero vector.
c             If job = 1 on entry then the contributions from the
c             heat source in each element are taken into account.
c
c na	    = integer. The first dimension of the array a.
c             a is declared as an array of dimension a(na,node,node).
c
c x, y      = two real arrays containing the coordinates of the nodes.
c
c ijk       =  an integer array containing the connectivity matrix.
c              ijk(i,nel), i=1,2,..node, is the list of the nodes
c              constituting the element nel, ans listed in
c              counter clockwise order.
c
c xyk	    = subroutine defining the material properties at each
c	      element. Form:
c 	      call xyk(nel,xyke,x,y,ijk,node) with on return
c             xyke =  material constant matrices.
c	      for each element nel, xyke(1,nel),xyke(2,nel)
c             and xyke(3,nel) represent the constants
c             K11, K22, and K12 at that element.
c
c nodcode   = an integer array containing the boundary information for
c             each node with the following meaning.
c
c fs	    = real array of length nelx on entry containing the heat
c             source for each element (job = 1 only)
c
c on return
c ---------
c a         = matrix in unassembled form. a(nel,*,*) contains the
c             element matrix for element nel.
c
c f	    = real array containing the right hand for the linears
c             system to solve, in assembled form.
c
c ierr	    = integer. Error message. If (ierr .ne. 0) on return
c             it means that one of the elements has a negative or zero
c             area probably because of a bad ordering of the nodes
c             (see ijk above). Use the subroutine chkelmt to reorder
c             the nodes properly if necessary.
c jwk  = integer work array of length nx
c
c-----------------------------------------------------------------------
      real*8 a(na,node,node),x(*),y(*),f(*), fs(*)
      integer ijk(node,*)
      external xyk, funb, func, fung
c
      ierr = 0
c
c     assemble the matrix
c
      call unassbl_lstif (a,na,f,nx,nelx,ijk,
     *     node,x,y,ierr,xyk,funb,func,fung)
c
c     if applicable (job .eq. 1) get heat source function
c
      indic = 0
      if (job .eq. 1)
     *     call hsourc (indic,nelx,node,x,y,ijk,fs,f)
c
c     done
c
      return
      end
c----- end of genfeu_lstif ---------------------------------------------
c-----------------------------------------------------------------------
      subroutine assmb1 (u,nu,a,ja,ia,fu,f,nx,nelx,ijk,nodcode,
     *     node,iwk,jwk)
c--------------------------------------------------------------
c u	 = unassembled matrix u(na,node,node)
c nu	 = 1-st dimension of u
c a,ja,ia= assembled matrix on output
c fu	 = unassembled right hand side
c f      = right hand side (global load vector) assembled
c nx     = number of nodes at input
c nelx	 = number of elements at input
c ijk	 = connectivity matrix: for node k, ijk(*,k) point to the
c          nodes of element k.
c node	 = total number of nodal points in each element
c
c nodcode= boundary information list for each node with the
c	   following meaning:
c	nodcode(i) = 0 -->  node i is internal
c	nodcode(i) = 1 -->  node i is a boundary but not a corner point
c	nodcode(i) = 2 -->  node i is a corner point (corner points
c
c x,y   = real*8 arrays containing the $x$ and $y$ coordinates
c	  resp. of the nodes.
c         K11, K22, and K12 at that element.
c iwk,jwk = two integer work arrays.
c ierr	= error message integer .
c	  ierr = 0 --> normal return
c	  ierr = 1 --> negative area encountered (due to bad
c	           numbering of nodes of an element- see
c		   message printed in unit iout). not used..
c iout	= output unit (not used here).
c--------------------------------------------------------------
      implicit real*8 (a-h,o-z)
      real*8 u(nu,node,node),a(*),fu(node,*),f(*)
      integer ja(*),ia(*),ijk(node,*),iwk(*),jwk(*),nodcode(*)
c     max number of nonzeros per row allowed  = 200
c--------------------------------------------------------------
c     initialize
c--------------------------------------------------------------
      do 100 i=1,nx
         f(i) = 0.0d0
 100  continue
c
c     initialize  pointer arrays.
c
      do 5 k=1,nx+1
         ia(k) = 1
         jwk(k) = 0
 5    continue
      do 6 k=1,nelx
         do 59 j=1,node
            knod = ijk(j,k)
            ia(knod) = ia(knod) + 1
 59      continue
 6    continue
c---------------------------------------------------
      do 7 k=1, nx
         if (nodcode(k) .ge.1 ) ia(k)=ia(k)+1
 7    continue
c
      ksav = ia(1)
      ia(1) = 1
      do 101 j=2, nx+1
         ksavn = ia(j)
         ia(j) = ia(j-1) +  ksav
         iwk(j-1) = ia(j-1)-1
         ksav = ksavn
 101  continue
c-----------------
c     main loop
c-----------------
      do 102 nel=1, nelx
c
c     get nodal points
c
         do 120 ka=1, node
            ii = ijk(ka,nel)
            f(ii) = f(ii) + fu(ka,nel)
c
c     unpack row into jwk1
c
            irowst = ia(ii)
            ilast  = iwk(ii)
            do 109 k=irowst,ilast
               jwk(ja(k)) = k
 109        continue
c
            do 108 kb = 1,node
c
c     column number = jj
c
               jj = ijk(kb,nel)
               k = jwk(jj)
               if (k .eq. 0) then
                  ilast = ilast+1
                  jwk(jj) = ilast
                  ja(ilast) = jj
                  a(ilast) = u(nel,ka,kb)
               else
                  a(k) = a(k) + u(nel,ka,kb)
               endif
 108        continue
c     refresh jwk
            do 119 k=irowst,ilast
               jwk(ja(k)) = 0
 119        continue
            iwk(ii) = ilast
 120     continue
c
 102  continue
      return
c---------end-of-assmb1----------------------------------------------
      end
c--------------------------------------------------------------------


module newtons

  use precision
  use globals

contains

  function energy(object) result(e)

    type(particle), intent(in) :: object
    real(fdp) :: e

    e = 0.5d0 * object%mass * sum(object%velocity**2)

  end function energy

  function crossproduct(v1, v2) result(v3)

    real(fdp), dimension(3), intent(in) :: v1, v2
    real(fdp), dimension(3) :: v3

    v3(1) = v1(2)*v2(3) - v1(3)*v2(2)
    v3(2) = v1(3)*v2(1) - v1(1)*v2(3)
    v3(3) = v1(1)*v2(2) - v1(2)*v2(1)

  end function crossproduct

  function position(object, v, t) result(s)

    type(particle), intent(in) :: object
    real(fdp), dimension(dim) :: v
    real(fdp), intent(in) :: t
    real(fdp), dimension(dim) :: s

    s = object%position + v * t

  end function position

  function velocity(object, a, t) result(v)

    type(particle), intent(in) :: object
    real(fdp), dimension(dim) :: a
    real(fdp), intent(in) :: t
    real(fdp), dimension(dim) :: v

    v = object%velocity + a * t

  end function velocity

  function acceleration(object, force) result(a)

    type(particle), intent(in) :: object
    real(fdp), dimension(dim), intent(in) :: force
    real(fdp), dimension(dim) :: a

    a = force / object%mass

  end function acceleration

  function magneticforce(object, b) result(f)

    type(particle), intent(in) :: object
    real(fdp), dimension(dim), intent(in) :: b
    real(fdp), dimension(dim) :: f

    f = object%charge * crossproduct(object%velocity, b)

  end function magneticforce

end module newtons


      SUBROUTINE loop_fusion04(A,N,F)
      INTEGER N,I
      REAL F(N),A(N), S
      DO I = 1,N
        A(i) = F(i)
      ENDDO

      S = 0.

      DO i =1, n
        S= S + A(i)
      ENDDO

      END


module functions

    implicit none
    integer :: fevals_proc
    real(kind=8) :: k
    save

contains

    real(kind=8) function f(x)
        implicit none
        real(kind=8), intent(in) :: x
        integer proc_num, ierr

        ! keep track of number of function evaluations by
        ! each process:
        fevals_proc = fevals_proc + 1

        f = 1.d0 + x**3 + sin(k*x)

    end function f

end module functions


!
!     CalculiX - A 3-dimensional finite element program
!              Copyright (C) 1998 Guido Dhondt
!
!     This program is free software; you can redistribute it and/or
!     modify it under the terms of the GNU General Public License as
!     published by the Free Software Foundation(version 2);
!
!
!     This program is distributed in the hope that it will be useful,
!     but WITHOUT ANY WARRANTY; without even the implied warranty of
!     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!     GNU General Public License for more details.
!
!     You should have received a copy of the GNU General Public License
!     along with this program; if not, write to the Free Software
!     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
!
      subroutine plane3(co,nodep,a,b,c,d)
!
!     calculate the equation of the plane through the
!     nodes nodep(1),nodep(2) and nodep(3) in the form
!     a*x+b*y+c*z+d=0 such that the triangle through the
!     nodes nodep(1),nodep(2),nopep(3) is numbered clockwise
!     when looking in the direction of vector (a,b,c)
!
      implicit none
!
      integer nodep(3),i
!
      real*8 co(3,*),a,b,c,d,dd,p12(3),p23(3),p31(3)
!
!     sides of the triangle
!
      do i=1,3
         p12(i)=co(i,nodep(2))-co(i,nodep(1))
         p23(i)=co(i,nodep(3))-co(i,nodep(2))
         p31(i)=co(i,nodep(1))-co(i,nodep(3))
      enddo
!
!     normalized vector normal to the triangle: xn = p12 x p23
!
      a=p12(2)*p23(3)-p12(3)*p23(2)
      b=p12(3)*p23(1)-p12(1)*p23(3)
      c=p12(1)*p23(2)-p12(2)*p23(1)
      dd=dsqrt(a*a+b*b+c*c)
      a=a/dd
      b=b/dd
      c=c/dd
!
!     determining the inhomogeneous term
!
      d=-a*co(1,nodep(1))-b*co(2,nodep(1))-c*co(3,nodep(1))
!
      return
      end


      DOUBLE PRECISION FUNCTION CHDET(A,LDA,N)
      IMPLICIT NONE
*
*  -- user defined driver routine --
*
*
*     .. Scalar Arguments ..
      INTEGER            LDA, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A(LDA,*)
*     ..
*
*  Purpose
*  =======
*
*  The function CH2DET computes the determinant of a matrix
*  proceeding from the Cholesky factorization A of this matrix.
*
*
*  Arguments
*  =========
*
*  A        (input) DOUBLE PRECISION array of DIMENSION (N,N).
*           Before entry, the array A must contain in its upper half
*           the Cholesky factor of the matrix A.
*           Unchanged on exit.
*
*  LDA      (input) INTEGER.
*           LDA is the leading dimension of the array A.
*           Unchanged on exit.
*
*  N        (input) INTEGER.
*           N is the order of the matrix A.
*           Unchanged on exit.
*
*
*  =====================================================================
*
*     ..
*     .. Parameters ..
*     ..
*     .. Local Scalars ..
      INTEGER            I
      DOUBLE PRECISION   TEN
*     ..
*     .. Local Arrays ..
      DOUBLE PRECISION   DET(2)
*     ..
*     .. External Functions ..
*     ..
*     .. External Subroutines ..
*     ..
*     .. Intrinsic Functions ..
*     ..
*     .. Executable Statements ..
*
*
      DET(1) = 1.0D0
      DET(2) = 0.0D0
      TEN = 10.0D0
      DO 50 I = 1, N
         DET(1) = A(I,I)*A(I,I)*DET(1)
         IF (DET(1) .EQ. 0.0D0) GO TO 60
 10      IF (DABS(DET(1)) .GE. 1.0D0) GO TO 20
         DET(1) = TEN*DET(1)
         DET(2) = DET(2) - 1.0D0
         GO TO 10
 20      CONTINUE
 30      IF (DABS(DET(1)) .LT. TEN) GO TO 40
         DET(1) = DET(1)/TEN
         DET(2) = DET(2) + 1.0D0
         GO TO 30
 40      CONTINUE
 50   CONTINUE
 60   CONTINUE
      CHDET=DET(1)*10**DET(2)
      RETURN
      END


c these functions come from the numerical recipes book. I modified the julian day base to be
c 0 for 3/1/1900 to be compatible with Microsoft Excel's method of calculating Julian dates
C for astronomy (not the julian calendar)
C Caldat returns a calendar date given a julian day
C Julian is a function that returns a julian day given a calendar day

      SUBROUTINE caldat(julian,mm,id,iyyy)
! passes julian and returns mm, dd, yyyy
      INTEGER id,iyyy,julian,mm,IGREG
      PARAMETER (IGREG=2299161)
!(IGREG=2299161)
      INTEGER ja,jalpha,jb,jc,jd,je
      Julian=julian+2415079   !provide a reference of 3/1/1900
      if(julian.ge.IGREG)then
        jalpha=int(((julian-1867216)-0.25d0)/36524.25d0)
        ja=julian+1+jalpha-int(0.25d0*jalpha)
      else if(julian.lt.0)then
        ja=julian+36525*(1-julian/36525)
      else
        ja=julian
      endif
      jb=ja+1524
      jc=int(6680.0d0+((jb-2439870)-122.1d0)/365.25d0)
      jd=365*jc+int(0.25d0*jc)
      je=int((jb-jd)/30.6001d0)
      id=jb-jd-int(30.6001d0*je)
      mm=je-1
      if(mm.gt.12)mm=mm-12
      iyyy=jc-4715
      if(mm.gt.2)iyyy=iyyy-1
      if(iyyy.le.0)iyyy=iyyy-1
      if(julian.lt.0)iyyy=iyyy-100*(1-julian/36525)
      return
      END

      FUNCTION julday(Date)
      INTEGER julday,id,iyyy,mm,IGREG
      PARAMETER (IGREG=15+31*(10+12*1582))
      INTEGER ja,jm,jy
      Character*10 date

      Call FromCalendar(date,mm,id,iyyy)

      jy=iyyy
      if (jy.eq.0) pause 'julday: there is no year zero'
      if (jy.lt.0) jy=jy+1
      if (mm.gt.2) then
        jm=mm+1
      else
        jy=jy-1
        jm=mm+13
      endif
      julday=365*jy+int(0.25d0*jy+2000.d0)+int(30.6001d0*jm)+id+1718995
      if (id+31*(mm+12*iyyy).ge.IGREG) then
        ja=int(0.01d0*jy)
        julday=julday+2-ja+int(0.25d0*ja)
        !julday=julday-2415019   ! 1/1/1900 JD zero
        julday=julday-2415079;      ! on 3/1/1900 jday = 1
      endif
      return
      END
C**********************************************************************************
C Subroutine fromCalendar
C**********************************************************************************
      Subroutine FromCalendar(date,month,day,year)
C takes a calendar date string as mm/dd/yyyy and returns the month, day and year

      character*10 date
      Character*10 date2
      integer month, day, year,pos

       pos=index(date,'/')

      read(date(1:pos-1),*) month
      date2=date(pos+1:)
      pos=index(date2,'/')
      if (pos.gt.0) then
         read(date2(1:pos-1),*) day
         read(date2(pos+1:),*) year
        else                               ! case for no year
         read(date2,*) day
        endif
      return
       END

       Function toCalendarDate(month,day,year)

       character*10 toCalendarDate, temp
       integer month, day, year

       write (temp,'(i2.2,A1,i2.2,A1,i4.4)') mm,'/',id,'/',iyyy
       toCalendarDate=temp
       return
       END

        integer Function CurYear(date)

          character*10 date
          integer month, day, iyear
          call FromCalendar(Date,month,day, CurYear)

          return
       END


program leqsol95

    ! LAPACK95のコンパイル時にあわせて作成されたモジュールを使います。
      use f95_lapack
      use obj

      implicit none

      integer(int32), parameter :: NN=10000! rank
      real(real64)  :: t1,t2 ! time measure
      !real(real64) :: A(1:N,1:N), B(1:N), X(1:N) ! A, x and B
      !real(real64) :: Val(1:10)
      !integer(int32) :: index_i(1:10),index_j(1:10)
      real(real64)  ,allocatable :: Val_c(:)
      integer(int32),allocatable :: index_i_c(:),index_j_c(:)
      integer(int32) :: i,m,k,j
      real(real64) :: Amat(NN,NN),bvec(NN),xvec(NN)
      type(LinearSolver_) :: solver ! linear solver instance.
      type(IO_) :: f
      type(Random_) ::rdm

      call rdm%init()
      !call f%open("Amat.txt")
      do i=1,size(Amat,1)
          Amat(i,i)=rdm%random()+10.0d0
      enddo
      do i=1,size(Amat,1)-1
          Amat(i,i+1)=Amat(i,i+1)+rdm%random()+1.0d0
          Amat(i+1,i)=Amat(i+1,i)+rdm%random()+2.0d0
      enddo
      do i=1,NN
          Bvec(i)=rdm%random()+2.0d0
      enddo
    ! LAPACK95（ラッパー）を介してLAPACKのルーチンを呼び出します。
    ! K に K を上三角行列にコレスキー分解した結果が，
    ! P に連立一次方程式の解が格納されます。
    ! K や P を書き換えられたくない場合は注意しましょう。

    call cpu_time(t1)
      call LA_POSV( Amat, Bvec )
      call cpu_time(t2)
      !write(*,'(A)') 'd ='
      !write(*,'(F12.4)') P
      print *, "Solved by LAPACK-LA_POSV","/",t2-t1," sec."

    end program leqsol95


! **************************************************************************
! ... test_adj.f90
! ... Quim Ballabrera, January 2018
! ...
! ... System Dynamics
! ... Adjoint methods applied to the Lorenz model
! ...
! ... Use the adjoint operator definition
! ...                         < A x, p > = < x, A*T p>
! ... where
! ... A = Linear Tangent Model
! ... x = Perturbation
! ... p = Adjoint variable
! ...
! **************************************************************************

program test_adj

use sysdyn

implicit none

integer, parameter                            :: NVE = lorenz_n

logical Zexist

integer iu,iter,rr,record
real(dp) alpha,tr,prod1,prod2
real(dp), dimension(NVE)                      :: xr,dx,Ldx
real(dp), dimension(NVE)                      :: p        ! Adjoint variable

character(len=180) nfile,tfile,dfile

NAMELIST /namltm/ tfile,          &
                  record,         &
                  dfile

call header()

! ... Default initialization
! ...
tfile  = '../../work/lorenz_simulation.dat'
record = 101
dx     = [0.1D0,0.2D0,0.5D0]

call getarg(1,nfile)
inquire(file=nfile,exist=Zexist)

if ((len_trim(nfile).gt.0).and. &
    (.not.Zexist))    call stop_error(1,'Namelist file not found')

if (Zexist) then
  write(*,*) 'Reading ', trim(nfile)
  iu = unitfree()
  open(iu,file=nfile,status='old')
  rewind(iu)
  read(iu,namltm)
  close(iu)

  ! ... Read the perturbation:
  ! ...
  open(iu,file=tfile,status='old')
  read(iu,*) tr, dx
  close(iu)

endif

iu = unitfree()
open(iu,file=tfile,status='old')
do rr=1,record
  read(iu,*,end=2000,err=2000) tr, xr
enddo

! ... Save the reference for the LTM code
! ... LORENZ_REF(1:LORENZ_N) is a vector in the LORENZ module
! ...
lorenz_ref(:) = xr(:)

write(*,*)
write(*,*) 'Reference state : '
write(*,'(100(F12.7))') SNGL(xr)

write(*,*)
write(*,*) 'Perturbation vector : '
write(*,'(100(F12.8))') SNGL(dx)

write(*,*)
write(*,*) 'Adjoint validation:'
write(*,*) '------------------------------------------------------'

alpha = one
do iter=1,5

  ! ... The reference field is in vector LORENZ_REF(:)
  ! ...
  call lorenz_ltm(NVE,tr,alpha*dx,Ldx)
  prod1 = DOT_PRODUCT(Ldx,Ldx)

  p(:) = zero
  call lorenz_adj(NVE,tr,p,Ldx)
  prod2 = alpha*DOT_PRODUCT(p,dx)

  write(*,*)
  write(*,'(T2,"alpha, a = ",F9.6)') alpha
  write(*,'(T2,"< LTM(a*dx), LTM(a*dx) >  = ",F10.6)') prod1
  write(*,'(T2,"< ADJ(LTM(dx)), a*dx >    = ",F10.6)') prod2

  alpha = 0.1D0*alpha
enddo

! ... Success run
! ...
call stop_error(0,'TEST_ADJ Ok')

! ... Faulted run
! ...
2000 call stop_error(1,'TEST_ADJ: Error reading input file')

end program test_adj


      program dmain

      character*10 fread, fwrite
      parameter (fread='icf.in',fwrite='icf.out')
      integer nnzmax, nmax, maxnp
      parameter (nnzmax=600000,nmax=30000,maxnp=25)
      integer nread, nwrite, mmunit
      parameter (nread=1,nwrite=2,mmunit=8)
c     **********
c
c     Sample driver for the incomplete Cholesky decomposition.
c
c     MINPACK-2 Project. July 1998.
c     Argonne National Laboratory.
c     Chih-Jen Lin and Jorge J. More'.
c
c     **********
      character *60 prob_name

      integer i, j, np, nnza, nnzl
      integer n, nnz, info, iters, maxiter
      integer p(maxnp)
      double precision ftime, stime
      double precision fval, rerr

      integer arow_ind(nnzmax), acol_ind(nnzmax), acol_ptr(nmax+1)
      integer lcol_ptr(nmax), lrow_ind(nnzmax)
      integer iw(3*nmax)
      double precision alpha, rtol
      double precision a(nnzmax), adiag(nmax), l(nnzmax), ldiag(nmax)
      double precision w(4*nmax)
      double precision x(nmax), b(nmax)

c     Variables for the Matrix Market format.

      integer nrows, ncols
      character rep*10, field*7, symm*19
      integer ival
      complex cval

      double precision ddot, dnrm2
      external mminfo, mmread
      external dicfs, dnrm2, dpcg, dssyax, dtimer, srtdat2

      open (nread,file=fread,status='old')
      open (nwrite,file=fwrite)

c     Read values of p for the ICF preconditioner.

      read (nread,*) np
      read (nread,*) (p(i),i=1,np)

      do while (1 .eq. 1)

         read (nread,*) prob_name
         if (prob_name .eq. 'STOP') stop
         write (*,*) ' '
         write (*,*) prob_name
         open (mmunit,file=prob_name)

c        Read the Matrix Market information.

         call mminfo(mmunit,rep,field,symm,nrows,ncols,nnz)

c        Check the Matrix Market information.

         if (rep .ne. 'coordinate' .or. field .ne. 'real' .or.
     +      symm .ne. 'symmetric' .or. nrows .gt. nmax) then
            print *,'Matrix Market input error'
            stop
         end if

c        Read the matrix elements.

         call mmread(mmunit,rep,field,symm,nrows,ncols,nnz,
     +        nnzmax,arow_ind,acol_ind,ival,a,cval)
         n = nrows

c        Change from coordinate storage to compresses column storage.

         call srtdat2(n,nnz,a,adiag,arow_ind,acol_ind,acol_ptr,iw)

c        Generate the right hand side.
c        We set b = A*e, where e is the vector of ones.
c        Note that the paper of Lin and More', "Incomplete Cholesky
c        factorizations with limited memory", uses b = e.

         do i = 1, n
            b(i) = adiag(i)
         end do
         do i = 1, n
            do k = acol_ptr(i), acol_ptr(i+1)-1
               b(arow_ind(k)) = b(arow_ind(k)) + a(k)
               b(i) = b(i) + a(k)
            end do
         end do

c        Modify the matrix for the optimization problems.

         if (prob_name(8:11) .eq. 'dgl2' .or.
     +       prob_name(8:14) .eq. 'nlmsurf' .or.
     +       prob_name(8:13) .eq. 'jimack') then
            do i = 1, n
               adiag(i) = adiag(i)*(1.0d0 + 1.d-5)
            end do
         end if

         do j = 1, np

c           Check for sufficient memory.

            nnz = acol_ptr(n+1) - 1
            if (nnz + p(j)*n .gt. nnzmax) then
               write (*,*) 'Parameter nnzmax is too small'
               stop
            end if

c           Compute the preconditioner L.

            call dtimer(stime)

            if (p(j) .lt. 0) then
               do i = 1, n
                  ldiag(i) = sqrt(adiag(i))
                  lcol_ptr(i) = 1
               end do
               lcol_ptr(n+1) = 1
            else
               alpha = 0.0d0
               call dicfs(n,nnz,a,adiag,acol_ptr,arow_ind,
     +                    l,ldiag,lcol_ptr,lrow_ind,
     +                    p(j),alpha,iw,w,w(n+1))
            end if

c           Solve Ax = b with the preconditioned
c           conjugate gradient method.

            rtol = 1.0d-6
            maxiter = max(n,100)
            call dpcg(n,a,adiag,acol_ptr,arow_ind,l,ldiag,
     +           lcol_ptr,lrow_ind,b,rtol,maxiter,x,
     +           iters,info,w,w(n+1),w(2*n+1),w(3*n+1))

            if (info .eq. 2) then
               write (*,*) 'Indefinite matrix'
               call dssyax(n,a,adiag,acol_ptr,arow_ind,w,w(n+1))
               write (*,*) 'Negative curvature',
     +            ddot(n,w,1,w(n+1),1)/dnrm2(n,w,1)**2
            end if

            call dtimer(ftime)

c           Compute relative error and function value.

            call dssyax(n,a,adiag,acol_ptr,arow_ind,x,w)

            fval = 0.5*ddot(n,x,1,w,1) - ddot(n,b,1,x,1)
            do i = 1, n
               w(i) = w(i) - b(i)
            end do
            rerr = dnrm2(n,w,1)/dnrm2(n,b,1)

            nnzl = lcol_ptr(n+1) - 1
            nnza = acol_ptr(n+1) - 1

            write (nwrite,1000) prob_name(8:30), n, p(j), nnz, nnzl,
     +         dble(nnzl+n)/dble(nnz+n),
     +         dble(nnza+n*p(j)-nnzl)/dble(nnza+n*p(j)),
     +         iters,alpha,info,rerr,fval,ftime-stime
         end do

      end do

      close(nread)
      close(nwrite)
      close(mmunit)

      stop

 1000 format (1x,a23/,
     +        1x,'Order of the matrix                     ',3x,i10/,
     +        1x,'Memory parameter p                      ',3x,i10/,
     +        1x,'Number of nonzeroes in A                ',3x,i10/,
     +        1x,'Number of nonzeroes in L                ',3x,i10/,
     +        1x,'Memory usage in L nnz(L)/nnz(A)         ',3x,f10.2/,
     +        1x,'Wasted memory in L                      ',3x,f10.2/,
     +        1x,'Number of conjugate gradient iterations ',3x,i10/,
     +        1x,'Final shift                             ',3x,d10.3/,
     +        1x,'Info value for pcg                      ',3x,i10/,
     +        1x,'Relative error in the residual          ',3x,d10.3/,
     +        1x,'Function value                '          ,3x,d20.8/,
     +        1x,'Time for icf and pcg                    ',3x,d10.3/)

      end


C
C**************************  RELXOM ************************************
C
C  calculate relaxation parameter for the case NLRELX=2, using
C  Huyakorn et al's adaptation (WRR 1986 22(13), pg 1795) of Cooley's
C  empirical scheme (WRR 1983 19(5), pg 1274)
C
C***********************************************************************
C
      SUBROUTINE RELXOM(N,ITER,OMEGA,OMEGAP,PIKMXV,PNEW,POLD)
C
      IMPLICIT  NONE
      INTEGER   K
      INTEGER   N,ITER
      REAL*8    ZETA,DIFF,DIFMX,DIFAB,DIFABM,DIFMXP
      REAL*8    OMEGA,OMEGAP
      REAL*8    PIKMXV(*),PNEW(*),POLD(*)
C
      IF (ITER .EQ. 1) THEN
         OMEGA=1.0D0
      ELSE
         DIFABM=0.0D0
         DO K=1,N
            DIFF=PNEW(K)-POLD(K)
            DIFAB=DABS(DIFF)
            IF (DIFAB .GE. DIFABM) THEN
               DIFABM=DIFAB
               DIFMX=DIFF
            END IF
         END DO
         DIFMXP=PIKMXV(ITER-1)
         ZETA=DIFMX/(OMEGAP*DIFMXP)
         IF (ZETA .GE. -1.0D0) THEN
            OMEGA=(3.0D0 + ZETA)/(3.0D0 + DABS(ZETA))
         ELSE
            OMEGA=0.5D0/DABS(ZETA)
         END IF
      END IF
      OMEGAP=OMEGA
C
      RETURN
      END


! This code is from QEDMOD with minor modifications.
!
!   V.M. Shabaev, I.I. Tupitsyn, V.A. Yerokhin, QEDMOD: Fortran program for calculating
!   the model Lamb-shift operator, Computer Physics Communications, Volume 189, 2015,
!   Pages 175-181, ISSN 0010-4655, http://dx.doi.org/10.1016/j.cpc.2014.12.002.
!
! For a description of the changes made, see the README.
!
! Original code is distributed under the CPC non-profit use licence
! (http://cpc.cs.qub.ac.uk/licence/licence.html). Redistributed here as part of
! GRASP2K with permission from the authors.
!
! Originally from: f/potgen/uehling.f
!
c       =================================================
        subroutine SHAB_uehling(maxii,r,unuc,uehl)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
c       This subroutine computes Uehling vacuum-polarization
c       potential by using the accurate approximation method,
c       from the paper:
c       L.W.Fullerton and G.A. Rinker, PRA 13, 1283 (1976)
c       This approximation may be applied to point-charge
c       or to finite charge distribution nuclear models.
c
c       Input parameters:
c       r(i)    .... Radial grid
c       maxii   .... the size of the r array
c       unuc(i) .... Nonpoint part of the nuclear potential
c                    multiplied by r(i).
c
c       Output parameters:
c       uehl(i) .... Uehling potential multiplied by r(i).
c
c       Common block parameters:
c       cl     ..... Speed of light
c       z      ..... Nuclear charge
c       knucl  ..... Nuclear model (0 - point, 1 - Fermi model,
c                    2 - uniform sphere)
c       ii     ..... Number of grid points
c       h, al, bt ..... Parameter of the radial grid
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        implicit real*8 (a-h,o-z)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        common /SHAB_const/cl,z
        common /SHAB_grd/h,r1,r2,al,bt,ii
        common /SHAB_ncls/knucl,inucl,rnucl
        real*8 r(maxii),unuc(maxii),uehl(maxii)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        parameter (pi=3.1415926535897932385d0)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
c       Calculation Uehling potential times on r
c       - - - - - - - - - - - - - - - - - - - - - - - - -
	Do i=1,maxii
          uehl(i)=0.D0
	End do
c       - - - - - - - - - - - - - - - - - - - - - - - - -
	coef1=-2.d0/3.d0/cl**2/(4*pi)
	coef2=-z*8.d0/3.d0/cl/(4*pi)
	ir=0
 500    ir=ir+1
        rr=r(ir)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        if (knucl.ne.0) then
c       - - - - - - - - - - - - - - - - - - - - - - - - -
c       Finite charge nuclear distribution
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        npoints=4*maxii
        r0=r(1)/10
        rmax=r(inucl)
        tmax=dlog(rmax)
        tmin=dlog(r0)
        hi=(tmax-tmin)/(npoints-1)
        fint=0.d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        do i=1,npoints
c       - - - - - - - - - - - - - - - - - - - - - - - - -
          t=hi*(i-1)+tmin
          ri=dexp(t)
          x1=2*cl*dabs(rr-ri)
          x2=2*cl*dabs(rr+ri)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
          d1=SHAB_dk0(x1)
          d2=SHAB_dk0(x2)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
          fu=d1-d2
          fr=fu*SHAB_ro_nucl(ri)*ri
          fr=fr*ri
          fint=fint+fr*hi
          if (i.eq.npoints-2) then
            fr1=fr
          endif
          if (i.eq.npoints-1) then
            fr2=fr
          endif
          if (i.eq.npoints) then
            fr3=fr
          endif
        enddo
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        dfr=(3*fr3-4*fr2+fr1)/(2*hi)
        fint=fint-0.5d0*hi*fr3-hi**2/12.d0*dfr
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        Uehl(ir)=fint*coef1
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        else
c       - - - - - - - - - - - - - - - - - - - - - - - - -
c       Point nuclear distribution
c       - - - - - - - - - - - - - - - - - - - - - - - - -
          x=2*cl*rr
          Uehl(ir)=SHAB_dk1(x)*coef2
        endif
c       - - - - - - - - - - - - - - - - - - - - - - - - -
	If (dabs(uehl(ir)/r(ir)).gt.1.D-40.and.ir.lt.ii) go to 500
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        imax=ir
        uehl(ii+3)=imax
        uehl(ii+4)=0.d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        return
        end
c       =================================================
        real*8 function SHAB_dk0(x)
        implicit real*8(a-h,o-z)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
c       Fullerton'a and Rinker'a, PRA 13, 1283 (1976)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        SHAB_dk0=0.d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        a0= 0.88357293375d0
        a1=-0.28259817381d0
        a2=-0.58904879578d0
        a3= 0.12500133434d0
        a4=-0.032729913852d0
        a5= 0.0082888574511d0
        a6=-0.0000103277658d0
        a7= 0.0000636436689d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        b0=-319.999594323d0
        b1= 2.53900995981d0
        b2= 1.d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        c0=-319.999594333d0
        c1= 2.53901020662d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        d0=5.018065179d0
        d1=71.51891262d0
        d2=211.6209929d0
        d3=31.40327478d0
        d4=-1.d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        e0=2.669207401d0
        e1=51.72549669d0
        e2=296.9809720d0
        e3=536.4324164d0
        e4=153.5335924d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        if(x.ge.0.d0.and.x.le.1.d0) then
          dp7=a0+a1*x+a2*x**2+a3*x**3+a4*x**4+a5*x**5
     #        +a6*x**6+a7*x**7
          dp21=(b0+b1*x**2+b2*x**4)/(c0+c1*x**2)
          if (x.gt.0) then
            SHAB_dk0=dp7+x*dp21*dlog(x)
          else
            SHAB_dk0=dp7
          endif
        else
          SHAB_dk0=dexp(-x)*(d0+d1/x+d2/x**2+d3/x**3+d4/x**4)
     #        /(dsqrt(x))**3/(e0+e1/x+e2/x**2+e3/x**3+e4/x**4)
        end if
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        return
        end
c       =================================================
        real*8 function SHAB_dk1(x)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        implicit real*8(a-h,o-z)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
c       Fullerton'a and Ringer'a, PRA 13, 1283 (1976)
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        SHAB_dk1=0.d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        a0=-0.71740181754d0
        a1= 1.1780972274d0
        a2=-0.37499963087d0
        a3= 0.13089675530d0
        a4=-0.038258286439d0
        a5=-0.0000242972873d0
        a6=-0.0003592014867d0
        a7=-0.0000171700907d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        b0=-64.0514843293d0
        b1= 0.711722714285d0
        b2= 1.d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        c0= 64.0514843293d0
        c1= -0.711722686403d0
        c2=0.0008042207748
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        d0=217.2386409d0
        d1=1643.364528d0
        d2=2122.244512d0
        d3=-45.12004044d0
        d4=1.d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        e0=115.5589983d0
        e1=1292.191441d0
        e2=3831.198012d0
        e3=2904.410075d0
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        if(x.ge.0.d0.and.x.le.1.d0) then
          dp7=a0+a1*x+a2*x**2+a3*x**3+a4*x**4+a5*x**5
     #        +a6*x**6+a7*x**7
          dp21=(b0+b1*x**2+b2*x**4)/(c0+c1*x**2+c2*x**4)
          SHAB_dk1=dp7+dp21*dlog(x)
        else
          SHAB_dk1=dexp(-x)*(d0+d1/x+d2/x**2+d3/x**3+d4/x**4)
     #        /(dsqrt(x))**3/(e0+e1/x+e2/x**2+e3/x**3)
        end if
c       - - - - - - - - - - - - - - - - - - - - - - - - -
        return
        end


module zmom_mod
! Berechnet den Z-Impuls

USE Variablen
USE derivatives_x_mod
USE derivatives_y_mod
USE derivatives_z_mod
USE neumann_mod
contains

subroutine zmom

!delta_u, delta_v , delta_w
! werden als subroutinen genutzt

! dphi Hilfsvariable
! rho_w*u-tau_xz
dphi(1:m_local,1:n_local,1:o_local)=&
rho_w(1:m_local,1:n_local,1:o_local)*&
u(1:m_local,1:n_local,1:o_local)+&
tau_xz(1:m_local,1:n_local,1:o_local)

!ableiten
CALL Dphi_Dx_p(dphi,delta_u,qxi_i,qxi_nr,qxi_r)

! rho_w*v-tau_zy
dphi(1:m_local,1:n_local,1:o_local)=&
rho_w(1:m_local,1:n_local,1:o_local)*&
v(1:m_local,1:n_local,1:o_local)-&
tau_zy(1:m_local,1:n_local,1:o_local)

!ableiten
CALL Dphi_Dy_p(dphi,delta_v,qyi_i,qyi_nr,qyi_r)

! rho_w*w+p-tau_zz
dphi(1:m_local,1:n_local,1:o_local)=&
rho_w(1:m_local,1:n_local,1:o_local)*&
w(1:m_local,1:n_local,1:o_local)+&
p(1:m_local,1:n_local,1:o_local)-&
tau_zz(1:m_local,1:n_local,1:o_local)

!ableiten
CALL Dphi_Dz_p(dphi,delta_w,qzi_i,qzi_nr,qzi_r)

!!bc neumann
!call bcneu

!rho_u_neu=rho_u-dt*(delta_u+delta_v+delta_w)
rho_w_neu(1:m_local,1:n_local,1:o_local)=&
rho_w(1:m_local,1:n_local,1:o_local)-&
dt*(delta_u(1:m_local,1:n_local,1:o_local)+&
delta_v(1:m_local,1:n_local,1:o_local)+&
delta_w(1:m_local,1:n_local,1:o_local))

end subroutine

end module


    subroutine diss_oxygen
!   **********************
    use constante, only: PI_n_O2_atm
    implicit none
    include 'combine_foam.inc'

!   full dynamic exchanges between surface and atmosphere
!   Calcul de Cste Henry pour oxygen
    do j=1,nbasin-1
        RT0=22.414d-03
        dum=temp_box(j)/100.
        xlogK=-58.3877+85.8079/dum+23.8439*dlog(dum) &
              +salin(j)*(-0.034892+dum*(-0.0019387*dum+0.015568))
        beta(j)=dexp(xlogK)/RT0
        beta(j)=beta(j)*0.21
    end do
    ! Set beta = 0 for atmospheric box to avoid numeric error while summing on all basins
    beta(nbasin) = 0

!   surface ocean at steady-state with the atmosphere:
    xnum=var(11,nbasin)
    somme=0.
    do j=1,nbasin
        somme = somme + beta(j)*vol(j)*indice_surface(j)
    enddo
    xden=PI_n_O2_atm + somme
    po2=xnum/xden
    do j0=1,nsurface
        j = jbox_surface(j0)
        var(11,j)=beta(j)*po2
    enddo
    return
    end


!---------------------------------------------------------------------------------------------------
! All the formulations  have been taken from the original
! article,the Span-Wagner EoS for CO2.
! The EoS has been published in J. Phys. Chem. Ref. Data, Vol.25,
! pp. 1509-1596, 1996.
!---------------------------------------------------------------------------------------------------

!--------------------------------------------------------------------------------------------------
! SUBROUTINE   deriv_disfonc
! @brief   Compute distance fuction p.1545 Table 32
! @comments All distance fuctions and their derivatives are involved only in
!           termes 40-42 of the residual part
! @authors Yu Fang
! @date    26-10-2016
!----------------------------------------------------------------------------------------------------
SUBROUTINE   deriv_disfonc ( T,v,trib40, trib41, trib42,&
                             dtrib_ddelt40, dtrib_ddelt41, dtrib_ddelt42,&
                             dtrib_dtau40, dtrib_dtau41, dtrib_dtau42,&
                             ddtrib_dddelt40, ddtrib_dddelt41, ddtrib_dddelt42,&
                             ddtrib_ddtau40,ddtrib_ddtau41, ddtrib_ddtau42,&
                             ddtrib_ddeltdtau40, ddtrib_ddeltdtau41,ddtrib_ddeltdtau42 )

     USE def_constants

     IMPLICIT NONE

!IN/OUT
     REAL(pr) :: T,v

     REAL(pr) :: trib40, trib41, trib42

     REAL(pr) :: dtrib_ddelt40, dtrib_ddelt41, dtrib_ddelt42,&
                 dtrib_dtau40, dtrib_dtau41, dtrib_dtau42

     REAL(pr) :: ddtrib_dddelt40, ddtrib_dddelt41, ddtrib_dddelt42,&
                 ddtrib_ddtau40,ddtrib_ddtau41, ddtrib_ddtau42,&
                 ddtrib_ddeltdtau40,ddtrib_ddeltdtau41,ddtrib_ddeltdtau42

!LOCAL
    REAL(pr) :: tri40,tri41,tri42,th

    REAL(pr) :: tau, delt, rho

    REAL(pr) :: dtri_ddelt40, dtri_ddelt41, dtri_ddelt42,&
                 ddtri_dddelt40, ddtri_dddelt41, ddtri_dddelt42

    REAL(pr) :: deltm1,invdeltm1,deltm1_s,invbet,inv2betm1,inv2betm2

!Preparation of  variables

    rho        = 1_pr    / v
    delt       = 1_pr     / (rho_cr*v)
    tau        = T_cr    / T
!
!
    deltm1     = delt    - 1_pr
    invdeltm1  = 1_pr    / deltm1
    deltm1_s  = deltm1 * deltm1
    invbet     = 1_pr    / bet40
    inv2betm1  = 1_pr    / (2_pr * bet40) - 1_pr
    inv2betm2  = 1_pr    / (2_pr * bet40) - 2_pr

    th       = 1_pr - tau + AA40 * deltm1_s**(invbet/2_pr)
    tri40    = th**2_pr + BB40 * deltm1_s**a40
    tri41    = th**2_pr + BB41 * deltm1_s**a41
    tri42    = th**2_pr + BB42 * deltm1_s**a42
!
!
!------------------------------------------------------------------------------------------------------------------------
!Compute triangle (DELTA)^bi 40-42
!------------------------------------------------------------------------------------------------------------------------
    trib40 = tri40**(b40)
    trib41 = tri41**(b41)
    trib42 = tri42**(b42)
!
!
!--------------------------------------------------------------------------------------------------------------------------
!Compute derivatives of triangle (DELTA) 40-42
!--------------------------------------------------------------------------------------------------------------------------

    dtri_ddelt40 = deltm1 * (AA40 * th * 2_pr / bet40 * deltm1_s**inv2betm1 + 2_pr * BB40 * a40 * deltm1_s**(a40-1_pr))
!    print*, dtri_ddelt40
    dtri_ddelt41 = deltm1 * (AA40 * th * 2_pr / bet40 * deltm1_s**inv2betm1 + 2_pr * BB41 * a41 * deltm1_s**(a41-1_pr))

    dtri_ddelt42 = deltm1 * (AA40 * th * 2_pr / bet40 * deltm1_s**inv2betm1 + 2_pr * BB42 * a42 * deltm1_s**(a42-1_pr))

!
!
    ddtri_dddelt40 =  invdeltm1 * dtri_ddelt40 + deltm1_s * (4_pr * BB40 * a40 * (a40 - 1_pr) * deltm1_s**(a40-2_pr) + &
                                                              2_pr * AA40**2_pr * invbet**2_pr * (deltm1_s**inv2betm1)**2_pr +&
                                                              AA40 * th * 4_pr * invbet * inv2betm1 * deltm1_s**inv2betm2)

    ddtri_dddelt41 =  invdeltm1 * dtri_ddelt41 + deltm1_s * (4_pr * BB41 * a41 * (a41 - 1_pr) * deltm1_s**(a41-2_pr) + &
                                                              2_pr * AA40**2_pr * invbet**2_pr * (deltm1_s**inv2betm1)**2_pr +&
                                                              AA40 * th * 4_pr* invbet * inv2betm1 * deltm1_s**inv2betm2)

    ddtri_dddelt42 =  invdeltm1 * dtri_ddelt42 + deltm1_s * (4_pr * BB42 * a42 * (a42 - 1_pr) * deltm1_s**(a42-2_pr) + &
                                                              2_pr * AA40**2_pr * invbet**2_pr * (deltm1_s**inv2betm1)**2_pr +&
                                                              AA40 * th *4_pr* invbet * inv2betm1 * deltm1_s**inv2betm2)
!
!
!--------------------------------------------------------------------------------------------------------------------------------
!Compute derivatives of triangle^bi  40-42
!--------------------------------------------------------------------------------------------------------------------------------

    dtrib_ddelt40 = b40 * tri40**(b40-1_pr) * dtri_ddelt40
!    print*, dtrib_ddelt40,b40,tri40,dtri_ddelt40

    dtrib_ddelt41 = b41 * tri41**(b41-1_pr) * dtri_ddelt41

    dtrib_ddelt42 = b42 * tri42**(b42-1_pr) * dtri_ddelt42
!
!
    dtrib_dtau40  =  - 2_pr * th * b40 * tri40**(b40-1_pr)

    dtrib_dtau41  =  - 2_pr * th * b41 * tri41**(b41-1_pr)

    dtrib_dtau42  =  - 2_pr * th * b42 * tri42**(b42-1_pr)
!
!
    ddtrib_dddelt40 = b40 * (tri40**(b40-1_pr) * ddtri_dddelt40 + (b40 - 1_pr) * tri40**(b40-2_pr) * dtri_ddelt40**2_pr)

    ddtrib_dddelt41 = b41 * (tri41**(b41-1_pr) * ddtri_dddelt41 + (b41 - 1_pr) * tri41**(b41-2_pr) * dtri_ddelt41**2_pr)

    ddtrib_dddelt42 = b42 * (tri42**(b42-1_pr) * ddtri_dddelt42 + (b42 - 1_pr) * tri42**(b42-2_pr) * dtri_ddelt42**2_pr)
!
!
    ddtrib_ddtau40 = 2_pr * b40 * tri40**(b40-1_pr) + 4_pr * th**2_pr * b40 * (b40 - 1_pr) * tri40**(b40-2_pr)

    ddtrib_ddtau41 = 2_pr * b41 * tri41**(b41-1_pr) + 4_pr * th**2_pr * b41 * (b41 - 1_pr) * tri41**(b41-2_pr)

    ddtrib_ddtau42 = 2_pr * b42 * tri42**(b42-1_pr) + 4_pr * th**2_pr * b42 * (b42 - 1_pr) * tri42**(b42-2_pr)
!
!
    ddtrib_ddeltdtau40 = - AA40 * b40 * 2_pr * invbet * tri40**(b40-1_pr) * deltm1 * deltm1_s**inv2betm1&
                         - 2_pr * th * b40 * (b40 - 1_pr) * tri40**(b40-2_pr) * dtri_ddelt40

    ddtrib_ddeltdtau41 = - AA40 * b41 * 2_pr * invbet * tri41**(b41-1_pr) * deltm1 * deltm1_s**inv2betm1&
                         - 2_pr * th * b41 * (b41 - 1_pr) * tri41**(b41-2_pr) * dtri_ddelt41

    ddtrib_ddeltdtau42 = - AA40 * b42 * 2_pr * invbet * tri42**(b42-1_pr) * deltm1 * deltm1_s**inv2betm1&
                         - 2_pr * th * b42 * (b42 - 1_pr) * tri42**(b42-2_pr) * dtri_ddelt42

!
!
END SUBROUTINE deriv_disfonc


MODULE mo_particle_external_properties
  USE mo_submctl, ONLY : pi6, eps, rg, surfw0, grav, spec
  USE classSection, ONLY : Section
  IMPLICIT NONE

  ! This module contains a collection of function to calculate physical and thermodynamical particle properties,
  ! such as diameters, fall velocities, equilibirium saturation ratios at a droplet surface etc.

  CONTAINS

    !
    ! Function for calculating terminal velocities for different particle types and size ranges.
    !     Tomi Raatikainen (2.5.2017)
    !     - Changed from radius to diameter since ~the rest of the model as well as the calculations below take diameter anyway! -Juha
    REAL FUNCTION terminal_vel(diam,rhop,rhoa,visc,beta,flag)
      IMPLICIT NONE
      REAL, INTENT(in) :: diam, rhop ! Particle diameter and density
      REAL, INTENT(in) :: rhoa, visc, beta ! Air density, viscocity and Cunningham correction factor
      INTEGER, INTENT(IN) :: flag ! Parameter for identifying aerosol (1), cloud droplets (2), precip (3), ice (4) and snow(5)
      ! Constants
      REAL, PARAMETER :: rhoa_ref = 1.225 ! reference air density (kg/m^3)

      terminal_vel = 0.
      IF( ANY(flag == [1,2,3])) THEN
         ! Aerosol and cloud and rain droplets
         IF (diam<80.0e-6) THEN
            ! Stokes law with Cunningham slip correction factor
            terminal_vel = (diam**2)*(rhop-rhoa)*grav*beta/(18.*visc) !(4.*radius**2)*(rhop-rhoa)*grav*beta/(18.*visc) ![m s-1]
         ELSE IF (diam<1.2e-3) THEN
            ! Droplets from 40 um to 0.6 mm: linear dependence on particle radius and a correction for reduced pressure
            !   R.R. Rogers: A Short Course in Cloud Physics, Pergamon Press Ltd., 1979.
            terminal_vel = 4.e3*diam*SQRT(rhoa_ref/rhoa)!8.e3*radius*sqrt(rhoa_ref/rhoa)
         ELSE
            ! Droplets larger than 0.6 mm: square root dependence on particle radius and a correction for reduced pressure
            !   R.R. Rogers: A Short Course in Cloud Physics, Pergamon Press Ltd., 1979.
            ! Note: this is valid up to 2 mm or 9 m/s (at 1000 mbar), where droplets start to break
            terminal_vel = 2.01e2*SQRT( MIN(diam/2.,2.0e-3)*rhoa_ref/rhoa ) !2.01e2*SQRT( MIN(radius,2.0e-3)*rhoa_ref/rhoa)
         END IF
      ELSE IF (flag==4) THEN   ! Ice
         ! Ice crystal terminal fall speed from Ovchinnikov et al. (2014)
         terminal_vel = 12.0*SQRT(diam)
      ELSE IF (flag==5) THEN   ! Snow
         ! The same for snow
         terminal_vel = 12.0*SQRT(diam)
      END IF

    END FUNCTION terminal_vel

    !!!!!!!!!!! THEN DIAMETER CALCULATIONS BELOW COULD BE PUT UNDER A COMMON INTERFACE TO FACILITATE DIFFERENT DATATYPES!!!!!
    !
    ! Function for calculating dimension (or wet diameter) for any particle type
    ! - Aerosol, cloud and rain are spherical
    ! - Snow and ice can be irregular and their densities can be size-dependent
    !
    ! Correct dimension is needed for irregular particles (e.g. ice and snow) for calculating fall speed (deposition and coagulation)
    ! and capacitance (condensation). Otherwise compact spherical structure can be expected,
    !
    SUBROUTINE calcDiamSALSA(n,ppart,dia)
      IMPLICIT NONE
      INTEGER, INTENT(in) :: n
      TYPE(Section), INTENT(in) :: ppart(n)
      REAL, INTENT(OUT) :: dia(n)
      INTEGER :: i,nspec

      nspec = spec%getNSpec()

      dia(:) = 10.e-9
      DO i=1,n
         IF (ppart(i)%numc > ppart(i)%nlim) &
              dia(i)=(SUM(ppart(i)%volc(1:nspec))/ppart(i)%numc/pi6)**(1./3.)
      END DO

    END SUBROUTINE calcDiamSALSA

    !
    ! Function for calculating effective (wet) radius for any particle type
    ! - Aerosol, cloud and rain are spherical
    ! - Snow and ice can be irregular and their densities can be size-dependent
    !
    ! Correct dimension is needed for irregular particles (e.g. ice and snow) for calculating fall speed (deposition and coagulation)
    ! and capacitance (condensation). Otherwise compact spherical structure can be expected,
    !
    REAL FUNCTION calcDiamLES(n,numc,mass,flag)
      USE mo_submctl, ONLY : pi6
      IMPLICIT NONE
      INTEGER, INTENT(IN) :: n ! Number of species
      INTEGER, INTENT(IN) :: flag ! Parameter for identifying aerosol (1), cloud droplets (2), precip (3), ice (4) and snow(5) particle phases
      REAL, INTENT(IN) :: numc, mass(n)

      calcDiamLES=0.

      ! Don't calculate if very low number concentration
      IF (numc<1e-15) RETURN

      IF (flag==4) THEN   ! Ice
         ! Spherical ice
         calcDiamLES=( SUM(mass(:)/spec%rhoice(:))/numc/pi6)**(1./3.)
      ELSE IF (flag==5) THEN   ! Snow
         ! Spherical snow
         calcDiamLES=( SUM(mass(:)/spec%rhosnow(:))/numc/pi6)**(1./3.)
      ELSE
         ! Radius from total volume of a spherical particle or aqueous droplet
         calcDiamLES=( SUM(mass(:)/spec%rholiq(:))/numc/pi6)**(1./3.)
      ENDIF

    END FUNCTION calcDiamLES

    !
    ! Function for calculating equilibrium water saturation ratio at droplet surface based on Köhler theory
    !
    REAL FUNCTION calcSweq(part,T)
      TYPE(Section), INTENT(in) :: part ! Any particle
      REAL, INTENT(IN) :: T ! Absolute temperature (K)
      REAL :: dwet

      REAL :: znw,zns ! Moles of water and soluble material
      REAL :: zvw, zvs, zvtot ! Volume concentrations of water and soluble material and total dry
      INTEGER :: iwa, ndry ! Index for water, number of "dry" species
      INTEGER :: i

      iwa = spec%getIndex("H2O")
      ndry = spec%getNSpec(type="dry")

      ! Wet diameter  !! USE THE FUNCTIONS PROVIDED FOR THIS??
      dwet = (SUM(part%volc(:))/part%numc/pi6)**(1./3.)

      ! Equilibrium saturation ratio = xw*exp(4*sigma*v_w/(R*T*Dwet))

      znw = part%volc(iwa)*spec%rhowa/spec%mwa
      zvw = part%volc(iwa)*spec%rhowa/spec%mwa
      zns = 0.
      zvs = 0.
      zvtot = 0.
      DO i = 1,ndry
         zns = zns + spec%diss(i)*part%volc(i)*spec%rholiq(i)/spec%MM(i)
         zvs = zvs + MIN(1.,spec%diss(i)) * part%volc(i) ! Use "diss" here just to select the soluble species
         zvtot = zvtot + part%volc(i)
      END DO

      ! Combine the two cases from original code since they're exactly the same??
      IF (zvw > 1.e-28*part%numc .OR. zvs > 1.e-28*part%numc) THEN
         ! Aqueous droplet OR dry partially soluble particle
         calcSweq = (znw/(zns+znw)) * exp(4.*surfw0*spec%mwa/(rg*T*spec%rhowa*dwet))
      ELSE IF (zvtot-zvs > 1.e-28*part%numc) THEN
         ! Dry insoluble particle
         calcSweq = exp(4.*surfw0*spec%mwa/(rg*T*spec%rhowa*dwet))
      ELSE
         ! Just add eps to avoid divide by zero
         calcSweq = (znw/(eps+zns+znw)) * exp(4.*surfw0*spec%mwa/(rg*T*spec%rhowa*dwet))
      END IF

    END FUNCTIOn calcSweq

  END MODULE mo_particle_external_properties


!-------------------------------------------------------------------------------------------------------------------------------
! This module contains all the filters
MODULE Filters
!...............................................................................................................................

    IMPLICIT NONE

CONTAINS

    !-------------------------------------------------------------------------------------------------------------------------------
    ! Discrete time Low-Pass Filter
    REAL FUNCTION LPFilter(InputSignal, DT, CornerFreq, iStatus, reset, inst)
    !...............................................................................................................................

            ! Inputs

        REAL(4), INTENT(IN)         :: InputSignal
        REAL(4), INTENT(IN)         :: DT                       ! time step [s]
        REAL(4), INTENT(IN)         :: CornerFreq               ! corner frequency [rad/s]
        INTEGER(4), INTENT(IN)      :: iStatus                  ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation.
        INTEGER(4), INTENT(INOUT)   :: inst                     ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other.
        LOGICAL(4), INTENT(IN)      :: reset                    ! Reset the filter to the input signal

            ! Local

        REAL(4), DIMENSION(99), SAVE    :: InputSignalLast      ! Input signal the last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE    :: OutputSignalLast ! Output signal the last time this filter was called. Supports 99 separate instances.

            ! Initialization

        IF ((iStatus == 0) .OR. reset) THEN
            OutputSignalLast(inst) = InputSignal
            InputSignalLast(inst) = InputSignal
        ENDIF

            ! Body

        LPFilter = (DT*CornerFreq*InputSignal + DT*CornerFreq*InputSignalLast(inst) - (DT*CornerFreq-2.0)*OutputSignalLast(inst))/(DT*CornerFreq+2.0)

            ! Save signals for next time step

        InputSignalLast(inst)  = InputSignal
        OutputSignalLast(inst) = LPFilter
        inst = inst + 1

    END FUNCTION LPFilter
    !-------------------------------------------------------------------------------------------------------------------------------
    ! Discrete time second order Low-Pass Filter
    REAL FUNCTION SecLPFilter(InputSignal, DT, CornerFreq, Damp, iStatus, reset, inst)
    !...............................................................................................................................

        IMPLICIT NONE

            ! Inputs

        REAL(4), INTENT(IN)         :: InputSignal
        REAL(4), INTENT(IN)         :: DT                       ! time step [s]
        REAL(4), INTENT(IN)         :: CornerFreq               ! corner frequency [rad/s]
        REAL(4), INTENT(IN)         :: Damp                     ! Dampening constant
        INTEGER(4), INTENT(IN)      :: iStatus                  ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation.
        INTEGER(4), INTENT(INOUT)   :: inst                     ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other.
        LOGICAL(4), INTENT(IN)      :: reset                    ! Reset the filter to the input signal

            ! Local

        REAL(4), DIMENSION(99), SAVE :: InputSignalLast1    ! Input signal the last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE :: InputSignalLast2    ! Input signal the next to last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE :: OutputSignalLast1   ! Output signal the last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE :: OutputSignalLast2   ! Output signal the next to last time this filter was called. Supports 99 separate instances.

            ! Initialization

        IF ((iStatus == 0) .OR. reset )  THEN
            OutputSignalLast1(inst)  = InputSignal
            OutputSignalLast2(inst)  = InputSignal
            InputSignalLast1(inst)   = InputSignal
            InputSignalLast2(inst)   = InputSignal
        ENDIF

            ! Body

        SecLPFilter = 1/(4+4*DT*Damp*CornerFreq+DT**2*CornerFreq**2) * ( (8-2*DT**2*CornerFreq**2)*OutputSignalLast1(inst) &
                        + (-4+4*DT*Damp*CornerFreq-DT**2*CornerFreq**2)*OutputSignalLast2(inst) + (DT**2*CornerFreq**2)*InputSignal &
                            + (2*DT**2*CornerFreq**2)*InputSignalLast1(inst) + (DT**2*CornerFreq**2)*InputSignalLast2(inst) )

            ! Save signals for next time step

        InputSignalLast2(inst)   = InputSignalLast1 (inst)
        InputSignalLast1(inst)   = InputSignal
        OutputSignalLast2(inst)  = OutputSignalLast1 (inst)
        OutputSignalLast1(inst)  = SecLPFilter
        inst = inst + 1

    END FUNCTION SecLPFilter
    !-------------------------------------------------------------------------------------------------------------------------------
    ! Discrete time High-Pass Filter
    REAL FUNCTION HPFilter( InputSignal, DT, CornerFreq, iStatus, reset, inst)
    !...............................................................................................................................

            ! Inputs

        REAL(4), INTENT(IN)     :: InputSignal
        REAL(4), INTENT(IN)     :: DT                       ! time step [s]
        REAL(4), INTENT(IN)     :: CornerFreq               ! corner frequency [rad/s]
        INTEGER, INTENT(IN)     :: iStatus                  ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation.
        INTEGER, INTENT(INOUT)  :: inst                     ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other.
        LOGICAL(4), INTENT(IN)  :: reset                    ! Reset the filter to the input signal

            ! Local

        REAL(4)                         :: K                        ! Constant gain
        REAL(4), DIMENSION(99), SAVE    :: InputSignalLast      ! Input signal the last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE    :: OutputSignalLast ! Output signal the last time this filter was called. Supports 99 separate instances.

            ! Initialization

        IF ((iStatus == 0) .OR. reset)  THEN
            OutputSignalLast(inst) = InputSignal
            InputSignalLast(inst) = InputSignal
        ENDIF

        K = 2.0 / DT

            ! Body

        HPFilter = K/(CornerFreq + K)*InputSignal - K/(CornerFreq + K)*InputSignalLast(inst) - (CornerFreq - K)/(CornerFreq + K)*OutputSignalLast(inst)

            ! Save signals for next time step

        InputSignalLast(inst)   = InputSignal
        OutputSignalLast(inst)  = HPFilter
        inst = inst + 1

    END FUNCTION HPFilter
    !-------------------------------------------------------------------------------------------------------------------------------
    ! Discrete time inverted Notch Filter with descending slopes, G = CornerFreq*s/(Damp*s^2+CornerFreq*s+Damp*CornerFreq^2)
    REAL FUNCTION NotchFilterSlopes(InputSignal, DT, CornerFreq, Damp, iStatus, reset, inst)
    !...............................................................................................................................

            ! Inputs

        REAL(4), INTENT(IN)     :: InputSignal
        REAL(4), INTENT(IN)     :: DT                       ! time step [s]
        REAL(4), INTENT(IN)     :: CornerFreq               ! corner frequency [rad/s]
        REAL(4), INTENT(IN)     :: Damp                     ! Dampening constant
        INTEGER, INTENT(IN)     :: iStatus                  ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation.
        INTEGER, INTENT(INOUT)  :: inst                     ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other.
        LOGICAL(4), INTENT(IN)  :: reset                    ! Reset the filter to the input signal

            ! Local

        REAL(4), DIMENSION(99), SAVE :: InputSignalLast1    ! Input signal the last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE :: InputSignalLast2    ! Input signal the next to last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE :: OutputSignalLast1   ! Output signal the last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE :: OutputSignalLast2   ! Output signal the next to last time this filter was called. Supports 99 separate instances.

            ! Initialization

        IF ((iStatus == 0) .OR. reset) THEN
            OutputSignalLast1(inst)  = InputSignal
            OutputSignalLast2(inst)  = InputSignal
            InputSignalLast1(inst)   = InputSignal
            InputSignalLast2(inst)   = InputSignal
        ENDIF

            ! Body

        NotchFilterSlopes = 1.0/(4.0+2.0*DT*Damp*CornerFreq+DT**2.0*CornerFreq**2.0) * ( (8.0-2.0*DT**2.0*CornerFreq**2.0)*OutputSignalLast1(inst) &
                        + (-4.0+2.0*DT*Damp*CornerFreq-DT**2.0*CornerFreq**2.0)*OutputSignalLast2(inst) + &
                            (2.0*DT*Damp*CornerFreq)*InputSignal + (-2.0*DT*Damp*CornerFreq)*InputSignalLast2(inst) )

            ! Save signals for next time step

        InputSignalLast2(inst)   = InputSignalLast1(inst)
        InputSignalLast1(inst)   = InputSignal          !Save input signal for next time step
        OutputSignalLast2(inst)  = OutputSignalLast1(inst)      !Save input signal for next time step
        OutputSignalLast1(inst)  = NotchFilterSlopes
        inst = inst + 1

    END FUNCTION NotchFilterSlopes
    !-------------------------------------------------------------------------------------------------------------------------------
    ! Discrete time Notch Filter, G = (s^2 + 2*omega*betaNum*s + omega^2)/(s^2 + 2*omega*betaDen*s + omega^2)
    REAL FUNCTION NotchFilter(InputSignal, DT, omega, betaNum, betaDen, iStatus, reset, inst)
    !...............................................................................................................................

            ! Inputs

        REAL(4), INTENT(IN)     :: InputSignal
        REAL(4), INTENT(IN)     :: DT                       ! time step [s]
        REAL(4), INTENT(IN)     :: omega                    ! corner frequency [rad/s]
        REAL(4), INTENT(IN)     :: betaNum                  ! Dampening constant in numerator of filter transfer function
        REAL(4), INTENT(IN)     :: betaDen                  ! Dampening constant in denominator of filter transfer function
        INTEGER, INTENT(IN)     :: iStatus                  ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation.
        INTEGER, INTENT(INOUT)  :: inst                     ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other.
        LOGICAL(4), INTENT(IN)  :: reset                    ! Reset the filter to the input signal

            ! Local
        REAL(4)                         :: K, P1, P2, P3, P4, P5    ! Constant gain
        REAL(4), DIMENSION(99), SAVE    :: InputSignalLast1         ! Input signal the last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE    :: InputSignalLast2         ! Input signal the next to last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE    :: OutputSignalLast1        ! Output signal the last time this filter was called. Supports 99 separate instances.
        REAL(4), DIMENSION(99), SAVE    :: OutputSignalLast2        ! Output signal the next to last time this filter was called. Supports 99 separate instances.

            ! Initialization

        IF ((iStatus == 0) .OR. reset) THEN
            OutputSignalLast1(inst)  = InputSignal
            OutputSignalLast2(inst)  = InputSignal
            InputSignalLast1(inst)   = InputSignal
            InputSignalLast2(inst)   = InputSignal
        ENDIF

        K = 2/DT
        P1 = (K**2 + 2*omega*BetaNum*K + omega**2)/(K**2 + 2*omega*BetaDen*K + omega**2)
        P2 = (2*omega**2 - 2*K**2)  / (K**2 + 2*omega*BetaDen*K + omega**2);
        P3 = (K**2 - 2*omega*BetaNum*K + omega**2) / (K**2 + 2*omega*BetaDen*K + omega**2)
        P4 = (2*omega**2 - 2*K**2)  / (K**2 + 2*omega*BetaDen*K + omega**2)
        P5 = (K**2 - 2*omega*BetaDen*K + omega**2)/ (K**2 + 2*omega*BetaDen*K + omega**2)

        ! Body
        NotchFilter = P1*InputSignal + P2*InputSignalLast1(inst) + P3*InputSignalLast2(inst) - P4*OutputSignalLast1(inst) - P5*OutputSignalLast2(inst)

        ! Save signals for next time step
        InputSignalLast2(inst)   = InputSignalLast1(inst)
        InputSignalLast1(inst)   = InputSignal          !Save input signal for next time step
        OutputSignalLast2(inst)  = OutputSignalLast1(inst)      !Save input signal for next time step
        OutputSignalLast1(inst)  = NotchFilter
        inst = inst + 1

    END FUNCTION NotchFilter
    !-------------------------------------------------------------------------------------------------------------------------------
    ! Prefilter measured wind turbine signals to separate the filtering from the actual control actions
    SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst)
    !...............................................................................................................................

        USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

        TYPE(ControlParameters), INTENT(INOUT)  :: CntrPar
        TYPE(LocalVariables), INTENT(INOUT)     :: LocalVar
        TYPE(ObjectInstances), INTENT(INOUT)    :: objInst

        ! Filter the HSS (generator) speed measurement:
        ! Apply Low-Pass Filter (choice between first- and second-order low-pass filter)
        IF (CntrPar%F_LPFType == 1) THEN
            LocalVar%GenSpeedF = LPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)
        ELSEIF (CntrPar%F_LPFType == 2) THEN
            LocalVar%GenSpeedF = SecLPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, CntrPar%F_LPFDamping, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Second-order low-pass filter on generator speed
        END IF

        IF (CntrPar%F_NotchType == 1) THEN
            LocalVar%GenSpeedF = NotchFilter(LocalVar%GenSpeedF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, .FALSE., objInst%instNotch)
        END IF

        ! Filtering the tower fore-aft acceleration signal
        LocalVar%FA_AccHPF = HPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%FA_HPFCornerFreq, LocalVar%iStatus, .FALSE., objInst%instHPF)

        END SUBROUTINE PreFilterMeasuredSignals
    END MODULE Filters