!#########################################################################################
!# ***************************************************************************************
!# <name> elasticity_callback </name>
!# ***************************************************************************************
!#
!# <purpose>
!# This module contains callback functions for the elasticity problem that are
!# used during the matrix/vector assembly.
!# </purpose>
!#########################################################################################

module elasticity_callback

  implicit none

contains

! ****************************************************************************************

!<subroutine>
  subroutine elast_mat_Poisson_2D(rdiscretisationTrial, rdiscretisationTest, rform, &
                                  nel, nptsPerEl,Dpoints, IdofsTrial, IdofsTest, &
                                  rdomainIntSubset, Dcoefficients, rcollection)

    use fsystem
    use spatialdiscretisation
    use collection
    use scalarpde
    use domainintegration

!<description>
    ! This subroutine is called during the matrix assembly. It computes the coefficients
    ! in front of the terms of the bilinear form.
    ! The routine accepts a set of elements and a set of points on these elements
    ! (cubature points) in real coordinates.
    ! According to the terms in the bilinear form, the routine simultaneously computes
    ! for all these points and all the terms in the bilinear form the corresponding
    ! coefficients.
    ! The routine implements the interface defined in the file
    ! 'intf_coefficientMatrixSc.inc'.
!</description>

!<input>
    ! discretisation structure of the trial space
    type(t_spatialDiscretisation), intent(in) :: rdiscretisationTrial

    ! discretisation structure of the test space
    type(t_spatialDiscretisation), intent(in) :: rdiscretisationTest

    ! bilinear form to be evaluated
    type(t_bilinearForm), intent(in) :: rform

    ! number of elements where the coefficients are to be computed
    integer, intent(in) :: nel

    ! number of points per element where the coefficients are to be computed
    integer, intent(in) :: nptsPerEl

    ! array of all points on all elements where coefficients are to be computed
    ! DIMENSION(spatial dimension, nptsPerEl, nel)
    real(DP), dimension(:,:,:), intent(in) :: Dpoints

    ! array of trial space DOF in all elements
    ! DIMENSION(#local DOF in trial space, nel)
    integer, dimension(:,:), intent(in) :: IdofsTrial

    ! array of test space DOF in all elements
    ! DIMENSION(#local DOF in test space, nel)
    integer, dimension(:,:), intent(in) :: IdofsTest

    ! structure providing more detailed information about the current element set
    type(t_domainIntSubset), intent(in) :: rdomainIntSubset
!</input>

!<inputoutput>
    ! optional collection structure for additional information provided by the user
    type(t_collection), intent(inout), optional :: rcollection
!</inputoutput>

!<output>
    ! array of coefficients of the terms in the bilinear form for all given points on
    ! all given elements, DIMENSION(itermCount, nptsPerEl, nel) with itermCount the
    ! number of terms in the bilinear form
    real(DP), dimension(:,:,:), intent(out) :: Dcoefficients
!</output>
!</subroutine>

    Dcoefficients = 1.0_DP

  end subroutine elast_mat_Poisson_2D

! ****************************************************************************************

!<subroutine>
  subroutine elast_RHS_Poisson_2D_vol(rdiscretisation, rform, nel, nptsPerEl, Dpoints, &
                                      IdofsTest, rdomainIntSubset, Dcoefficients, &
                                      rcollection)

    use fsystem
    use spatialdiscretisation
    use derivatives
    use collection
    use scalarpde
    use domainintegration
    use elasticity_basic

!<description>
    ! This subroutine is called during the vector assembly. It computes the function f
    ! in the linear form (usually L(v) = (f,v)_0 + (g,v)_N), i.e. the volumetric
    ! contributions.
!</description>

!<input>
    ! discretisation structure
    type(t_spatialDiscretisation), intent(in) :: rdiscretisation

    ! linear form to be evaluated
    type(t_linearForm), intent(in) :: rform

    ! number of elements where the coefficients are to be computed
    integer, intent(in) :: nel

    ! number of points per element where the coefficients are to be computed
    integer, intent(in) :: nptsPerEl

    ! array of all points on all elements where coefficients are to be computed
    ! DIMENSION(spatial dimension, nptsPerEl, nel)
    real(DP), dimension(:,:,:), intent(in) :: Dpoints

    ! array of test space DOF in all elements
    ! DIMENSION(#local DOF in test space, nel)
    integer, dimension(:,:), intent(in) :: IdofsTest

    ! structure providing more detailed information about the current element set
    type(t_domainIntSubset), intent(in) :: rdomainIntSubset
!</input>

!<inputoutput>
    ! optional collection structure for additional information provided by the user
    type(t_collection), intent(inout), optional :: rcollection
!</inputoutput>

!<output>
    ! array of coefficients of the terms in the linear form for all given points on
    ! all given elements, DIMENSION(itermCount, nptsPerEl, nel) with itermCount the
    ! number of terms in the linear form
    real(DP), dimension(:,:,:), intent(out) :: Dcoefficients
!</output>
!</subroutine>

    real(DP), dimension(:,:), pointer :: Duxx, Duyy

    if (rprob%csimulation .eq. SIMUL_REAL) then
      Dcoefficients(1,:,:) = rprob%dforceVolumeX
    else if (rprob%csimulation .eq. SIMUL_ANALYTICAL) then
      ! compute Laplace operator
      allocate(Duxx(nptsPerEl,nel), Duyy(nptsPerEl,nel))
      ! Set rcollection%IquickAccess(4) = 0 in order to tell the routine elast_analFunc()
      ! that the displacement components are to be treated. (See comment in routine
      ! elast_calcErrors() in module elasticity_basic.)
      rcollection%IquickAccess(4) = 0
      call elast_analFunc(1, DER_DERIV_XX, rdiscretisation, nel, nptsPerEl, Dpoints, &
                          rdomainIntSubset, Duxx, rcollection)
      call elast_analFunc(1, DER_DERIV_YY, rdiscretisation, nel, nptsPerEl, Dpoints, &
                          rdomainIntSubset, Duyy, rcollection)
      Dcoefficients(1,:,:) = -Duxx - Duyy
      deallocate(Duxx, Duyy)
    endif

  end subroutine elast_RHS_Poisson_2D_vol

! ****************************************************************************************

!<subroutine>
  subroutine elast_RHS_Poisson_2D_bound(rdiscretisation, rform, nel, nptsPerEl, Dpoints, &
                                        ibct, DpointPar, IdofsTest, rdomainIntSubset, &
                                        Dcoefficients, rcollection)

    use fsystem
    use boundary
    use collection
    use scalarpde
    use domainintegration
    use spatialdiscretisation
    use derivatives
    use elasticity_basic

!<description>
    ! This subroutine is called during the vector assembly. It computes the function g
    ! in the linear form (usually L(v) = (f,v)_0 + (g,v)_N), i.e. the contributions
    ! stemming from nonzero Neumann boundary conditions.
!</description>

!<input>
    ! discretisation structure
    type(t_spatialDiscretisation), intent(in) :: rdiscretisation

    ! linear form to be evaluated
    type(t_linearForm), intent(in) :: rform

    ! number of elements where the coefficients are to be computed
    integer, intent(in) :: nel

    ! number of points per element where the coefficients are to be computed
    integer, intent(in) :: nptsPerEl

    ! array of all points on all elements where coefficients are to be computed
    ! DIMENSION(spatial dimension, nptsPerEl, nel)
    real(DP), dimension(:,:,:), intent(in) :: Dpoints

    ! index of the boundary component that contains the points
    integer, intent(in) :: ibct

    ! parameter values (LENGTH PARAMETRISATION) of the points on the boundary component
    ! DIMENSION(nptsPerEl, nel)
    real(DP), dimension(:,:), intent(in) :: DpointPar

    ! array of test space DOF in all elements
    ! DIMENSION(#local DOF in test space, nel)
    integer, dimension(:,:), intent(in) :: IdofsTest

    ! structure providing more detailed information about the current element set
    type(t_domainIntSubset), intent(in) :: rdomainIntSubset
!</input>

!<inputoutput>
    ! optional collection structure for additional information provided by the user
    type(t_collection), intent(inout), optional :: rcollection
!</inputoutput>

!<output>
    ! array of coefficients of the terms in the linear form for all given points on
    ! all given elements, DIMENSION(itermCount, nptsPerEl, nel) with itermCount the
    ! number of terms in the linear form
    real(DP), dimension(:,:,:), intent(out) :: Dcoefficients
!</output>
!</subroutine>

    ! local variables
    real(DP) :: dminPar, dmaxPar, dt, dnx, dny
    integer :: iel, ipoint, iseg
    real(DP), dimension(:,:), pointer :: Dux, Duy

    ! get the minimum and maximum parameter value, corresponding to start and end point
    ! of the interval
    dminPar = DpointPar(1,1)
    dmaxPar = DpointPar(1,1)
    do iel = 1, nel
      do ipoint = 1, nptsPerEl
        dminPar = min(DpointPar(ipoint,iel), dminPar)
        dmaxPar = max(DpointPar(ipoint,iel), dmaxPar)
      enddo
    enddo

    if (rprob%csimulation .eq. SIMUL_ANALYTICAL) then
      ! in case of an analytical test function, the Neumann contributions are given by
      ! grad(u) * n, where n is the normal vector

      ! compute grad(u)
      allocate(Dux(nptsPerEl,nel), Duy(nptsPerEl,nel))
      ! Set rcollection%IquickAccess(4) = 0 in order to tell the routine elast_analFunc()
      ! that the displacement components are to be treated. (See comment in routine
      ! elast_calcErrors() in module elasticity_basic.)
      rcollection%IquickAccess(4) = 0
      call elast_analFunc(1, DER_DERIV_X, rdiscretisation, nel, nptsPerEl, Dpoints, &
                          rdomainIntSubset, Dux, rcollection)
      call elast_analFunc(1, DER_DERIV_Y, rdiscretisation, nel, nptsPerEl, Dpoints, &
                          rdomainIntSubset, Duy, rcollection)

      do iel = 1, nel
        do ipoint = 1, nptsPerEl
          ! get the parameter value corresponding to the current boundary point
          dt = DpointPar(ipoint,iel)
          ! Compute the normal vector in the current boundary point. At the endpoints of
          ! the interval the normal vector is based on the current edge.
          if (DpointPar(ipoint,iel) .eq. dminPar) then
            ! start point
            call boundary_getNormalVec2D(rdiscretisation%p_rboundary,&
               ibct, dt, dnx, dny, BDR_NORMAL_RIGHT, BDR_PAR_LENGTH)

          else if (DpointPar(ipoint,iel) .eq. dmaxPar) then
            ! end point
            call boundary_getNormalVec2D(rdiscretisation%p_rboundary,&
               ibct, dt, dnx, dny, BDR_NORMAL_LEFT, BDR_PAR_LENGTH)
          else
            ! inner point
            call boundary_getNormalVec2D(rdiscretisation%p_rboundary,&
               ibct, dt, dnx, dny, cparType=BDR_PAR_LENGTH)
          endif

          ! compute the normal value grad(u)*n
          Dcoefficients(1,ipoint,iel) = dnx * Dux(ipoint,iel) + dny * Duy(ipoint,iel)

        enddo
      enddo
      deallocate(Dux, Duy)
    else if (rprob%csimulation .eq. SIMUL_REAL) then
      ! in case of a real simulation, constant nonzero Neumann contributions are provided
      ! by the user for each boundary segment

      ! in rcollection%IquickAccess(2) the current segment number is stored
      iseg = rcollection%IquickAccess(2)
      Dcoefficients(1,ipoint,iel) = rprob%DbcValue(1, iseg, ibct)
    endif

  end subroutine elast_RHS_Poisson_2D_bound

! ****************************************************************************************

!<subroutine>
  subroutine elast_RHS_2D_vol(rdiscretisation, rform, nel, nptsPerEl, Dpoints, &
                              IdofsTest, rdomainIntSubset, Dcoefficients, rcollection)

    use fsystem
    use collection
    use scalarpde
    use domainintegration
    use derivatives
    use elasticity_basic

!<description>
    ! This subroutine is called during the vector assembly. It computes the function f
    ! in the linear form (usually L(v) = (f,v)_0 + (g,v)_N), i.e. the volumetric forces
    ! When rcollection%IquickAccess(1) .eq. 1, then forces in x-direction are computed,
    ! when rcollection%IquickAccess(1) .eq. 2, then forces in y-direction.
!</description>

!<input>
    ! discretisation structure
    type(t_spatialDiscretisation), intent(in) :: rdiscretisation

    ! linear form to be evaluated
    type(t_linearForm), intent(in) :: rform

    ! number of elements where the coefficients are to be computed
    integer, intent(in) :: nel

    ! number of points per element where the coefficients are to be computed
    integer, intent(in) :: nptsPerEl

    ! array of all points on all elements where coefficients are to be computed
    ! DIMENSION(spatial dimension, nptsPerEl, nel)
    real(DP), dimension(:,:,:), intent(in) :: Dpoints

    ! array of test space DOF in all elements
    ! DIMENSION(#local DOF in test space, nel)
    integer, dimension(:,:), intent(in) :: IdofsTest

    ! structure providing more detailed information about the current element set
    type(t_domainIntSubset), intent(in) :: rdomainIntSubset
!</input>

!<inputoutput>
    ! optional collection structure for additional information provided by the user
    type(t_collection), intent(inout), optional :: rcollection
!</inputoutput>

!<output>
    ! array of coefficients of the terms in the linear form for all given points on
    ! all given elements, DIMENSION(itermCount, nptsPerEl, nel) with itermCount the
    ! number of terms in the linear form
    real(DP), dimension(:,:,:), intent(out) :: Dcoefficients
!</output>
!</subroutine>

    real(DP), dimension(:,:), pointer :: Du1xx, Du1yx, Du1yy, Du2xx, Du2xy, Du2yy, &
                                         Dpx, Dpy

    if (rcollection%IquickAccess(1) .eq. 1) then
      ! forces in x-direction

      if (rprob%csimulation .eq. SIMUL_REAL) then
        Dcoefficients(1,:,:) = rprob%dforceVolumeX

      else ! rprob%csimulation .eq. SIMUL_ANALYTICAL
        allocate(Du1xx(nptsPerEl,nel), Du1yy(nptsPerEl,nel), Du2xy(nptsPerEl,nel))
        ! Set rcollection%IquickAccess(4) = 0 in order to tell the routine elast_analFunc()
        ! that the displacement components are to be treated. (See comment in routine
        ! elast_calcErrors() in module elasticity_basic.)
        rcollection%IquickAccess(4) = 0
        call elast_analFunc(1, DER_DERIV_XX, rdiscretisation, nel, nptsPerEl, Dpoints, &
                            rdomainIntSubset, Du1xx, rcollection)
        if (rprob%cformulation .ne. FORMULATION_STOKES) then
          ! for Stokes the mixed derivatives are not required
          call elast_analFunc(2, DER_DERIV_XY, rdiscretisation, nel, nptsPerEl, Dpoints, &
                              rdomainIntSubset, Du2xy, rcollection)
        endif
        call elast_analFunc(1, DER_DERIV_YY, rdiscretisation, nel, nptsPerEl, Dpoints, &
                            rdomainIntSubset, Du1yy, rcollection)
        if (rprob%cformulation .eq. FORMULATION_DISPL) then
          Dcoefficients(1,:,:) = - (2*rprob%dmu + rprob%dlambda) * Du1xx &
                                 -  rprob%dmu                    * Du1yy &
                                 - (rprob%dmu + rprob%dlambda)   * Du2xy
        else
          ! mixed formulation or Stokes
          allocate(Dpx(nptsPerEl,nel))
          ! Set rcollection%IquickAccess(4) = 1 in order to tell the routine
          ! elast_analFunc() that the pressure component is to be treated. (See comment in
          ! routine elast_calcErrors() in module elasticity_basic.)
          rcollection%IquickAccess(4) = 1
          call elast_analFunc(3, DER_DERIV_X, rdiscretisation, nel, nptsPerEl, Dpoints, &
                              rdomainIntSubset, Dpx, rcollection)
          if (rprob%cformulation .eq. FORMULATION_MIXED) then
            Dcoefficients(1,:,:) = - 2*rprob%dmu * Du1xx - rprob%dmu * (Du1yy + Du2xy) &
                                   + 2*rprob%dmu * Dpx
            ! Why the factor 2 * mu in front of Dpx?
            ! This code merely provides analytic solutions to test the application.
            ! Typically, mu has order O(1e+6) or even larger. Using a typical analytic
            ! solution provided by elast_danalyticFunction(...) (which is in the range
            ! of O(1)), e.g. some trigonomic function, this would result in a massive
            ! imbalance between u and p:
            !      -2mu * div(eps(u))  +  p    = f
            !          O(1e+6)         + O(1)  = O(1e+6)
            ! Such a problem is not well posed. Results for p would be way off for such
            ! imbalanced equation system. Remedy:
            ! * Use a set of analytic solutions for u and p with u O(1) and p O(mu), or
            ! * Scale the pressure solution here, not in elast_danalyticFunction(...).
          else ! FORMULATION_STOKES
            Dcoefficients(1,:,:) = - 2*rprob%dmu * (Du1xx + Du1yy) + Dpx
          endif
          deallocate(Dpx)
        endif
        deallocate(Du1xx, Du1yy, Du2xy)
      endif

    else if (rcollection%IquickAccess(1) .eq. 2) then
      ! forces in y-direction

      if (rprob%csimulation .eq. SIMUL_REAL) then
        Dcoefficients(1,:,:) = rprob%dforceVolumeY

      else if (rprob%csimulation .eq. SIMUL_ANALYTICAL) then
        allocate(Du2xx(nptsPerEl,nel), Du2yy(nptsPerEl,nel), Du1yx(nptsPerEl,nel))
        ! Set rcollection%IquickAccess(4) = 0 in order to tell the routine elast_analFunc()
        ! that the displacement components are to be treated. (See comment in routine
        ! elast_calcErrors() in module elasticity_basic.)
        rcollection%IquickAccess(4) = 0
        call elast_analFunc(2, DER_DERIV_XX, rdiscretisation, nel, nptsPerEl, Dpoints, &
                            rdomainIntSubset, Du2xx, rcollection)
        if (rprob%cformulation .ne. FORMULATION_STOKES) then
          ! for Stokes the mixed derivatives are not required
          call elast_analFunc(1, DER_DERIV_XY, rdiscretisation, nel, nptsPerEl, Dpoints, &
                              rdomainIntSubset, Du1yx, rcollection)
        endif
        call elast_analFunc(2, DER_DERIV_YY, rdiscretisation, nel, nptsPerEl, Dpoints, &
                            rdomainIntSubset, Du2yy, rcollection)
        if (rprob%cformulation .eq. FORMULATION_DISPL) then
          Dcoefficients(1,:,:) = - (rprob%dmu + rprob%dlambda)     * Du1yx &
                                 -  rprob%dmu                      * Du2xx &
                                 - (2 * rprob%dmu + rprob%dlambda) * Du2yy
        else
          ! mixed formulation or Stokes
          allocate(Dpy(nptsPerEl,nel))
          ! Set rcollection%IquickAccess(4) = 1 in order to tell the routine
          ! elast_analFunc() that the pressure component is to be treated. (See comment in
          ! routine elast_calcErrors() in module elasticity_basic.)
          rcollection%IquickAccess(4) = 1
          call elast_analFunc(3, DER_DERIV_Y, rdiscretisation, nel, nptsPerEl, Dpoints, &
                              rdomainIntSubset, Dpy, rcollection)
          if (rprob%cformulation .eq. FORMULATION_MIXED) then
            Dcoefficients(1,:,:) = - rprob%dmu * (Du2xx + Du1yx) - 2*rprob%dmu * Du2yy &
                                   + 2*rprob%dmu * Dpy
            ! Why the factor 2 * mu in front of Dpx? See comment above!

          else ! FORMULATION_STOKES
            Dcoefficients(1,:,:) = - 2*rprob%dmu * (Du2xx + Du2yy) + Dpy
          endif
          deallocate(Dpy)
        endif
        deallocate(Du2xx, Du2yy, Du1yx)
      endif
    endif

  end subroutine elast_RHS_2D_vol

! ****************************************************************************************

!<subroutine>
  subroutine elast_RHS_2D_surf(rdiscretisation, rform, nel, nptsPerEl, &
                               Dpoints, ibct, DpointPar, IdofsTest, rdomainIntSubset, &
                               Dcoefficients, rcollection)

    use fsystem
    use boundary
    use collection
    use scalarpde
    use domainintegration
    use spatialdiscretisation
    use elasticity_basic

!<description>
    ! This subroutine is called during the vector assembly. It computes the function g
    ! in the linear form (usually L(v) = (f,v)_0 + (g,v)_N), i.e. the
    ! surface forces in x- or y-direction (nonzero Neumann boundary conditions).
!</description>

!<input>
    ! discretisation structure
    type(t_spatialDiscretisation), intent(in) :: rdiscretisation

    ! linear form to be evaluated
    type(t_linearForm), intent(in) :: rform

    ! number of elements where the coefficients are to be computed
    integer, intent(in) :: nel

    ! number of points per element where the coefficients are to be computed
    integer, intent(in) :: nptsPerEl

    ! array of all points on all elements where coefficients are to be computed
    ! DIMENSION(spatial dimension, nptsPerEl, nel)
    real(DP), dimension(:,:,:), intent(in) :: Dpoints

    ! index of the boundary component that contains the points
    integer, intent(in) :: ibct

    ! parameter values (LENGTH PARAMETRISATION) of the points on the boundary component
    ! DIMENSION(nptsPerEl, nel)
    real(DP), dimension(:,:), intent(in) :: DpointPar

    ! array of test space DOF in all elements
    ! DIMENSION(#local DOF in test space, nel)
    integer, dimension(:,:), intent(in) :: IdofsTest

    ! structure providing more detailed information about the current element set
    type(t_domainIntSubset), intent(in) :: rdomainIntSubset
!</input>

!<inputoutput>
    ! optional collection structure for additional information provided by the user
    type(t_collection), intent(inout), optional :: rcollection
!</inputoutput>

!<output>
    ! array of coefficients of the terms in the linear form for all given points on
    ! all given elements, DIMENSION(itermCount, nptsPerEl, nel)
    ! with itermCount the number of terms in the linear form
    real(DP), dimension(:,:,:), intent(out) :: Dcoefficients
!</output>
!</subroutine>

    ! local variables
    real(DP), dimension(:,:,:), pointer :: DstressTensor
    real(DP) :: dminPar, dmaxPar, dt, dnx, dny
    integer :: iel, ipoint, icomp, iseg

    ! get the minimum and maximum parameter value, corresponding to start and end point
    ! of the interval
    dminPar = DpointPar(1,1)
    dmaxPar = DpointPar(1,1)
    do iel = 1, nel
      do ipoint = 1, nptsPerEl
        dminPar = min(DpointPar(ipoint,iel), dminPar)
        dmaxPar = max(DpointPar(ipoint,iel), dmaxPar)
      enddo
    enddo

    ! in rcollection%IquickAccess(1) the current component is stored
    icomp = rcollection%IquickAccess(1)

    if (rprob%csimulation .eq. SIMUL_ANALYTICAL .and. icomp .lt. 3) then
      ! In case of an analytical test function, the Neumann contributions are given by
      ! sigma * n, where sigma is the Cauchy stress tensor and n the normal vector.
      ! For the third component (pressure) in a mixed formulation or Stokes, there is
      ! no such contribution.

      allocate(DstressTensor(2,nptsPerEl,nel))
      ! compute the required row icomp of the Cauchy stress tensor
      call elast_stressTensor(icomp, rdiscretisation, nel, nptsPerEl, Dpoints, &
                              rdomainIntSubset, DstressTensor, rcollection)
      do iel = 1, nel
        do ipoint = 1, nptsPerEl
          ! get the parameter value corresponding to the current boundary point
          dt = DpointPar(ipoint,iel)
          ! Compute the normal vector in the current boundary point. At the endpoints of
          ! the interval the normal vector is based on the current edge.
          if (DpointPar(ipoint,iel) .eq. dminPar) then
            ! start point
            call boundary_getNormalVec2D(rdiscretisation%p_rboundary, ibct, &
                                         dt, dnx, dny, BDR_NORMAL_RIGHT, BDR_PAR_LENGTH)

          else if (DpointPar(ipoint,iel) .eq. dmaxPar) then
            ! end point
            call boundary_getNormalVec2D(rdiscretisation%p_rboundary, ibct, &
                                         dt, dnx, dny, BDR_NORMAL_LEFT, BDR_PAR_LENGTH)
          else
            ! inner point
            call boundary_getNormalVec2D(rdiscretisation%p_rboundary, ibct, &
                                         dt, dnx, dny, cparType=BDR_PAR_LENGTH)
          endif

          ! Now compute sigma * n
          Dcoefficients(1,ipoint,iel) = dnx * DstressTensor(1,ipoint,iel) &
                                      + dny * DstressTensor(2,ipoint,iel)
        enddo
      enddo
      deallocate(DstressTensor)

    else if (rprob%csimulation .eq. SIMUL_REAL) then
      ! in case of a real simulation, the Neumann contributions are given by constant
      ! surface forces on the current segment, which are provided by the user

      ! in rcollection%IquickAccess(2) the current segment number is stored
      iseg = rcollection%IquickAccess(2)
      Dcoefficients(1,:,:) = rprob%DbcValue(icomp, iseg, ibct)
    else
      ! in every other case set the values to zero
      Dcoefficients(1,:,:) = 0.0_DP
    endif

  end subroutine elast_RHS_2D_surf

! ****************************************************************************************

!<subroutine>
  subroutine elast_stressTensor(irow, rdiscretisation, nel, nptsPerEl, Dpoints, &
                                rdomainIntSubset, Dstresses, rcollection)

    use fsystem
    use genoutput
    use collection
    use scalarpde
    use domainintegration
    use derivatives
    use elasticity_basic

!<description>
    ! This routine computes one row of the 2x2 Cauchy stress tensor in a given set
    ! of points/elements. The stress tensor is given by:
    !
    ! pure displacement formulation:
    !   2*mu*eps + lambda*div(u)*I
    !      / 2*mu*eps11 + lambda*div(u)       2*mu*eps12      \
    !   =  |                                                  |
    !      \      2*mu*eps12       2*mu*eps22 + lambda*div(u) /
    !
    !     / (2*mu + lambda)*u1x + lambda*u2y      mu*(u1y + u2x)       \
    !   = |                                                            |
    !     \        mu*(u1y + u2x)     (2*mu + lambda)*u2y + lambda*u1x /
    !
    ! mixed formulation:
    !   2*mu*eps - p*I
    !     / 2*mu*eps11 - p     2*mu*eps12    \   / 2*mu*u1x - p    mu*(u1y + u2x) \
    !   = |                                  | = |                                |
    !     \   2*mu*eps12      2*mu*eps22 - p /   \ mu*(u1y + u2x)   2*mu*u2y - p  /
    !
    ! Stokes:
    !   2*mu*grad(u) - p*I
    !     /  2*mu*grad(u)11 - p     2*mu*grad(u)12      \   / 2*mu*u1x - p   2*mu*u1y   \
    !   = |                                             | = |                           |
    !     \  2*mu*grad(u)21         2*mu*grad(u)22 - p  /   \   2*mu*u2x   2*mu*u2y - p /
!</description>

!<input>
    ! specifies which row of the stress tensor is to be computed
    integer, intent(in) :: irow

    ! discretisation structure
    type(t_spatialDiscretisation), intent(in) :: rdiscretisation

    ! number of elements where the coefficients are to be computed
    integer, intent(in) :: nel

    ! number of points per element where the coefficients are to be computed
    integer, intent(in) :: nptsPerEl

    ! array of all points on all elements where coefficients are to be computed
    ! (usually coincides with rdomainSubset%p_DcubPtsReal)
    real(DP), dimension(:,:,:), intent(in) :: Dpoints

    ! structure providing more detailed information about the current element set
    type(t_domainIntSubset), intent(in) :: rdomainIntSubset

    ! optional collection structure for additional information provided by the user
    type(t_collection), intent(inout), optional :: rcollection
!</input>

!<output>
    ! array for storing the desired stresses in all points on all elements
    !   DIMENSION(2, snptsPerEl, nel)
    real(DP), dimension(:,:,:), intent(out) :: Dstresses
!</output>
!</subroutine>

    real(DP), dimension(:,:), pointer :: Du1x, Du2x, Du1y, Du2y, Dpfunc
    allocate(Du1x(nptsPerEl,nel), Du2x(nptsPerEl,nel), &
             Du1y(nptsPerEl,nel), Du2y(nptsPerEl,nel))
    if (rprob%cformulation .eq. FORMULATION_MIXED .or. &
        rprob%cformulation .eq. FORMULATION_STOKES) then
      allocate(Dpfunc(nptsPerEl,nel))
    endif
    ! Set rcollection%IquickAccess(4) = 0 in order to tell the routine elast_analFunc()
    ! that the displacement components are to be treated. (See comment in routine
    ! elast_calcErrors() in module elasticity_basic.)
    rcollection%IquickAccess(4) = 0
    call elast_analFunc(1, DER_DERIV_X,rdiscretisation, nel, nptsPerEl, Dpoints, &
                        rdomainIntSubset, Du1x, rcollection)
    call elast_analFunc(1, DER_DERIV_Y,rdiscretisation, nel, nptsPerEl, Dpoints, &
                        rdomainIntSubset, Du1y, rcollection)
    call elast_analFunc(2, DER_DERIV_X, rdiscretisation, nel, nptsPerEl, Dpoints, &
                        rdomainIntSubset, Du2x, rcollection)
    call elast_analFunc(2, DER_DERIV_Y, rdiscretisation, nel, nptsPerEl, Dpoints, &
                        rdomainIntSubset, Du2y, rcollection)
    if (rprob%cformulation .eq. FORMULATION_MIXED .or. &
        rprob%cformulation .eq. FORMULATION_STOKES) then
      ! Set rcollection%IquickAccess(4) = 1 in order to tell the routine
      ! elast_analFunc() that the pressure component is to be treated. (See comment in
      ! routine elast_calcErrors() in module elasticity_basic.)
      rcollection%IquickAccess(4) = 1
      call elast_analFunc(3, DER_FUNC, rdiscretisation, nel, nptsPerEl, Dpoints, &
                          rdomainIntSubset, Dpfunc, rcollection)
    endif

    if (irow .eq. 1) then
      if (rprob%cformulation .eq. FORMULATION_DISPL) then
        Dstresses(1,:,:) = (2*rprob%dmu + rprob%dlambda) * Du1x +  rprob%dlambda * Du2y
        Dstresses(2,:,:) = rprob%dmu * (Du1y + Du2x)
      else if (rprob%cformulation .eq. FORMULATION_MIXED) then
        Dstresses(1,:,:) = 2*rprob%dmu * Du1x - Dpfunc
        Dstresses(2,:,:) = rprob%dmu * (Du1y + Du2x)
      else
        Dstresses(1,:,:) = 2*rprob%dmu * Du1x - Dpfunc
        Dstresses(2,:,:) = 2*rprob%dmu * Du1y
      endif
    else if (irow .eq. 2) then
      if (rprob%cformulation .eq. FORMULATION_DISPL) then
        Dstresses(1,:,:) = rprob%dmu * (Du2x + Du1y)
        Dstresses(2,:,:) = (2*rprob%dmu + rprob%dlambda) * Du2y +  rprob%dlambda * Du1x
      else if (rprob%cformulation .eq. FORMULATION_MIXED) then
        Dstresses(1,:,:) = rprob%dmu * (Du2x + Du1y)
        Dstresses(2,:,:) = 2*rprob%dmu * Du2y - Dpfunc
      else
        Dstresses(1,:,:) = 2*rprob%dmu * Du2x
        Dstresses(2,:,:) = 2*rprob%dmu * Du2y - Dpfunc
      endif
    else
      call output_line('Invalid row number ' // trim(sys_siL(irow,4)) // '!', &
                       OU_CLASS_ERROR, OU_MODE_STD, 'elast_stressTensor')
      call sys_halt()
    endif

    deallocate(Du1x, Du2x, Du1y, Du2y)
    if (rprob%cformulation .eq. FORMULATION_MIXED .or. &
        rprob%cformulation .eq. FORMULATION_STOKES) then
      deallocate(Dpfunc)
    endif

  end subroutine elast_stressTensor

! ****************************************************************************************

!<subroutine>
  subroutine elast_boundValue_2D(Icomponents, rdiscretisation, rboundaryRegion, &
                                 ielement, cinfoNeeded, iwhere, dwhere, Dvalues, &
                                 rcollection)

    use fsystem
    use collection
    use spatialdiscretisation
    use boundary
    use derivatives
    use elasticity_basic

!<description>
    ! This subroutine is called during the discretisation of boundary conditions. It
    ! calculates a special quantity on the boundary, which is then used by the
    ! discretisation routines to generate a discrete 'snapshot' of the (actually analytic)
    ! boundary conditions.
!</description>

!<input>
    ! specifier for Dirichlet boundary
    ! Icomponents(1) defines the index of the boundary component the value is to be
    ! calculated for (e.g., 1 = 1st solution component = x-displacement, 2 = 2nd solution
    ! component = y-displacement)
    integer, dimension(:), intent(in) :: Icomponents

    ! discretisation structure
    type(t_spatialDiscretisation), intent(in) :: rdiscretisation

    ! current boundary region
    type(t_boundaryRegion), intent(in) :: rboundaryRegion

    ! number of the current element on the boundary
    integer, intent(in) :: ielement

    ! The type of information, the routine should calculate. One of the DISCBC_NEEDxxxx
    ! constants. Depending on the constant, the routine has to return one or more
    ! information values in the result array.
    integer, intent(in) :: cinfoNeeded

    ! Reference to a geometric object where information is to be computed.
    ! cinfoNeeded = DISCBC_NEEDFUNC / DISCBC_NEEDDERIV:
    !   iwhere = number of the point in the triangulation or
    !          = 0, if only the parameter value of the point is known; this
    !               can be found in dwhere,
    ! cinfoNeeded = DISCBC_NEEDINTMEAN:
    !   iwhere = number of the edge where the integral mean value is to be computed
    integer, intent(in) :: iwhere

    ! Reference to a geometric object where information is to be computed.
    ! cinfoNeeded = DISCBC_NEEDFUNC / DISCBC_NEEDDERIV:
    !   dwhere = parameter value of the point where the value should be computed,
    ! cinfoNeeded = DISCBC_NEEDINTMEAN:
    !   dwhere = 0 (not used)
    real(DP), intent(in) :: dwhere
!</input>

!<inputoutput>
    ! optional collection structure for additional information provided by the user
    type(t_collection), intent(inout), optional :: rcollection
!</inputoutput>

!<output>
    ! Array for storing the calculated values. If only one value is needed it is stored
    ! in Dvalues(1). If multiple values are needed, then, e.g., for DISCBC_NEEDDERIV:
    ! Dvalues(1) = x-derivative, Dvalues(2) = y-derivative, ...
    real(DP), dimension(:), intent(out) :: Dvalues
!</output>
!</subroutine>

    real(DP), dimension(2,1,1) :: Dpoints
    real(DP), dimension(1,1) :: Daux

    ! coordinates of the boundary point
    real(DP) :: dx, dy

    ! calculate value
    if (rprob%csimulation .eq. SIMUL_REAL) then
      ! currently, only zero Dirichlet boundary values are supported for real simulations
! BRAL: das muss verbessert werden!
      Dvalues(1) = 0.0_DP
    else if (rprob%csimulation .eq. SIMUL_ANALYTICAL) then
      ! set coordinates of the current point
      call boundary_getCoords(rdiscretisation%p_rboundary, &
                              rboundaryRegion%iboundCompIdx, dwhere, dx, dy)
      Dpoints(1,1,1) = dx
      Dpoints(2,1,1) = dy
      if (Icomponents(1) .lt. 3) then
        ! boundary values for x- or y-displacements
        Daux = elast_danalyticFunction(Dpoints, 1, 1, DER_FUNC, &
                                       rprob%CfuncID(Icomponents(1)))
      else
        ! boundary values for pressure solution

        if (rprob%dnu .eq. 0.5) then
          ! incompressible case
          Daux = elast_danalyticFunction(Dpoints, 1, 1, DER_FUNC, rprob%CfuncID(3))
        else
          ! in the compressible case the pressure is related to the displacements
          ! via p = - dlambda * div(u) = -dlambda * (u1_x + u2_y)
          Daux = -rprob%dlambda * &
            (elast_danalyticFunction(Dpoints, 1, 1, DER_DERIV_X, rprob%CfuncID(1)) &
           + elast_danalyticFunction(Dpoints, 1, 1, DER_DERIV_Y, rprob%CfuncID(2)))
        endif
      endif
      Dvalues(1) = Daux(1,1)
    endif
  end subroutine elast_boundValue_2D

end module elasticity_callback


module maths_mod
use vars_mod, only : PHC, PK, PHK, PI, PME, PH, PC, PH2C3, PHC2, identity, fact
implicit none

contains

! ---------------------------------------------------------
! Given x(:) and y(:) which tabulate a function and the derivative at the boundary points
! this function returns the second derivative of the spline at each point
! ---------------------------------------------------------
		subroutine splin1(x,y,yp1,ypn,y2)
		real*8, INTENT(IN) :: x(:), y(:), yp1, ypn
		real*8, INTENT(INOUT) :: y2(size(x))
		integer :: n, i, k
		real*8 :: p, qn, sig, un, u(size(x))

			n = size(x)

			if (yp1 > .99d30) then
				y2(1) = 0.d0
				u(1) = 0.d0
			else
				y2(1) = -0.5d0
				u(1) = (3.d0/(x(2)-x(1)))*((y(2)-y(1))/(x(2)-x(1))-yp1)
			endif

			do i = 2, n-1
				sig = (x(i)-x(i-1))/(x(i+1)-x(i-1))
				p = sig * y2(i-1)+2.d0
				y2(i) = (sig-1.d0)/p
				u(i) = (6.d0*((y(i+1)-y(i))/(x(i+1)-x(i))-(y(i)-y(i-1))/(x(i)-x(i-1)))/&
					(x(i+1)-x(i-1))-sig*u(i-1))/p
			enddo
			if (ypn > .99d30) then
				qn = 0.d0
				un = 0.d0
			else
				qn = 0.5d0
				un = (3.d0/(x(n)-x(n-1)))*(ypn-(y(n)-y(n-1))/(x(n)-x(n-1)))
			endif

			y2(n) = (un-qn*u(n-1))/(qn*y2(n-1)+1.d0)

			do k = n-1, 1, -1
				y2(k) = y2(k)*y2(k+1)+u(k)
			enddo

		end subroutine splin1

! ---------------------------------------------------------
! Given xa(:) and ya(:) which tabulate a function, returns the interpolation using
! splines of vector x(:) in y(:)
! ---------------------------------------------------------
		subroutine spline(xa,ya,x,y)
		real*8, INTENT(INOUT) :: y(:)
		real*8, INTENT(IN) :: xa(:), ya(:), x(:)
		real*8 :: y2a(size(xa))
		integer :: n_x, n, i, k, khi, klo
		real*8 :: a, b, h, extrap

			n = size(xa)
			n_x = size(x)
			call splin1(xa,ya,1.d30,1.d30,y2a)

			do i = 1, n_x

! Downward extrapolation
				if (x(i) < xa(1)) then
!					y(i) = ya(1)
					y(i) = ya(1) + (ya(1)-ya(2))/(xa(1)-xa(2)) * (xa(1) - x(i))
				else

! Upward extrapolation
				if (x(i) > xa(n)) then
!					y(i) = ya(n)
					y(i) = ya(n) + (ya(n)-ya(n-1)) / (xa(n)-xa(n-1)) * (x(i) - xa(n))
				else
! In range
						klo = 1
						khi = n
1						if(khi-klo > 1) then
							k = (khi+klo)/2
							if (xa(k) > x(i)) then
								khi = k
							else
								klo = k
							endif
							go to 1
						endif

						h = xa(khi)-xa(klo)

						if (h == 0.d0) then
							print *, 'bad xa input in spline'
							pause
						endif
						a = (xa(khi)-x(i))/h
						b = (x(i)-xa(klo))/h

						y(i) = a*ya(klo)+b*ya(khi)+((a**3.d0-a)*y2a(klo)+(b**3.d0-b)*y2a(khi))*(h**2.d0)/6.d0
					endif
				endif
			enddo

		end subroutine spline

!----------------------------------------------------------------
! This function returns the strength of the Zeeman components
! Do it using table 3.1 of Landi degl'Innocenti & Landolfi (2004)
! to avoid overflow with the factorials if using the 3-j symbol
!----------------------------------------------------------------
	function strength_zeeman(J_up,J_low,M_up,M_low)
	real(kind=8) :: J_up, J_low, M_up, M_low, strength_zeeman, strength_zeeman2

! 		strength_zeeman2 = 3.d0 * w3js(int(2.d0*J_up),int(2.d0*J_low),2,int(2.d0*M_up),&
! 					-int(2.d0*M_low),int(2.d0*(M_low-M_up)))**2

		if (J_up == J_low+1) then
			if (M_up == M_low+1) then
				strength_zeeman = (3.d0*(J_low+M_low+1)*(J_low+M_low+2)) / (2.d0*(J_low+1.d0)*(2.d0*J_low+1.d0)*(2.d0*J_low+3.d0))
 				return
			endif
			if (M_up == M_low) then
				strength_zeeman = (3.d0*(J_low-M_low+1)*(J_low+M_low+1)) / ((J_low+1.d0)*(2.d0*J_low+1.d0)*(2.d0*J_low+3.d0))
 				return
			endif
			if (M_up == M_low-1) then
				strength_zeeman = (3.d0*(J_low-M_low+1)*(J_low-M_low+2)) / (2.d0*(J_low+1.d0)*(2.d0*J_low+1.d0)*(2.d0*J_low+3.d0))
 				return
			endif
		endif

		if (J_up == J_low) then
			if (M_up == M_low+1) then
				strength_zeeman = (3.d0*(J_low-M_low)*(J_low+M_low+1)) / (2.d0*J_low*(J_low+1.d0)*(2.d0*J_low+1.d0))
 				return
			endif
			if (M_up == M_low) then
				strength_zeeman = (3.d0*M_low**2) / (J_low*(J_low+1.d0)*(2.d0*J_low+1.d0))
 				return
			endif
			if (M_up == M_low-1) then
				strength_zeeman = (3.d0*(J_low+M_low)*(J_low-M_low+1)) / (2.d0*J_low*(J_low+1.d0)*(2.d0*J_low+1.d0))
 				return
			endif
		endif

		if (J_up == J_low-1) then
			if (M_up == M_low+1) then
				strength_zeeman = (3.d0*(J_low-M_low)*(J_low-M_low-2)) / (2.d0*J_low*(2.d0*J_low-1.d0)*(2.d0*J_low+1.d0))
 				return
			endif
			if (M_up == M_low) then
				strength_zeeman = (3.d0*(J_low-M_low)*(J_low+M_low)) / (J_low*(2.d0*J_low-1.d0)*(2.d0*J_low+1.d0))
 				return
			endif
			if (M_up == M_low-1) then
				strength_zeeman = (3.d0*(J_low+M_low)*(J_low+M_low-1)) / (2.d0*J_low*(2.d0*J_low-1.d0)*(2.d0*J_low+1.d0))
 				return
			endif
		endif

	end function strength_zeeman

!----------------------------------------------------------------
! This function calculates the 3-j symbol
! J_i and M_i have to be twice the actual value of J and M
!----------------------------------------------------------------
   function w3js(j1,j2,j3,m1,m2,m3)
   integer :: m1, m2, m3, j1, j2, j3
	integer :: ia, ib, ic, id, ie, im, ig, ih, z, zmin, zmax, jsum
	real(kind=8) :: w3js, cc, denom, cc1, cc2

      w3js = 0.d0
      if (m1+m2+m3 /= 0) return
      ia = j1 + j2
      if (j3 > ia) return
      ib = j1 - j2
      if (j3 < abs(ib)) return
		if (abs(m1) > j1) return
		if (abs(m2) > j2) return
		if (abs(m3) > j3) return

      jsum = j3 + ia
      ic = j1 - m1
      id = j2 - m2

		ie = j3 - j2 + m1
		im = j3 - j1 - m2
		zmin = max0(0,-ie,-im)
		ig = ia - j3
		ih = j2 + m2
		zmax = min0(ig,ih,ic)
		cc = 0.d0

		do z = zmin, zmax, 2
			denom = fact(z/2)*fact((ig-z)/2)*fact((ic-z)/2)*fact((ih-z)/2)*&
				fact((ie+z)/2)*fact((im+z)/2)
      	if (mod(z,4) /= 0) denom = -denom
			cc = cc + 1.d0/denom
		enddo

		cc1 = fact(ig/2)*fact((j3+ib)/2)*fact((j3-ib)/2)/fact((jsum+2)/2)
      cc2 = fact((j1+m1)/2)*fact(ic/2)*fact(ih/2)*fact(id/2)*fact((j3-m3)/2)*fact((j3+m3)/2)
      cc = cc * sqrt(1.d0*cc1*cc2)
		if (mod(ib-m3,4) /= 0) cc = -cc
		w3js = cc
		if (abs(w3js) < 1.d-8) w3js = 0.d0
1000 		return
   end function w3js

!----------------------------------------------------------------
! Calculates the factorial of the integers up to 301 and save it into the fact array
!----------------------------------------------------------------
   subroutine factrl
	integer :: i
      fact(0) = 1.d0
      do i = 1,101
			fact(i) = fact(i-1) * dble(i)
		enddo
   end subroutine factrl

! ---------------------------------------------------------
! Returns Planck's function for a frequency and a set of temperatures in cgs
! INPUT:
!     - nu : frequency
!     - T : temperature
! ---------------------------------------------------------
   function planck_vect(nu,T)
	real*8, INTENT(IN) :: nu, T(:)
   real*8 :: planck_vect(size(T))
   real*8 :: temporal(size(T))

		where (T /= 0.d0)
			planck_vect = PHC * nu**3.d0 / (dexp(PHK * nu / T) - 1.d0)
		elsewhere
			planck_vect = 0.d0
		endwhere

   end function planck_vect

! ---------------------------------------------------------
! Returns Planck's function for a frequency and temperature in cgs
! INPUT:
!     - nu : frequency (Hz)
!     - T : temperature (K)
! ---------------------------------------------------------
   function planck(nu,T)
   real*8 :: planck
   real*8, INTENT(IN) :: nu, T
   real*8 :: temporal
      if (T /= 0.d0) then
         planck = PHC * nu**3.d0 / (dexp(PHK * nu / T) - 1.d0)
      else
			planck = 0.d0
      endif

   end function planck

! ---------------------------------------------------------
! Returns Planck's function for a wavelength and a set of temperatures in cgs
! INPUT:
!     - lambda : wavelength
!     - T : temperature
! ---------------------------------------------------------
   function planck_vect_lambda(lambda,T)
	real*8, INTENT(IN) :: lambda, T(:)
   real*8 :: planck_vect_lambda(size(T))
   real*8 :: temporal(size(T))

		where (T /= 0.d0)
			planck_vect_lambda = PHC2 / lambda**5.d0 / (dexp(PHK * PC / (lambda * T)) - 1.d0)
		elsewhere
			planck_vect_lambda = 0.d0
		endwhere

   end function planck_vect_lambda

! ---------------------------------------------------------
! Returns Planck's function for a wavelength and temperature in cgs
! INPUT:
!     - lambda : wavelength (cm)
!     - T : temperature (K)
! ---------------------------------------------------------
   function planck_lambda(lambda,T)
   real*8 :: planck_lambda
   real*8, INTENT(IN) :: lambda, T
   real*8 :: temporal

      if (T /= 0.d0) then
         temporal = dexp(PHK * PC / (lambda * T) - 1.d0)
         planck_lambda = PHC2 / lambda**5.d0 / temporal
      else
			planck_lambda = 0.d0
      endif

   end function planck_lambda

! ---------------------------------------------------------
! Returns the derivative of the Planck's function for a wavelength and a set of temperatures in cgs
! INPUT:
!     - lambda : wavelength
!     - T : temperature
! ---------------------------------------------------------
   function planck_vect_lambda_deriv(lambda,T)
	real*8, INTENT(IN) :: lambda, T(:)
   real*8 :: planck_vect_lambda_deriv(size(T))
   real*8 :: temporal(size(T))

		where (T /= 0.d0)
			temporal = dexp(PHK * PC / (lambda * T))
			planck_vect_lambda_deriv = PH2C3 / (PK*T**2*lambda**6.d0) / (temporal-1.d0) * temporal
		elsewhere
			planck_vect_lambda_deriv = 0.d0
		endwhere

   end function planck_vect_lambda_deriv

! ---------------------------------------------------------
! Returns the derivative of the Planck's function for a wavelength and temperature in cgs
! INPUT:
!     - lambda : wavelength (cm)
!     - T : temperature (K)
! ---------------------------------------------------------
   function planck_lambda_deriv(lambda,T)
   real*8 :: planck_lambda_deriv
   real*8, INTENT(IN) :: lambda, T
   real*8 :: temporal

      if (T /= 0.d0) then
         temporal = dexp(PHK * PC / (lambda * T))
         planck_lambda_deriv = PH2C3 / (PK*T**2*lambda**6.d0) / (temporal-1.d0) * temporal
      else
			planck_lambda_deriv = 0.d0
      endif

   end function planck_lambda_deriv

!-------------------------------------------------------------------
! Returns which is the index of the array wave_total closer to wave
!-------------------------------------------------------------------
	function close_to(wave_total, wave)
	real(kind=8) :: wave_total(:), wave
	real(kind=8), allocatable :: diff(:)
	integer :: n, i, location(1)
	integer :: close_to

		n = size(wave_total)
		allocate(diff(n))
		diff = dabs(wave_total-wave)
		location = minloc(diff)
		deallocate(diff)
		close_to = location(1)

	end function close_to

!-------------------------------------------------------------------
! Returns which is the index of the array wave_total closer to wave
!-------------------------------------------------------------------
	function extremes(freq, line_frequency, delta)
	real(kind=8) :: freq(:), line_frequency, delta
	integer :: left, right, extremes(2)

		left = close_to(freq, line_frequency-delta)
		right = close_to(freq, line_frequency+delta)

		extremes(1) = min(left,right)
		extremes(2) = max(left,right)

	end function extremes

!-----------------------------------------------------------------
! Calculates the ratio between two ionization stages using the Saha equation
!-----------------------------------------------------------------
	function saha(t, pe, ul ,uu, chi)
	real(kind=8) :: t(:), pe(:), ul(:), uu(:), chi, saha(size(t))
	real(kind=8) :: constant

		constant = 2.d0 * (2.d0*PI*PME)**(1.5d0) / PH**3 * PK**(2.5d0)
		saha = constant * uu / ul * t**2.5d0 * 10.d0**(-5040.d0*chi/t) / pe

	end function saha

!--------------------------------------------------------------
! Generates Voigt and anomalous dispersion profiles using a different damping for each frequency
! point
! See Humlicek (1982) JQSRT 27, 437
!--------------------------------------------------------------
	function fvoigt(da, dv)
	real*8 :: da
	real*8 :: dv, fvoigt
	complex :: w4, z, t, u, v4
	real*8 :: s
	integer :: i, n

! There is a problem for very small dampings. We force it to be 0 using a threshold
		if (da < 1.d-3) da = 0.d0
		z = cmplx(dv, da)
		t = cmplx(da, -dv)
		s = dabs(dv) + da
		u = t*t

		if (s >= 15.d0) then
			w4 = t * 0.5641896d0 / (0.5d0+u)
		elseif (s >= 5.5) then
			w4 = t*(1.410474d0+u*0.5641896d0)/(0.75d0+u*(3.d0+u))
		elseif (da >= 0.195d0*dabs(dv)-0.176d0) then
			w4 = (16.4955d0+t*(20.20933d0+t*(11.96482d0+t*(3.778987d0+t*0.5642236d0)))) / &
				(16.4955d0+t*(38.82363d0+t*(39.27121d0+t*(21.69274d0+t*(6.699398d0+t)))))
		else
			w4 = t*(36183.31d0-u*(3321.9905d0-u*(1540.787d0-u*(219.0313d0-u*(35.76683d0-&
				u*(1.320522d0-u*0.56419d0))))))
			v4 = (32066.6d0-u*(24322.84d0-u*(9022.228d0-u*(2186.181d0-u*(364.2191d0-&
				u*(61.57037d0-u*(1.841439d0-u)))))))
			w4 = cexp(u) - w4/v4
		endif

		fvoigt = dble(w4)

	end function fvoigt

!--------------------------------------------------------------
! Generates Voigt and anomalous dispersion profiles using a different damping for each frequency
! point
! See Humlicek (1982) JQSRT 27, 437
!--------------------------------------------------------------
	function vecfvoigt(da, dv)
	real*8 :: da(:)
	real*8 :: dv(:), vecfvoigt(size(dv))
	complex :: w4, z, t, u, v4
	real*8 :: s
	integer :: i, n

		n = size(dv)
		do i = 1, n
! There is a problem for very small dampings. We force it to be 0 using a threshold
			if (da(i) < 1.d-3) da(i) = 0.d0
			z = cmplx(dv(i), da(i))
			t = cmplx(da(i), -dv(i))
			s = dabs(dv(i)) + da(i)
			u = t*t

			if (s >= 15.d0) then
				w4 = t * 0.5641896d0 / (0.5d0+u)
			elseif (s >= 5.5) then
				w4 = t*(1.410474d0+u*0.5641896d0)/(0.75d0+u*(3.d0+u))
			elseif (da(i) >= 0.195d0*dabs(dv(i))-0.176d0) then
				w4 = (16.4955d0+t*(20.20933d0+t*(11.96482d0+t*(3.778987d0+t*0.5642236d0)))) / &
					(16.4955d0+t*(38.82363d0+t*(39.27121d0+t*(21.69274d0+t*(6.699398d0+t)))))
			else
				w4 = t*(36183.31d0-u*(3321.9905d0-u*(1540.787d0-u*(219.0313d0-u*(35.76683d0-&
					u*(1.320522d0-u*0.56419d0))))))
				v4 = (32066.6d0-u*(24322.84d0-u*(9022.228d0-u*(2186.181d0-u*(364.2191d0-&
					u*(61.57037d0-u*(1.841439d0-u)))))))
				w4 = cexp(u) - w4/v4
			endif
			vecfvoigt(i) = dble(w4)
		enddo

	end function vecfvoigt

!--------------------------------------------------------------
! Generates Voigt and anomalous dispersion profiles using a different damping for each frequency
! point
! See Humlicek (1982) JQSRT 27, 437
!--------------------------------------------------------------
	function vecfvoigt_zeeman(da, dv)
	real*8 :: da(:)
	real*8 :: dv(:), vecfvoigt_zeeman(2,size(dv))
	complex :: w4, z, t, u, v4
	real*8 :: s
	integer :: i, n

		n = size(dv)
		do i = 1, n
! There is a problem for very small dampings. We force it to be 0 using a threshold
			if (da(i) < 1.d-3) da(i) = 0.d0
			z = cmplx(dv(i), da(i))
			t = cmplx(da(i), -dv(i))
			s = dabs(dv(i)) + da(i)
			u = t*t

			if (s >= 15.d0) then
				w4 = t * 0.5641896d0 / (0.5d0+u)
			elseif (s >= 5.5) then
				w4 = t*(1.410474d0+u*0.5641896d0)/(0.75d0+u*(3.d0+u))
			elseif (da(i) >= 0.195d0*dabs(dv(i))-0.176d0) then
				w4 = (16.4955d0+t*(20.20933d0+t*(11.96482d0+t*(3.778987d0+t*0.5642236d0)))) / &
					(16.4955d0+t*(38.82363d0+t*(39.27121d0+t*(21.69274d0+t*(6.699398d0+t)))))
			else
				w4 = t*(36183.31d0-u*(3321.9905d0-u*(1540.787d0-u*(219.0313d0-u*(35.76683d0-&
					u*(1.320522d0-u*0.56419d0))))))
				v4 = (32066.6d0-u*(24322.84d0-u*(9022.228d0-u*(2186.181d0-u*(364.2191d0-&
					u*(61.57037d0-u*(1.841439d0-u)))))))
				w4 = cexp(u) - w4/v4
			endif
			vecfvoigt_zeeman(1,i) = dble(w4)
			vecfvoigt_zeeman(2,i) = aimag(w4)
		enddo

	end function vecfvoigt_zeeman

!--------------------------------------------------------------
! Inversion of a 4x4 matrix
!--------------------------------------------------------------
	subroutine invert(a)
	real(kind=8) :: a(4,4)
	real(kind=8) :: b(4,4), det, maxim, fabsmax
	! First some tests of singularity
		b = dabs(a)
		maxim = maxval(b)
		fabsmax = 1.d0 / maxim
		if (maxim == 0.d0) then
			print *, 'Singularity in the inversion'
!			stop
		endif

		a = a * fabsmax

   	b(1,1) = a(2,2) * a(3,3) * a(4,4) + a(2,3) * a(3,4) * a(4,2)&
      	+ a(2,4) * a(3,2) * a(4,3) - a(2,2) * a(3,4) * a(4,3)&
	   	- a(2,3) * a(3,2) * a(4,4) - a(2,4) * a(3,3) * a(4,2)
   	b(2,1) = a(2,3) * a(3,1) * a(4,4) + a(2,4) * a(3,3) * a(4,1)&
	   	+ a(2,1) * a(3,4) * a(4,3) - a(2,3) * a(3,4) * a(4,1)&
   		- a(2,4) * a(3,1) * a(4,3) - a(2,1) * a(3,3) * a(4,4)
   	b(3,1) = a(2,4) * a(3,1) * a(4,2) + a(2,1) * a(3,2) * a(4,4)&
	   	+ a(2,2) * a(3,4) * a(4,1) - a(2,4) * a(3,2) * a(4,1)&
   		- a(2,1) * a(3,4) * a(4,2) - a(2,2) * a(3,1) * a(4,4)
   	b(4,1) = a(2,1) * a(3,3) * a(4,2) + a(2,2) * a(3,1) * a(4,3)&
	   	+ a(2,3) * a(3,2) * a(4,1) - a(2,1) * a(3,2) * a(4,3)&
   		- a(2,2) * a(3,3) * a(4,1) - a(2,3) * a(3,1) * a(4,2)
   	b(1,2) = a(3,2) * a(4,4) * a(1,3) + a(3,3) * a(4,2) * a(1,4)&
	   	+ a(3,4) * a(4,3) * a(1,2) - a(3,2) * a(4,3) * a(1,4)&
   		- a(3,3) * a(4,4) * a(1,2) - a(3,4) * a(4,2) * a(1,3)
   	b(2,2) = a(3,3) * a(4,4) * a(1,1) + a(3,4) * a(4,1) * a(1,3)&
	   	+ a(3,1) * a(4,3) * a(1,4) - a(3,3) * a(4,1) * a(1,4)&
   		- a(3,4) * a(4,3) * a(1,1) - a(3,1) * a(4,4) * a(1,3)
   	b(3,2) = a(3,4) * a(4,2) * a(1,1) + a(3,1) * a(4,4) * a(1,2)&
	   	+ a(3,2) * a(4,1) * a(1,4) - a(3,4) * a(4,1) * a(1,2)&
   		- a(3,1) * a(4,2) * a(1,4) - a(3,2) * a(4,4) * a(1,1)
   	b(4,2) = a(3,1) * a(4,2) * a(1,3) + a(3,2) * a(4,3) * a(1,1)&
	   	+ a(3,3) * a(4,1) * a(1,2) - a(3,1) * a(4,3) * a(1,2)&
   		- a(3,2) * a(4,1) * a(1,3) - a(3,3) * a(4,2) * a(1,1)
   	b(1,3) = a(4,2) * a(1,3) * a(2,4) + a(4,3) * a(1,4) * a(2,2)&
   		+ a(4,4) * a(1,2) * a(2,3) - a(4,2) * a(1,4) * a(2,3)&
	   	- a(4,3) * a(1,2) * a(2,4) - a(4,4) * a(1,3) * a(2,2)
   	b(2,3) = a(4,3) * a(1,1) * a(2,4) + a(4,4) * a(1,3) * a(2,1)&
	   	+ a(4,1) * a(1,4) * a(2,3) - a(4,3) * a(1,4) * a(2,1)&
   		- a(4,4) * a(1,1) * a(2,3) - a(4,1) * a(1,3) * a(2,4)
   	b(3,3) = a(4,4) * a(1,1) * a(2,2) + a(4,1) * a(1,2) * a(2,4)&
	   	+ a(4,2) * a(1,4) * a(2,1) - a(4,4) * a(1,2) * a(2,1)&
   		- a(4,1) * a(1,4) * a(2,2) - a(4,2) * a(1,1) * a(2,4)
   	b(4,3) = a(4,1) * a(1,3) * a(2,2) + a(4,2) * a(1,1) * a(2,3)&
	   	+ a(4,3) * a(1,2) * a(2,1) - a(4,1) * a(1,2) * a(2,3)&
   		- a(4,2) * a(1,3) * a(2,1) - a(4,3) * a(1,1) * a(2,2)
   	b(1,4) = a(1,2) * a(2,4) * a(3,3) + a(1,3) * a(2,2) * a(3,4)&
	   	+ a(1,4) * a(2,3) * a(3,2) - a(1,2) * a(2,3) * a(3,4)&
   		- a(1,3) * a(2,4) * a(3,2) - a(1,4) * a(2,2) * a(3,3)
   	b(2,4) = a(1,3) * a(2,4) * a(3,1) + a(1,4) * a(2,1) * a(3,3)&
	   	+ a(1,1) * a(2,3) * a(3,4) - a(1,3) * a(2,1) * a(3,4)&
   		- a(1,4) * a(2,3) * a(3,1) - a(1,1) * a(2,4) * a(3,3)
   	b(3,4) = a(1,4) * a(2,2) * a(3,1) + a(1,1) * a(2,4) * a(3,2)&
	   	+ a(1,2) * a(2,1) * a(3,4) - a(1,4) * a(2,1) * a(3,2)&
   		- a(1,1) * a(2,2) * a(3,4) - a(1,2) * a(2,4) * a(3,1)
   	b(4,4) = a(1,1) * a(2,2) * a(3,3) + a(1,2) * a(2,3) * a(3,1)&
	   	+ a(1,3) * a(2,1) * a(3,2) - a(1,1) * a(2,3) * a(3,2)&
   		- a(1,2) * a(2,1) * a(3,3) - a(1,3) * a(2,2) * a(3,1)

		det = a(1,1) * b(1,1) + a(1,2) * b(2,1) + a(1,3) * b(3,1) + a(1,4) * b(4,1)

		a = b * (fabsmax / det)

	end subroutine invert

!----------------------------------------------------------------
! Given the values of the Zeeman opacities, fill the absorption matrix
! for each depth point
!----------------------------------------------------------------
		subroutine fill_matrix(opacity,ab_matrix)
		real(kind=8) :: opacity(:,:), ab_matrix(:,:,:)

			ab_matrix(1,1,:) = opacity(1,:)   !eta_I
			ab_matrix(2,2,:) = opacity(1,:)   !eta_I
			ab_matrix(3,3,:) = opacity(1,:)   !eta_I
			ab_matrix(4,4,:) = opacity(1,:)   !eta_I
			ab_matrix(1,2,:) = opacity(2,:)   !eta_Q
			ab_matrix(2,1,:) = opacity(2,:)   !eta_Q
			ab_matrix(1,3,:) = opacity(3,:)   !eta_U
			ab_matrix(3,1,:) = opacity(3,:)   !eta_U
			ab_matrix(1,4,:) = opacity(4,:)   !eta_V
			ab_matrix(4,1,:) = opacity(4,:)   !eta_V
			ab_matrix(2,3,:) = opacity(7,:)   !rho_V
			ab_matrix(3,2,:) = -opacity(7,:)  !-rho_V
			ab_matrix(2,4,:) = -opacity(6,:)  !-rho_U
			ab_matrix(4,2,:) = opacity(6,:)   !rho_U
			ab_matrix(3,4,:) = opacity(5,:)   !rho_Q
			ab_matrix(4,3,:) = -opacity(5,:)  !-rho_Q

		end subroutine fill_matrix

! ---------------------------------------------------------
! Solves RTE using short-characteristics. Source function is supposed to vary parabolically
! between points M, O and P and RTE is integrated analitically, resulting in
!            I(O)=I(M)*exp(-tau(MO))+psi(M)*S(M)+psi(O)*S(O)+psi(P)*S(P)
! where psi are functions of the optical distance tau(MO) and tau(OP)
! It returns the value of the three psi coefficients
! ---------------------------------------------------------
   subroutine par_sc(dtm, dtp, psim, psi0, psip)
   real(kind=8) :: short_car_parab
   real(kind=8), INTENT(IN) :: dtm, dtp
	real(kind=8), INTENT(INOUT) :: psim, psi0, psip
   real(kind=8) :: exu, u0, u1 ,u2, d2, d3, d4

         if (dtm.ge.1.d-4) then
            exu=dexp(-dtm)
            u0=1.d0-exu
            u1=dtm-1.d0+exu
            u2=dtm**2-2.d0*dtm+2.d0-2.d0*exu
         else
            d2=dtm**2
            d3=dtm**3
            d4=dtm**4
            u0=dtm-(d2/2.d0)
            u1=(d2/2.d0)-(d3/6.d0)
            u2=(d3/3.d0)-(d4/12.d0)
        endif

        if (dtm * dtp /= 0.d0 .and. dtm**2 /= 0.d0 .and. dtp**2 /= 0.d0) then
			  psim=(u2-u1*(dtp+2.d0*dtm))/(dtm*(dtm+dtp))+u0
      	  psi0=(u1*(dtm+dtp)-u2)/(dtm*dtp)
      	  psip=(u2-dtm*u1)/(dtp*(dtm+dtp))
	 	  else
		  	  psim = 0.d0
			  psi0 = 0.d0
			  psip = 0.d0
		  endif

   end subroutine par_sc

! ---------------------------------------------------------
! Solves RTE using short-characteristics. Source function is supposed to vary parabolically
! between points M, O and P and RTE is integrated analitically, resulting in
!            I(O)=I(M)*exp(-tau(MO))+psi(M)*S(M)+psi(O)*S(O)+psi(P)*S(P)
! where psi are functions of the optical distance tau(MO) and tau(OP)
! It returns the value of the three psi coefficients
! Works for vectorial functions
! ---------------------------------------------------------
   subroutine par_sc_vect(dtm, dtp, psim, psi0, psip)
   real(kind=8), INTENT(IN) :: dtm(:), dtp(:)
	real(kind=8), INTENT(INOUT) :: psim(size(dtm)), psi0(size(dtm)), psip(size(dtm))
   real(kind=8) :: exu(size(dtm)), u0(size(dtm)), u1(size(dtm)) ,u2(size(dtm)), d2(size(dtm))
	real(kind=8) :: d3(size(dtm)), d4(size(dtm))

	where (dtm >= 1.d-4)
		exu = dexp(-dtm)
		u0=1.d0-exu
		u1=dtm-1.d0+exu
		u2=dtm**2-2.d0*dtm+2.d0-2.d0*exu
	elsewhere
		d2=dtm**2
		d3=dtm**3
		d4=dtm**4
		u0=dtm-(d2/2.d0)
		u1=(d2/2.d0)-(d3/6.d0)
		u2=(d3/3.d0)-(d4/12.d0)
	endwhere

	psim = 0.d0
	psip = 0.d0
	psi0 = 0.d0

	where(dtm * dtp /= 0.d0 .and. dtm**2 /= 0.d0 .and. dtp**2 /= 0.d0)
		psim=(u2-u1*(dtp+2.d0*dtm))/(dtm*(dtm+dtp))+u0
		psi0=(u1*(dtm+dtp)-u2)/(dtm*dtp)
		psip=(u2-dtm*u1)/(dtp*(dtm+dtp))
	endwhere

   end subroutine par_sc_vect

! ---------------------------------------------------------
! Solves RTE using short-characteristics. Source function is supposed to vary linearly
! between points M and O and RTE is integrated analitically, resulting in
!            I(O)=I(M)*exp(-tau(MO))+psi(M)*S(M)+psi(O)*S(O)
! where psi are functions of the optical distance tau(MO)
! It returns the value of the two psi coefficients
! ---------------------------------------------------------
   subroutine lin_sc(dtm, psim, psi0)
   real(kind=8) :: short_car_linea
   real(kind=8), INTENT(IN) :: dtm
	real(kind=8), INTENT(INOUT) :: psim, psi0
   real(kind=8) :: exu, u0, u1, c0, cm, d2

      if (dtm.ge.1.d-4) then
         exu=dexp(-dtm)
         u0=1.d0-exu
         u1=dtm-1.d0+exu

         c0=u1/dtm
         cm=u0-c0
      else
         d2=dtm**2.d0
         c0=(dtm/2.d0)-(d2/6.d0)
         cm=(dtm/2.d0)-(d2/3.d0)
      endif
		psi0 = c0
		psim = cm

   end subroutine lin_sc

! ---------------------------------------------------------
! Solves RTE using short-characteristics. Source function is supposed to vary linearly
! between points M and O and RTE is integrated analitically, resulting in
!            I(O)=I(M)*exp(-tau(MO))+psi(M)*S(M)+psi(O)*S(O)
! where psi are functions of the optical distance tau(MO)
! It returns the value of the two psi coefficients
! Works for vectorial functions
! ---------------------------------------------------------
   subroutine lin_sc_vect(dtm, psim, psi0)
   real(kind=8), INTENT(IN) :: dtm(:)
	real(kind=8), INTENT(INOUT) :: psim(size(dtm)), psi0(size(dtm))
   real(kind=8) :: exu(size(dtm)), u0(size(dtm)), u1(size(dtm)), c0(size(dtm)), cm(size(dtm)), d2(size(dtm))

	where (dtm >= 1.d-4)
		exu=dexp(-dtm)
		u0=1.d0-exu
		u1=dtm-1.d0+exu
		c0=u1/dtm
		cm=u0-c0
	elsewhere
		d2=dtm**2.d0
		c0=(dtm/2.d0)-(d2/6.d0)
		cm=(dtm/2.d0)-(d2/3.d0)
	endwhere

	psi0 = c0
	psim = cm

   end subroutine lin_sc_vect

! ---------------------------------------------------------
! Performs the formal solution of the RT equation for a plane-parallel atmosphere
! for a given source function and opacity
! Opacity and source function have the index as
! opacity(depth,frequency)
! ---------------------------------------------------------
	function formal_sol_vect(height, opacity, source, mu, boundary, idir)
	real(kind=8) :: height(:), opacity(:,:), source(:,:), boundary(:), mu
	real(kind=8) :: formal_sol_vect(size(boundary))
	integer :: k, km, kp, idir, k0, kf, ndepths, i
	real(kind=8), dimension(size(boundary)) :: chim, chi0, chip, sm, s0, sp, Inten, dtp, dtm, exu
	real(kind=8), dimension(size(boundary)) :: psim, psi0, psip
	real(kind=8) :: dm, dp

		ndepths = size(height)

! Boundary condition
		if (idir == 1) then
			k0 = 2
			kf = ndepths
			Inten = boundary
		endif
		if (idir == -1) then
			k0 = ndepths-1
			kf = 1
			Inten = boundary
		endif

		do k = k0, kf, idir

! Parabolic short-characteristics
			if (k /= kf) then
				km = k - idir
				kp = k + idir
				chim = opacity(km,:)
				chi0 = opacity(k,:)
				chip = opacity(kp,:)
				sm = source(km,:)
				s0 = source(k,:)
				sp = source(kp,:)
				dm = dabs(1.d0/mu*(height(k) - height(km)))
				dp = dabs(1.d0/mu*(height(kp) - height(k)))
			else
! Linear short-characteristics
				km = k - idir
				chim = opacity(km,:)
				chi0 = opacity(k,:)
				chip = 0.d0
				sm = source(km,:)
				s0 = source(k,:)
				sp = 0.d0
				dm = dabs(1.d0/mu*(height(k) - height(km)))
				dp = 0.d0
			endif

			where(chim <= 0.d0)
				chim = 1.d-15
			endwhere
			where(chi0 <= 0.d0)
				chi0 = 1.d-15
			endwhere
			where(chip <= 0.d0)
				chip = 1.d-15
			endwhere
			dtm = 0.5d0 * (chim + chi0) * dm
			dtp = 0.5d0 * (chi0 + chip) * dp

			! do i = 1, size(dtm)
			! 	print *, dtm(i)
			! enddo

			where (dtm >= 1.d-4)
				exu = dexp(-dtm)
			elsewhere
				exu = 1.d0 - dtm + 0.5d0 * dtm**2.d0
			endwhere

			if (k /= kf) then
				call par_sc_vect(dtm,dtp,psim,psi0,psip)
				Inten = Inten * exu + psim*sm + psi0*s0 + psip*sp
			else
				call lin_sc_vect(dtm,psim,psi0)
				Inten = Inten * exu + psim*sm + psi0*s0
			endif

		enddo

		formal_sol_vect = Inten

	end function formal_sol_vect

! ---------------------------------------------------------
! Performs the formal solution of the RT equation for a plane-parallel magnetized atmosphere
! for a given source function and opacity
! ---------------------------------------------------------
	function formal_sol_polarized(height, opacity, source, mu, boundary, idir)
	real(kind=8) :: height(:), opacity(:,:), source(:), mu, boundary(:)
	real(kind=8) :: formal_sol_polarized(4), Inten(4)
	integer :: k, km, kp, idir, k0, kf
	real(kind=8) :: chim, chi0, chip, dtp, dtm, exu
	real(kind=8) :: psim, psi0, psip, psim_lin, psi0_lin, dm, dp

	integer :: i, j, n
	real(kind=8), allocatable :: ab_matrix(:,:,:), source_vector(:,:)
	real(kind=8) :: sm(4), s0(4), sp(4), mat1(4,4), mat2(4,4), mat3(4,4)

		n = size(height)

		allocate(ab_matrix(4,4,n))
		allocate(source_vector(4,n))

		call fill_matrix(opacity,ab_matrix)

! Transform K into K* and then into K'
		do i = 1, 4
			do j = 1, 4
				ab_matrix(i,j,:) = ab_matrix(i,j,:) / opacity(1,:)
			enddo
			ab_matrix(i,i,:) = ab_matrix(i,i,:) - 1.d0
			source_vector(i,:) = opacity(i,:) * source / opacity(1,:)
		enddo

! Boundary condition
		if (idir == 1) then
			k0 = 2
			kf = n
			Inten = boundary
		endif
		if (idir == -1) then
			k0 = n-1
			kf = 1
			Inten = boundary
		endif

		do k = k0, kf, idir

! Parabolic short-characteristics
			if (k /= kf) then
				km = k - idir
				kp = k + idir
				chim = opacity(1,km)
				chi0 = opacity(1,k)
				chip = opacity(1,kp)
				sm = source_vector(:,km)
				s0 = source_vector(:,k)
				sp = source_vector(:,kp)
				dm = dabs(1.d0/mu*(height(k) - height(km)))
				dp = dabs(1.d0/mu*(height(kp) - height(k)))
			else
! Linear short-characteristics
				km = k - idir
				chim = opacity(1,km)
				chi0 = opacity(1,k)
				chip = 0.d0
				sm = source_vector(:,km)
				s0 = source_vector(:,k)
				sp = 0.d0
				dm = dabs(1.d0/mu*(height(k) - height(km)))
				dp = 0.d0
			endif

			dtm = 0.5d0 * (chim + chi0) * dm
			dtp = 0.5d0 * (chi0 + chip) * dp

			if (dtm >= 1.d-4) then
				exu = dexp(-dtm)
			else
				exu = 1.d0 - dtm + 0.5d0 * dtm**2.d0
			endif

			call lin_sc(dtm,psim_lin,psi0_lin)
			mat1 = exu * identity - psim_lin*ab_matrix(:,:,km)
			mat2 = identity + psi0_lin * ab_matrix(:,:,k)
			call invert(mat2)

			if (k /= kf) then
				call par_sc(dtm,dtp,psim,psi0,psip)
				Inten = matmul(mat2,matmul(mat1,Inten) + psim*sm + psi0*s0 + psip*sp)
			else
				call lin_sc(dtm,psim,psi0)
				Inten = matmul(mat2,matmul(mat1,Inten) + psim*sm + psi0*s0)
			endif

		enddo

		deallocate(ab_matrix)
		deallocate(source_vector)

		formal_sol_polarized = Inten

	end function formal_sol_polarized

end module maths_mod


! Sample solution to the Lab 10 problem of computing a quadratic form

program quadform

    use omp_lib
    implicit none
    integer, parameter :: n=10
    integer :: nthreads, thread_num

    real (kind=8), dimension(n,n) :: A
    real (kind=8), dimension(n) :: x, Ax
    real (kind=8) :: xAx

    integer :: i,j

    nthreads = 1  ! if not using OpenMP
    !$ call omp_set_num_threads(6)

    ! initialize matrix A and vector x:
    A = 0.d0
    !$omp parallel do
    do i=1,n
        A(i,i) = 1.d0
        x(i) = i
        enddo

    ! multiply A*x to get Ax:
    !$omp parallel do private(j)
    do i=1,n
        Ax(i) = 0.d0
        do j=1,n
            Ax(i) = Ax(i) + A(i,j)*x(j)
            enddo
        enddo

    xAx = 0.d0
    !$omp parallel do reduction(+:xAx)
    do i=1,n
        xAx = xAx + x(i) * Ax(i)
        enddo

    print 600, xAx
600 format("xAx = ", e22.15)

end program quadform


      SUBROUTINE MTRIRG
C
C
C*****
C THIS ROUTINE COMPUTES THE   MASS    MATRIX FOR A AXI-SYMMETRIC RING
C WITH A TRIANGULAR CROSS SECTION
C*****
C
C
C                        ECPT FOR THE TRIANGULAR RING
C
C
C                                                      TYPE
C ECPT( 1) ELEMENT IDENTIFICATION                        I
C ECPT( 2) SCALAR INDEX NO. FOR GRID POINT A             I
C ECPT( 3) SCALAR INDEX NO. FOR GRID POINT B             I
C ECPT( 4) SCALAR INDEX NO. FOR GRID POINT C             I
C ECPT( 5) MATERIAL ORIENTATION ANGLE(DEGREES)           R
C ECPT( 6) MATERIAL IDENTIFICATION                       I
C ECPT( 7) COOR. SYS. ID. FOR GRID POINT A               I
C ECPT( 8) X-COOR. OF GRID POINT A (IN BASIC COOR.)      R
C ECPT( 9) Y-COOR. OF GRID POINT A (IN BASIC COOR.)      R
C ECPT(10) Z-COOR. OF GRID POINT A (IN BASIC COOR.)      R
C ECPT(11) COOR. SYS. ID. FOR GRID POINT B               I
C ECPT(12) X-COOR. OF GRID POINT B (IN BASIC COOR.)      R
C ECPT(13) Y-COOR. OF GRID POINT B (IN BASIC COOR.)      R
C ECPT(14) Z-COOR. OF GRID POINT B (IN BASIC COOR.)      R
C ECPT(15) COOR. SYS. ID. FOR GRID POINT C               I
C ECPT(16) X-COOR. OF GRID POINT C (IN BASIC COOR.)      R
C ECPT(17) Y-COOR. OF GRID POINT C (IN BASIC COOR.)      R
C ECPT(18) Z-COOR. OF GRID POINT C (IN BASIC COOR.)      R
C ECPT(19) EL. TEMPERATURE FOR MATERIAL PROPERTIES       R
C
C
      DOUBLE PRECISION CONSTD, D2PI
      DOUBLE PRECISION
     1                   D ,  GAMBQ,    R,   Z
     2,                            DELINT,   AK,  AKI
     3,                  AKT,  AM
      DOUBLE PRECISION   R1,  R2,  R3,  Z1,  Z2,  Z3,  ZMIN, DGAMA
     1,                  DR,  RH,  DZ,  ZH,  RA,  ZA,  AREA
     2,                  TWOPI,    RHOD,     DKI
C
      DIMENSION          IECPT(19)
      DIMENSION  AM(36)
C
      COMMON /CONDAD/ CONSTD(5)
      COMMON   /SMA2IO/
     1                   DUM1(10)
     2,                  IFMGG
     3,                  DUM2(25)
      COMMON   /SMA2CL/
     1                   DUM3(2)
     2,                  NPVT
     3,                  DUM4(7)
     4,                  LINK(10)           ,NOGO
      COMMON   /SMA2ET/
     1                   ECPT(19)
     2,                  DUM5(81)
      COMMON   /MATIN/
     1                   MATIDC             ,MATFLG
     2,                  ELTEMP             ,STRESS
     3,                  SINTH              ,COSTH
      COMMON   /MATOUT/
     1                   E(3)               ,ANU(3)
     2,                  RHO                ,G(3)
     3,                  ALF(3)             ,TZERO
      COMMON   /SMA2DP/
     1                   D(36) ,   GAMBQ(36),     R(3) ,    Z(3)
     2,                                      DELINT(8),     AK(36)
     3,                  AKI(36),  AKT(9)
     4,                  DGAMA,    ZMIN,     RHOD,     TWOPI
     5,                  DR,  RH,  DZ,  ZH,  RA,  ZA,  AREA
     8,                  IGP(3) , ICS(3) , SP(18)
     9,                  TEMPE
C
      EQUIVALENCE        (IECPT(1) , ECPT(1))
      EQUIVALENCE   (R(1),R1),     (R(2),R2),     (R(3),R3)
     1,             (Z(1),Z1),     (Z(2),Z2),     (Z(3),Z3)
      EQUIVALENCE  (AM(1) , AK(1))
      EQUIVALENCE ( CONSTD(2) , D2PI   )
C
C ----------------------------------------------------------------------
C
C STORE ECPT PARAMETERS IN LOCAL VARIABLES
C
      IDEL  = IECPT(1)
      IGP(1)= IECPT(2)
      IGP(2)= IECPT(3)
      IGP(3)= IECPT(4)
      MATID = IECPT(6)
      ICS(1)= IECPT(7)
      ICS(2)= IECPT(11)
      ICS(3)= IECPT(15)
      R(1)  = ECPT(8)
      D(1)  = ECPT(9)
      Z(1)  = ECPT(10)
      R(2)  = ECPT(12)
      D(2)  = ECPT(13)
      Z(2)  = ECPT(14)
      R(3)  = ECPT(16)
      D(3)  = ECPT(17)
      Z(3)  = ECPT(18)
      TEMPE = ECPT(19)
      DGAMA = ECPT(5)
C
C
C CHECK INTERNAL GRID POINTS FOR PIVOT POINT
C
      IPP = 0
      DO 100 I = 1,3
      IF (NPVT .EQ. IGP(I)) IPP = I
  100 CONTINUE
      IF (IPP .EQ. 0) CALL MESAGE (-30,34,IDEL)
C
C
C TEST THE VALIDITY OF THE GRID POINT COORDINATES
C
      IEROR1 = 0
      DO 200 I = 1,3
      IF (R(I).GT.0.0D0) GO TO 200
      IF (IEROR1.NE.0) GO TO 200
      CALL MESAGE (30, 211, IDEL)
      IEROR1 = 1
  200 CONTINUE
      IEROR2 = 0
      DO 210 I = 1, 3
      IF (D(I).EQ.0.0D0) GO TO 210
      IF (IEROR2.NE.0) GO TO 210
      CALL MESAGE (30, 212, IDEL)
      IEROR2 = 1
  210 CONTINUE
C
C
C COMPUTE THE ELEMENT COORDINATES
C
      IF (IEROR1.EQ.0.AND.IEROR2.EQ.0) GO TO 220
      NOGO = 2
      RETURN
  220 ZMIN = DMIN1 (Z1, Z2, Z3)
      Z1 = Z1 - ZMIN
      Z2 = Z2 - ZMIN
      Z3 = Z3 - ZMIN
C
C
C
C FORM THE TRANSFORMATION MATRIX (6X6) FROM FIELD COORDINATES TO GRID
C POINT DEGREES OF FREEDOM
C
      DO 300 I = 1,36
      GAMBQ(I) = 0.0D0
  300 CONTINUE
      GAMBQ( 1) = 1.0D0
      GAMBQ( 2) = R1
      GAMBQ( 3) = Z1
      GAMBQ(10) = 1.0D0
      GAMBQ(11) = R1
      GAMBQ(12) = Z1
      GAMBQ(13) = 1.0D0
      GAMBQ(14) = R2
      GAMBQ(15) = Z2
      GAMBQ(22) = 1.0D0
      GAMBQ(23) = R2
      GAMBQ(24) = Z2
      GAMBQ(25) = 1.0D0
      GAMBQ(26) = R3
      GAMBQ(27) = Z3
      GAMBQ(34) = 1.0D0
      GAMBQ(35) = R3
      GAMBQ(36) = Z3
C
C
C     NONEED TO COMPUTE DETERMINANT SINCE IT IS NOT USED SUBSEQUENTLY.
      ISING = -1
      CALL INVERD (6, GAMBQ(1),6 , D(10), 0, D(11) , ISING , SP)
C
      IF (ISING.EQ.2) GO TO 920
C
C
C
C CALCULATE THE INTEGRAL VALUES IN ARRAY DELINT WHERE THE ORDER IS
C INDICATED BY THE FOLLOWING TABLE
C
C              DELINT( 1) - ( 1,0)
C              DELINT( 2) - ( 1,1)
C              DELINT( 3) - ( 1,2)
C              DELINT( 4) - ( 2,0)
C              DELINT( 5) - ( 2,1)
C              DELINT( 6) - ( 0,2)
C              DELINT( 7) - ( 3,0)
C              DELINT( 8) - (-1,2)
C
C
C TEST FOR RELATIVE SMALL AREA OF INTEGRATION
C AND IF AREA IS SMALL THEN APPROXIMATE INTEGRALS
C
      DR = DMAX1 ( DABS(R1-R2) , DABS(R2-R3) , DABS(R3-R1) )
      RH = DMIN1 ( R1 , R2 , R3 ) / 10.0D0
      DZ = DMAX1 ( DABS(Z1-Z2) , DABS(Z2-Z3) , DABS(Z3-Z1) )
      ZH = DMIN1 ( Z1 , Z2 , Z3 ) / 10.0D0
      RA = (R1 + R2 + R3) / 3.0D0
      ZA = (Z1 + Z2 + Z3) / 3.0D0
      AREA =(R1*(Z2-Z3) + R2*(Z3-Z1) + R3*(Z1-Z2)) / 2.0D0
      KODE = 0
      IF (DABS( (R2-R1)/R2 ) .LT. 1.0D-5) KODE = 1
      IF ( DR .LE. RH  .OR.  DZ .LE. ZH ) KODE = -1
C
C
  310 CONTINUE
      I1 = 0
      DO 400 I = 1,3
      IP = I
      DO 350 J = 1,3
      IQ = J - 1
      IF (IP.EQ.2 .AND. IQ.EQ.2) IP = 0
      IF (IP.EQ.3 .AND. IQ.EQ.2) IP = -1
      IF (IP.EQ.3 .AND. IQ.EQ.1) GO TO 350
      I1 = I1 + 1
      IF (KODE) 320,330,340
  320 DELINT(I1) =((RA) ** IP)*((ZA) ** IQ) * AREA
      GO TO 350
  330 DELINT(I1) = DKI(1,3,1,2,1,3,IP,IQ,R,Z)
     1           + DKI(3,2,1,2,3,2,IP,IQ,R,Z)
      GO TO 350
  340 CONTINUE
      DELINT(I1) = DKI(1,3,3,2,1,3,IP,IQ,R,Z)
  350 CONTINUE
  400 CONTINUE
C
C
C TEST FOR EXCESSIVE ROUND-OFF ERROR IN INTEGRAL CALCULATIONS
C AND IF IT EXIST APPROXIMATE INTEGRALS
C
      IF (KODE .LT. 0) GO TO 500
      DO 450 I = 1,8
      IF (DELINT(I) .LT. 0.0D0) GO TO 475
  450 CONTINUE
      IF (DELINT(3) .LE. DELINT(6)) GO TO 475
      IF (DELINT(8) .GE. DELINT(3)) GO TO 475
      IF (DELINT(8) .GT. DELINT(6)) GO TO 475
      GO TO 500
  475 CONTINUE
      KODE = -1
      GO TO 310
  500 CONTINUE
C
C
C
C LOCATE THE MATERIAL PROPERTIES IN THE MAT1 OR MAT3 TABLE
C
      MATIDC = MATID
      MATFLG = 7
      ELTEMP = TEMPE
      CALL  MAT (IDEL)
C
C
C SET MATERIAL PROPERTIES IN DOUBLE PRECISION VARIABLES
C
      RHOD = RHO
C
C GENERATE THE CONSISTENT MASS MATRIX IN FIELD COORDINATES
C
      DO 600 I = 1,36
      AM(I) = 0.0D0
  600 CONTINUE
      TWOPI = D2PI * RHOD
      AM( 1)= TWOPI *       DELINT(1)
      AM( 2)= TWOPI *       DELINT(4)
      AM( 3)= TWOPI *       DELINT(2)
      AM( 7)= AM( 2)
      AM( 8)= TWOPI *       DELINT(7)
      AM( 9)= TWOPI *       DELINT(5)
      AM(13)= AM( 3)
      AM(14)= AM( 9)
      AM(15)= TWOPI *       DELINT(3)
      AM(22)= AM( 1)
      AM(23)= AM( 2)
      AM(24)= AM( 3)
      AM(28)= AM(23)
      AM(29)= AM( 8)
      AM(30)= AM( 9)
      AM(34)= AM(24)
      AM(35)= AM(30)
      AM(36)= AM(15)
C
C TRANSFORM THE ELEMENT   MASS    MATRIX FROM FIELD COORDINATES
C TO GRID POINT DEGREES OF FREEDOM
C
      CALL GMMATD (GAMBQ , 6, 6, 1, AK , 6, 6, 0, D )
      CALL GMMATD (D  , 6, 6, 0, GAMBQ , 6, 6, 0, AK)
C
C
C
C ZERO OUT THE (6X6) MATRIX USED AS INPUT TO THE INSERTION ROUTINE
C
      DO 700 I = 1,36
      AKI(I) = 0.0D0
  700 CONTINUE
C
C
C LOCATE THE TRANSFORMATION MATRICES FOR THE THREE GRID POINTS
C
      DO 800 I = 1,3
      IF (ICS(I) .EQ. 0) GO TO 800
      K = 9 * (I-1) + 1
      CALL TRANSD (ICS(I) , D(K))
  800 CONTINUE
C
C
C
C START THE LOOP FOR INSERTION OF THE THREE (6X6) MATRICES
C INTO THE MASTER MASS MATRIX
C
      IR1  = 2 * IPP - 1
      IAPP = 9 * (IPP-1) + 1
      DO 900 I = 1,3
C
C PLACE THE APPROIATE (2X2) SUBMATRIX OF THE MASS MATRIX
C IN A (3X3) MATRIX FOR TRANSFORMATION
C
      IC1 = 2 * I - 1
      IRC = (IR1 - 1) * 6 + IC1
      AKT(1) = AK(IRC)
      AKT(2) = 0.0D0
      AKT(3) = AK(IRC+1)
      AKT(4) = 0.0D0
      AKT(5) = 0.0D0
      AKT(6) = 0.0D0
      AKT(7) = AK(IRC+6)
      AKT(8) = 0.0D0
      AKT(9) = AK(IRC+7)
C
C TRANSFORM THE (3X3) MASS MATRIX
C
      IF (ICS(IPP) .EQ. 0) GO TO 820
      CALL GMMATD (D(IAPP) , 3, 3, 1, AKT(1) , 3, 3, 0, D(28) )
      DO 810 J = 1,9
      AKT(J) = D(J+27)
  810 CONTINUE
  820 CONTINUE
      IF (ICS(I) .EQ. 0) GO TO 840
      IAI = 9 * (I - 1) + 1
      CALL GMMATD (AKT(1) , 3, 3, 0, D(IAI) , 3, 3, 0, D(28) )
      DO 830 J = 1,9
      AKT(J) = D(J+27)
  830 CONTINUE
  840 CONTINUE
C
C PLACE THE TRANSFORMED (3X3) MATRIX INTO A (6X6) MATRIX FOR
C THE INSERTION ROUTINE
C
      J = 0
      DO 850 J1 = 1,18,6
      DO 850 J2 = 1,3
      J = J + 1
      K = J1 + J2 - 1
      AKI(K) = AKT(J)
  850 CONTINUE
C
C CALL THE INSERTION ROUTINE
C
      CALL SMA2B (AKI(1) , IGP(I), -1, IFMGG, 0.0D0)
  900 CONTINUE
      RETURN
C
C  SET FLAG FOR FATAL ERROR WHILE ALLOWING ERROR MESSAGES TO ACCUMULATE
C
  915 NOGO=1
      RETURN
  920 CALL MESAGE(30,37,IDEL)
      GO TO 915
C
      END


module cc_smoothers_module

  use amrex_fort_module, only : rt => amrex_real
  use bl_constants_module
  use cc_stencil_module

  implicit none

contains

  subroutine gs_rb_smoother_3d(omega, ss, uu, ff, mm, lo, ng, n, skwd)
    use bl_prof_module
    integer, intent(in) :: ng
    integer, intent(in) :: lo(:)
    integer, intent(in) :: n
    real (rt), intent(in) :: omega
    real (rt), intent(in) :: ff(lo(1):,lo(2):,lo(3):)
    real (rt), intent(inout) :: uu(lo(1)-ng:,lo(2)-ng:,lo(3)-ng:)
    real (rt), intent(in) :: ss(0:,lo(1):, lo(2):, lo(3):)
    integer            ,intent(in) :: mm(lo(1):,lo(2):,lo(3):)
    logical, intent(in), optional :: skwd
    integer :: i, j, k, ioff
    integer :: hi(size(lo))
    integer, parameter ::  XBC = 7, YBC = 8, ZBC = 9
    logical :: lskwd
    real(rt) :: dd, dhsq_inv, ss0, ss0_inv

    type(bl_prof_timer), save :: bpt

    call build(bpt, "gs_rb_smoother_3d")

    hi = ubound(ff)

    ss0_inv  =  1.d0/ss(0,lo(1),lo(2),lo(3))
    dhsq_inv = -ss(0,lo(1),lo(2),lo(3))/6.d0

    !$OMP PARALLEL DO PRIVATE(k,j,i,ioff,dd) IF((hi(3)-lo(3)).ge.3)
    do k = lo(3), hi(3)
       do j = lo(2), hi(2)
          ioff = 0; if ( mod (lo(1) + j + k, 2) /= n ) ioff = 1
          do i = lo(1)+ioff, hi(1), 2

             dd = (-6.d0*uu(i,j,k)   + &
                   uu(i+1,j,k) + uu(i-1,j,k) + &
                   uu(i,j+1,k) + uu(i,j-1,k) + &
                   uu(i,j,k+1) + uu(i,j,k-1) ) * dhsq_inv

             uu(i,j,k) = uu(i,j,k) + (ff(i,j,k) - dd)*ss0_inv
!            uu(i,j,k) = uu(i,j,k) + (ff(i,j,k) - dd)* 1.d0/ss(0,i,j,k)

          end do
       end do
    end do
    !$OMP END PARALLEL DO

    call destroy(bpt)

  end subroutine gs_rb_smoother_3d

end module cc_smoothers_module


************************************************************************
* SUBROUTINE PSQP_WRAP          ALL SYSTEMS                 2013/02/06
* PURPOSE :
* WRAPPING SUBROUTINE FOR PSQP.
*
* PARAMETERS :
*  II  NF  NUMBER OF VARIABLES.
*  II  NC  NUMBER OF LINEAR CONSTRAINTS.
*  RI  X(NF)  VECTOR OF VARIABLES.
*  II  IX(NF)  VECTOR CONTAINING TYPES OF BOUNDS. IX(I)=0-VARIABLE
*         X(I) IS UNBOUNDED. IX(I)=1-LOVER BOUND XL(I).LE.X(I).
*         IX(I)=2-UPPER BOUND X(I).LE.XU(I). IX(I)=3-TWO SIDE BOUND
*         XL(I).LE.X(I).LE.XU(I). IX(I)=5-VARIABLE X(I) IS FIXED.
*  RI  XL(NF)  VECTOR CONTAINING LOWER BOUNDS FOR VARIABLES.
*  RI  XU(NF)  VECTOR CONTAINING UPPER BOUNDS FOR VARIABLES.
*  RI  CF(NC+1)  VECTOR CONTAINING VALUES OF THE CONSTRAINT FUNCTIONS.
*  II  IC(NC)  VECTOR CONTAINING TYPES OF CONSTRAINTS.
*         IC(KC)=0-CONSTRAINT CF(KC) IS NOT USED. IC(KC)=1-LOVER
*         CONSTRAINT CL(KC).LE.CF(KC). IC(KC)=2-UPPER CONSTRAINT
*         CF(KC).LE.CU(KC). IC(KC)=3-TWO SIDE CONSTRAINT
*         CL(KC).LE.CF(KC).LE.CU(KC). IC(KC)=5-EQUALITY CONSTRAINT
*         CF(KC).EQ.CL(KC).
*  RI  CL(NC)  VECTOR CONTAINING LOWER BOUNDS FOR CONSTRAINT FUNCTIONS.
*  RI  CU(NC)  VECTOR CONTAINING UPPER BOUNDS FOR CONSTRAINT FUNCTIONS.
*  II  MIT  MAXIMUN NUMBER OF ITERATIONS.
*  II  MFV  MAXIMUN NUMBER OF FUNCTION EVALUATIONS.
*  II  MET  VARIABLE METRIC UPDATE USED. MET=1-THE BFGS UPDATE.
*         MET=2-THE HOSHINO UPDATE.
*  II  MEC  CORRECTION IF THE NEGATIVE CURVATURE OCCURS.
*         MEC=1-CORRECTION SUPPRESSED. MEC=2-POWELL'S CORRECTION.
*  RI  XMAX  MAXIMUM STEPSIZE.
*  RI  TOLX  TOLERANCE FOR CHANGE OF VARIABLES.
*  RI  TOLC  TOLERANCE FOR CONSTRAINT VIOLATIONS.
*  RI  TOLG  TOLERANCE FOR THE GRADIENT OF THE LAGRANGIAN FUNCTION.
*  RI  RPF  PENALTY COEFFICIENT.
*  RO  F  VALUE OF THE OBJECTIVE FUNCTION.
*  RO  GMAX  MAXIMUM PARTIAL DERIVATIVE OF THE LAGRANGIAN FUNCTION.
*  RO  CMAX  MAXIMUM CONSTRAINT VIOLATION.
*  II  IPRNT  PRINT SPECIFICATION. IPRNT=0-NO PRINT.
*         ABS(IPRNT)=1-PRINT OF FINAL RESULTS.
*         ABS(IPRNT)=2-PRINT OF FINAL RESULTS AND ITERATIONS.
*         IPRNT>0-BASIC FINAL RESULTS. IPRNT<0-EXTENDED FINAL
*         RESULTS.
*  II  IOUT  PRINT OUTPUT UNIT NUMBER.
*  II  IFILE PRINT OUTPUT FILENAME.
*  IO  ITERM  VARIABLE THAT INDICATES THE CAUSE OF TERMINATION.
*         ITERM=1-IF ABS(X-XO) WAS LESS THAN OR EQUAL TO TOLX IN
*                   MTESX (USUALLY TWO) SUBSEQUEBT ITERATIONS.
*         ITERM=2-IF ABS(F-FO) WAS LESS THAN OR EQUAL TO TOLF IN
*                   MTESF (USUALLY TWO) SUBSEQUEBT ITERATIONS.
*         ITERM=3-IF F IS LESS THAN OR EQUAL TO TOLB.
*         ITERM=4-IF GMAX IS LESS THAN OR EQUAL TO TOLG.
*         ITERM=11-IF NIT EXCEEDED MIT. ITERM=12-IF NFV EXCEEDED MFV.
*         ITERM=13-IF NFG EXCEEDED MFG. ITERM<0-IF THE METHOD FAILED.
*         IF ITERM=-6, THEN THE TERMINATION CRITERION HAS NOT BEEN
*         SATISFIED, BUT THE POINT OBTAINED IF USUALLY ACCEPTABLE.
*
* VARIABLES IN COMMON /STAT/ (STATISTICS) :
*  IO  NRES  NUMBER OF RESTARTS.
*  IO  NDEC  NUMBER OF MATRIX DECOMPOSITION.
*  IO  NREM  NUMBER OF CONSTRAINT DELETIONS.
*  IO  NADD  NUMBER OF CONSTRAINT ADDITIONS.
*  IO  NIT  NUMBER OF ITERATIONS.
*  IO  NFV  NUMBER OF FUNCTION EVALUATIONS.
*  IO  NFG  NUMBER OF GRADIENT EVALUATIONS.
*  IO  NFH  NUMBER OF HESSIAN EVALUATIONS.
*
* SUBPROGRAMS USED :
*  S   PSQP  RECURSIVE QUADRATIC PROGRAMMING METHOD WITH THE BFGS
*         VARIABLE METRIC UPDATE.
*
* EXTERNAL SUBROUTINES :
*  SE  OBJ  COMPUTATION OF THE VALUE OF THE OBJECTIVE FUNCTION.
*         CALLING SEQUENCE: CALL OBJ(NF,X,FF) WHERE NF IS THE NUMBER
*         OF VARIABLES, X(NF) IS A VECTOR OF VARIABLES AND FF IS THE
*         VALUE OF THE OBJECTIVE FUNCTION.
*  SE  DOBJ  COMPUTATION OF THE GRADIENT OF THE OBJECTIVE FUNCTION.
*         CALLING SEQUENCE: CALL DOBJ(NF,X,GF) WHERE NF IS THE NUMBER
*         OF VARIABLES, X(NF) IS A VECTOR OF VARIABLES AND GC(NF) IS
*         THE GRADIENT OF THE OBJECTIVE FUNCTION.
*  SE  CON  COMPUTATION OF THE VALUE OF THE CONSTRAINT FUNCTION.
*         CALLING SEQUENCE: CALL CON(NF,KC,X,FC) WHERE NF IS THE
*         NUMBER OF VARIABLES, KC IS THE INDEX OF THE CONSTRAINT
*         FUNCTION, X(NF) IS A VECTOR OF VARIABLES AND FC IS THE
*         VALUE OF THE CONSTRAINT FUNCTION.
*  SE  DCON  COMPUTATION OF THE GRADIENT OF THE CONSTRAINT FUNCTION.
*         CALLING SEQUENCE: CALL DCON(NF,KC,X,GC) WHERE NF IS THE
*         NUMBER OF VARIABLES, KC IS THE INDEX OF THE CONSTRAINT
*         FUNCTION, X(NF) IS A VECTOR OF VARIABLES AND GC(NF) IS THE
*         GRADIENT OF THE CONSTRAINT FUNCTION.
*
      SUBROUTINE PSQP_WRAP(NF,NC,X,IX,XL,XU,CF,IC,CL,CU,
     +                 MIT,MFV,MET,MEC,XMAX,TOLX,TOLC,TOLG,RPF,
     +                 F,GMAX,CMAX,IPRNT,IOUT,IFILE,ITERM)
*
*     POINTERS FOR AUXILIARY ARRAYS
*
      DOUBLE PRECISION XMAX,TOLX,TOLC,TOLG,RPF,F,CMAX,GMAX
      INTEGER          MIT,MFV,MET,MEC,IPRNT,IOUT,ITERM,NB,NC,NF
      CHARACTER*(*)    IFILE
      DOUBLE PRECISION CF(NC+1),CL(NC),CU(NC)
      DOUBLE PRECISION X(NF),XL(NF),XU(NF)
      INTEGER          IC(NC),IX(NF)
      INTEGER          NADD,NDEC,NFG,NFH,NFV,NIT,NREM,NRES
      INTEGER          LCFD,LCFO,LCG,LCP,LCR,LCZ,LG,LGC,LGF,
     +                 LGO,LH,LIA,LS,LXO
      COMMON           /STAT/NRES,NDEC,NREM,NADD,NIT,NFV,NFG,NFH
*
*  ALLOCATE WORKING ARRAYS
*
      INTEGER IA(:)
      DOUBLE PRECISION RA(:)
      ALLOCATABLE IA,RA
      ALLOCATE (IA(NF),RA((NF+NC+8)*NF+3*NC+1))
      LCG = 1
      LCFO = LCG + NF*NC
      LCFD = LCFO + NC + 1
      LGC = LCFD + NC
      LCR = LGC + NF
      LCZ = LCR + NF*(NF+1)/2
      LCP = LCZ + NF
      LGF = LCP + NC
      LG = LGF + NF
      LH = LG + NF
      LS = LH + NF* (NF+1)/2
      LXO = LS + NF
      LGO = LXO + NF
      LIA = 1
*
*  OPEN WRITE FILE
*
      IF (IPRNT.NE.0) THEN
          OPEN(UNIT=IOUT,FILE=IFILE,STATUS='UNKNOWN')
      END IF
*
*  CALL PSQP
*
      NB = 0
      CALL PSQP(NF,NB,NC,X,IX,XL,XU,CF,IC,CL,CU,RA,RA(LCFO),RA(LCFD),
     +          RA(LGC),IA,RA(LCR),RA(LCZ),RA(LCP),RA(LGF),RA(LG),
     +          RA(LH),RA(LS),RA(LXO),RA(LGO),XMAX,TOLX,TOLC,
     +          TOLG,RPF,CMAX,GMAX,F,MIT,MFV,MET,
     +          MEC,IPRNT,IOUT,ITERM)
*
*  CLOSE WRITE FILE
*
      IF (IPRNT.NE.0) THEN
          CLOSE(IOUT)
      END IF
*
*  DEALLOCATE WORKING ARRAYS
*
      DEALLOCATE(IA,RA)
*
      RETURN
      END
************************************************************************
      SUBROUTINE FUN(NF,KA,X,FA)
      INTEGER KA,NF
      DOUBLE PRECISION FA,X(*)
      KA=NF
      FA=X(1)
      RETURN
      END
************************************************************************
      SUBROUTINE DFUN(NF,KA,X,GA)
      INTEGER KA,NF
      DOUBLE PRECISION GA(*),X(*)
      KA=NF
      GA(1)=X(1)
      RETURN
      END


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! This program implements a simple molecular dynamics simulation,
!   using the velocity Verlet time integration scheme. The particles
!   interact with a central pair potential.
!
! Author:   Bill Magro, Kuck and Associates, Inc. (KAI), 1998
!
! Parallelism is implemented via OpenMP directives.
! THIS PROGRAM USES THE FORTRAN90 RANDOM_NUMBER FUNCTION AND ARRAY
!   SYNTAX
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

      program md
      implicit none

      ! simulation parameters
      integer ndim       ! dimensionality of the physical space
      integer nparts     ! number of particles
      integer nsteps     ! number of time steps in the simulation
      parameter(ndim=3,nparts=500,nsteps=500)
      real*8 mass        ! mass of the particles
      real*8 dt          ! time step
      real*8 box(ndim)   ! dimensions of the simulation box
      parameter(mass=1.0,dt=1.0e-4)

      ! simulation variables
      real*8 position(ndim,nparts)
      real*8 velocity(ndim,nparts)
      real*8 force(ndim,nparts)
      real*8 accel(ndim,nparts)
      real*8 potential, kinetic, E0
      integer i , l

!!      box(1:ndim) = 10.
      	do l=1,ndim
	      box(l) = 10.
	enddo

      ! set initial positions, velocities, and accelerations
      call initialize(nparts,ndim,box,position,velocity,accel)

      ! compute the forces and energies
      call compute(nparts,ndim,box,position,velocity,mass,
     .                                      force,potential,kinetic)
      E0 = potential + kinetic

      ! This is the main time stepping loop
      do i=1,nsteps
          call compute(nparts,ndim,box,position,velocity,mass,
     .                                      force,potential,kinetic)
!$omp master
          write(*,*) potential, kinetic,(potential + kinetic - E0)/E0
!$omp end master
          call update(nparts,ndim,position,velocity,force,accel,mass,dt)
      enddo

      end

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! Compute the forces and energies, given positions, masses,
! and velocities
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      subroutine compute(np,nd,box,pos,vel,mass,f,pot,kin)
      implicit none

      integer np
      integer nd
      real*8  box(nd)
      real*8  pos(nd,np)
      real*8  vel(nd,np)
      real*8  f(nd,np)
      real*8  mass
      real*8  pot
      real*8  kin

      real*8 dotr8
      external dotr8
      real*8 v, dv, x

      integer i, j, k, l
      real*8  rij(nd)
      real*8  d
      real*8  PI2
      parameter(PI2=3.14159265d0/2.0d0)

      ! statement function for the pair potential and its derivative
      ! This potential is a harmonic well which smoothly saturates to a
      ! maximum value at PI/2.
      v(x) = sin(min(x,PI2))**2.
      dv(x) = 2.*sin(min(x,PI2))*cos(min(x,PI2))

      pot = 0.0
      kin = 0.0

      ! The computation of forces and energies is fully parallel.
!$omp  parallel do
!$omp& default(shared)
!$omp& private(i,j,k,rij,d)
!$omp& reduction(+ : pot, kin)
      do i=1,np
        ! compute potential energy and forces
!!        f(1:nd,i) = 0.0
	do l=1,nd
		f(l,i) = 0.0
	enddo

        do j=1,np
             if (i .ne. j) then
               call dist(nd,box,pos(1,i),pos(1,j),rij,d)
               ! attribute half of the potential energy to particle 'j'
               pot = pot + 0.5*v(d)
               do k=1,nd
                 f(k,i) = f(k,i) - rij(k)*dv(d)/d
               enddo
             endif
        enddo
        ! compute kinetic energy
        kin = kin + dotr8(nd,vel(1,i),vel(1,i))
      enddo
!$omp  end parallel do
      kin = kin*0.5*mass

      return
      end

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! Initialize the positions, velocities, and accelerations.
! The Fortran90 random_number function is used to choose positions.
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      subroutine initialize(np,nd,box,pos,vel,acc)
      implicit none

      integer np
      integer nd
      real*8  box(nd)
      real*8  pos(nd,np)
      real*8  vel(nd,np)
      real*8  acc(nd,np)

      integer i, j
      real*8 x

      do i=1,np
        do j=1,nd
          call random_number(x)
          pos(j,i) = box(j)*x
          vel(j,i) = 0.0
          acc(j,i) = 0.0
        enddo
      enddo

      return
      end

	subroutine   random_number(x)
	real*8 x
	x=0.0
	end

! Compute the displacement vector (and its norm) between two particles.
      subroutine dist(nd,box,r1,r2,dr,d)
      implicit none

      integer nd
      real*8 box(nd)
      real*8 r1(nd)
      real*8 r2(nd)
      real*8 dr(nd)
      real*8 d

      integer i

      d = 0.0
      do i=1,nd
        dr(i) = r1(i) - r2(i)
        d = d + dr(i)**2.
      enddo
      d = sqrt(d)

      return
      end

! Return the dot product between two vectors of type real*8 and length n
      real*8 function dotr8(n,x,y)
      implicit none

      integer n
      real*8 x(n)
      real*8 y(n)

      integer i

      dotr8 = 0.0
      do i = 1,n
        dotr8 = dotr8 + x(i)*y(i)
      enddo

      return
      end

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! Perform the time integration, using a velocity Verlet algorithm
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      subroutine update(np,nd,pos,vel,f,a,mass,dt)
      implicit none

      integer np
      integer nd
      real*8  pos(nd,np)
      real*8  vel(nd,np)
      real*8  f(nd,np)
      real*8  a(nd,np)
      real*8  mass
      real*8  dt

      integer i, j
      real*8  rmass

      rmass = 1.0/mass

      ! The time integration is fully parallel
!$omp  parallel do
!$omp& default(shared)
!$omp& private(i,j)
      do i = 1,np
        do j = 1,nd
          pos(j,i) = pos(j,i) + vel(j,i)*dt + 0.5*dt*dt*a(j,i)
          vel(j,i) = vel(j,i) + 0.5*dt*(f(j,i)*rmass + a(j,i))
          a(j,i) = f(j,i)*rmass
        enddo
      enddo
!$omp  end parallel do

      return
      end


!##############################################################################
! MODULE globals
!
! This code is published under the GNU General Public License v3
!                         (https://www.gnu.org/licenses/gpl-3.0.en.html)
!
! Authors: Hans Fehr and Fabian Kindermann
!          contact@ce-fortran.com
!
! #VC# VERSION: 1.0  (23 January 2018)
!
!##############################################################################
module globals

    use toolbox

    implicit none

    ! number of years the household retires
    integer, parameter :: JR = 45

    ! number of years the household lives
    integer, parameter :: JJ = 80

    ! number of white noise (zeta) shocks
    integer, parameter :: NW = 7

    ! number of transitory (epsilon) shocks
    integer, parameter :: NS = 7

    ! number of rate of return (vtheta) shocks
    integer, parameter :: NR = 7

    ! number of eps-vtheta shocks
    integer, parameter :: NSR = NS*NR

    ! number of eta shocks
    integer, parameter :: NE = 1000

    ! number of points on the asset grid
    integer, parameter :: NA = 200

    ! number of points on the cash-on-hand-grid
    integer, parameter :: NX = 200

    ! household preference parameters
    real*8, parameter :: gamma = 0.10d0
    real*8, parameter :: egam = 1d0 - 1d0/gamma
    real*8, parameter :: beta = 0.96d0

    ! risk processes
    real*8, parameter :: sigma_zeta   = 0.0738d0
    real*8, parameter :: sigma_eps    = 0.0106d0
    real*8, parameter :: sigma_vtheta = 0.157d0**2d0
    real*8, parameter :: rho          = 0.00d0

    ! risk free rate and risk premium
    real*8, parameter :: r_f  = 0.02d0
    real*8, parameter :: mu_r = 0.04d0

    ! size of the asset grid
    real*8, parameter :: a_l    = 0.0d0
    real*8, parameter :: a_u    = 500d0
    real*8, parameter :: a_grow = 0.04d0

    ! growth of the cash-on-hand grid
    real*8, parameter :: X_grow = 0.04d0

    ! pension fraction of last income
    real*8, parameter :: kappa = 0.50d0

    ! should cohort averages and variance be calculated analytically
    logical, parameter :: analytical = .true.
    !logical, parameter :: analytical = .false.     ! for quintiles

    ! lower and upper bound for X-grid
    real*8 :: X_l, X_u

    ! lower and upper bound for the eta-grid
    real*8 :: eta_l(JJ), eta_u(JJ)

    ! discretized shocks
    real*8 :: dist_zeta(NW), zeta(NW)
    real*8 :: dist_epsvtheta(NSR), eps(NSR), vtheta(NSR)

    ! wages, transfer payments (old-age) and survival probabilities
    real*8 :: w, eff(JJ), pen(JJ), psi(JJ+1)

    ! cohort aggregate variables
    real*8 :: c_coh(JJ), y_coh(JJ), a_coh(JJ), o_coh(JJ)
    real*8 :: cv_c(JJ), cv_y(JJ), cv_a(JJ), cv_o(JJ)

    ! individual variables
    real*8 :: X(0:NX), a(0:NA)
    real*8 :: c(JJ, 0:NX), a_plus(JJ, 0:NX), V(JJ, 0:NX) = 0d0
    real*8 :: omega_plus(JJ, 0:NA), Q(JJ, 0:NA)
    real*8 :: phi_X(JJ, 0:NX), phi_a(JJ, 0:NA)
    real*8 :: eta(JJ, 0:NE), phi_e(JJ, 0:NE)

    ! numerical variables
    real*8 :: RHS(JJ, 0:NA)
    integer :: ij_com, ix_com, ia_com
    real*8 :: cons_com

contains

    ! the first order condition regarding consumption
    function foc_cons(x_in)

        implicit none
        real*8, intent(in) :: x_in
        real*8 :: foc_cons, a_plus, varphi, tomorrow
        integer :: ial, iar

        ! calculate tomorrows assets
        a_plus  = x_in

        ! calculate consumption
        cons_com = X(ix_com) - a_plus

        ! calculate linear interpolation for future part of first order condition
        call linint_Grow(a_plus, a_l, a_u, a_grow, NA, ial, iar, varphi)

        tomorrow = varphi*RHS(ij_com, ial) + (1d0-varphi)*RHS(ij_com, iar)

        ! calculate first order condition for consumption
        foc_cons = cons_com - tomorrow

    end function

    ! the first order condition regarding portfolio choice
    function foc_port(p)

        implicit none
        real*8, intent(in) :: p
        real*8 :: foc_port, omega_p, R_port, X_p, earnings, c_p, varphi, dist
        integer :: ixl, ixr, iw, isr

        ! store portfolio share
        omega_p  = p

        foc_port = 0d0
        if(ij_com+1 >= JR)then
            do isr = 1, NSR

                ! get return on the portfolio
                R_port = 1d0 + r_f + omega_p*(mu_r + vtheta(isr))

                ! get tomorrow's cash-on-hand (epsilon^+ = 0)
                X_p = R_port*a(ia_com) + pen(ij_com+1)

                ! derive interpolation weights
                call linint_Grow(X_p, X_l, X_u, X_grow, NX, ixl, ixr, varphi)

                ! get distributional weight
                dist = dist_epsvtheta(isr)

                ! calculate consumption and FOC
                c_p = varphi      *c(ij_com+1, ixl) + &
                      (1d0-varphi)*c(ij_com+1, ixr)
                c_p = max(c_p, 1d-10)
                foc_port = foc_port + dist*(mu_r + vtheta(isr))*a(ia_com)*margu(c_p)
            enddo
        else
            do iw = 1, NW
                do isr = 1, NSR

                    ! get return on the portfolio
                    R_port = 1d0 + r_f + omega_p*(mu_r + vtheta(isr))

                    ! derive labor earnings
                    earnings  = w*eff(ij_com+1)*zeta(iw)

                    ! get tomorrow's cash on hand
                    X_p = R_port*a(ia_com)/eps(isr) + earnings

                    ! derive interpolation weights
                    call linint_Grow(X_p, X_l, X_u, X_grow, NX, ixl, ixr, varphi)

                    ! get distributional weight
                    dist = dist_zeta(iw)*dist_epsvtheta(isr)

                    ! calculate consumption and FOC
                    c_p = varphi      *c(ij_com+1, ixl) + &
                          (1d0-varphi)*c(ij_com+1, ixr)
                    c_p = max(c_p, 1d-10)
                    foc_port = foc_port + dist*(mu_r + vtheta(isr))*a(ia_com)*margu(eps(isr)*c_p)
                enddo
            enddo
        endif

    end function

    ! calculates marginal utility of consumption
    function margu(cons)

        implicit none
        real*8, intent(in) :: cons
        real*8 :: margu

        margu = max(cons, 1d-10)**(-1d0/gamma)

    end function

    ! calculates the value function
    function valuefunc(a_plus, cons, ij)

        implicit none
        integer, intent(in) :: ij
        real*8, intent(in) :: a_plus, cons
        real*8 :: valuefunc, varphi, c_help
        integer :: ial, iar

        ! check whether consumption or leisure are too small
        c_help = max(cons, 1d-10)

        ! get tomorrows utility
        call linint_Grow(a_plus, a_l, a_u, a_grow, NA, ial, iar, varphi)

        ! calculate tomorrow's part of the value function
        valuefunc = 0d0
        if(ij < JJ)then
            valuefunc = max(varphi*Q(ij, ial) + (1d0-varphi)*Q(ij, iar), 1d-10)**egam/egam
        endif

        ! add todays part and discount
        valuefunc = c_help**egam/egam + beta*psi(ij+1)*valuefunc

    end function

end module