C++************************************************************************
C STACK1 :
C To combine mutually aligned and normalised images into a
C single output image
C Methods of combining the images: mean, median, mode, minimum, maximum.
C
C Calls EDRS routines (included here): LINSTK, COMBIN, WMODE, NTHMIN
C
C JLP
C Version of 05-04-94
C--************************************************************************
	PROGRAM STACK1
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*Method
*       obtain each input image and extract the required descriptor
*       items. obtain an output image. treat each line in turn...
*       copy one line from each image into workspace using linstk, then
*       combine them into a single output line using combin. when all
*       lines have been treated, update the output descriptor items
*
*
*Starlink parameters
*       method
*               the method of combining the images
*		mean, median, mode, minimum, maximum
*       errors
*               estimates of the image errors..used in weighting the
*               images
*       minimage
*               minimum number of good images required at any point
*       comments
*               comments to replace the comments of 'input1' in the output
*               image
*
* from "stack" (r.f. warren-smith)
*-----------------------------------------------------------------------
* Set maximum number of images which can be stacked
*
	PARAMETER (MAXIM=20)
	INTEGER*4 MADRID(1),IPLST,IPOUT
	CHARACTER CVAL*1,COMMENTS*80,IMGCHR*8,NAMEOUT*40
	CHARACTER NAMEIN(MAXIM)*40
*
* Dimension arrays to hold details of each image
*
	REAL WEIGHT(MAXIM),WORK(MAXIM,2)
	INTEGER INVAL(MAXIM),NPIX(MAXIM),NLINES(MAXIM),IP(MAXIM)
	COMMON /VMR/MADRID

*
* Initiallise size of output image
*
	NPOUT=1
	NLOUT=1

*
* Count through each possible input image
*
	CALL JLP_BEGIN
	CALL JLP_INQUIFMT

	WRITE(6,*) ' Enter the input images  (to stop: "return")'
	WRITE(6,*) ' (Maximum number: 5 or sometimes 6 images 1024x1024 on "ts" ...)'

	DO 12 IM=1,MAXIM

* Obtain the input image
*
	WRITE(6,*) ' Enter next input image (to stop: "return"): '
	NAMEIN(IM)=' '
	READ(5,10) NAMEIN(IM)
10	FORMAT(A)
	IF(NAMEIN(IM)(1:2).NE.'  ')THEN
	  CALL JLP_VM_READIMAG(IP(IM),NPIX(IM),NLINES(IM),
     1	NAMEIN(IM),COMMENTS)
*
* If no image was obtained, and im less than 1 abort,
* otherwise end the input loop.
*
	ELSE
          IF(IM.LE.2) THEN
	    WRITE(6,*) ' Fatal error: only one image!'
            GO TO 99
          ELSE
            GO TO 101
          ENDIF

	ENDIF

*
* If image was obtained successfully, set output image size to the
* largest image so far
*
	  NPOUT=MAX(NPOUT,NPIX(IM))
	  NLOUT=MAX(NLOUT,NLINES(IM))
*
* Store the necessary descriptor information from the image
*
	  INVAL(IM)=-32000
c            CALL GTDSCR(IMGCHR,'inval','integer',INVAL(IM),RVAL,CVAL
c     :       ,IERR)

12    CONTINUE

*
* Set number of images being used and obtain workspace
*
101   IM=IM-1
	WRITE(6,*) IM,' images to stack'
	I=NPOUT*IM*4
	CALL JLP_GETVM(IPLST,I)
*
* Obtain output image frame
*
	I=NPOUT*NLOUT*4
	CALL JLP_GETVM(IPOUT,I)
*
* Output image obtained successfully...obtain required method
* of combining images
*
	 WRITE(6,50)
50	 FORMAT(' Possible methods : 1=MEAN, 2=MEDIAN, 3=MODE,',
     1	' 4=MINIMUM, 5=MAXIMUM',/,
     1	' Enter the required method of combining the images :')
	 READ(5,*) IMETH

*
         WRITE(6,*) ' Output image to store the result :'
         READ(5,10) NAMEOUT

* Obtain minimum number of valid values required at each pixel
*
C	 WRITE(5,58)
C58	 FORMAT(' Minimum number of valid values (i.e. .NE.-32000) required at',
C     1	' each pixel')
C	 READ(5,*) NMIN
C JLP94:
         NMIN=MAX(3,IM-2)

*
* set default weights for images

         DO 61 I=1,IM
            WEIGHT(I)=1.0
61       CONTINUE

*
* if the method requires weights to be assigned to each image,
* (1=mean, 3=mode)
* obtain error estimates for each image

         IF((IMETH.EQ.1).OR.(IMETH.EQ.3)) THEN
64	 WRITE(6,57) IM
57	 FORMAT(' Enter error estimates for each image',
     1	' (Used to assign a weight to each image),(',I3,' values needed)')
	    READ(5,*) (WEIGHT(I),I=1,IM)

*
* Convert errors to weights
*
            DO 62 I=1,IM

               IF(WEIGHT(I).LT.1.0E-09.OR.WEIGHT(I).GT.1.0E09) THEN
*
* If any error estimate was invalid, give message and return
* for new entry
*
	          WRITE(6,*) ' Bad value'
                  GO TO 64
               ELSE
                  WEIGHT(I)=(1.0/WEIGHT(I))**2
               ENDIF

62          CONTINUE

         ENDIF

*
* Obtain descriptor items for output image
*
	 INVALB=-32000

* COUNT THROUGH THE OUTPUT IMAGE LINES

         DO 500 J=1,NLOUT

* COPY THE CORRESPONDING INPUT LINE OF EACH IMAGE INTO THE WORKSPACE
* USING LINSTK

            DO 498 IMAGE=1,IM
               CALL LINSTK(MADRID(IP(IMAGE)),NPIX(IMAGE),NLINES(IMAGE),
     1	INVAL(IMAGE),J,MADRID(IPLST),NPOUT,IMAGE)
498         CONTINUE

*
* CALL COMBIN TO COMBINE THE INPUT LINES INTO A SINGLE OUTPUT LINE
* USING THE METHOD SPECIFIED IN IMETH
*
	  CALL COMBIN(MADRID(IPLST),NPOUT,IM,WEIGHT,IMETH,
     1	INVAL,NMIN,INVALB,J,MADRID(IPOUT),WORK)
500      CONTINUE

	J=MIN(4,IM)
	WRITE(COMMENTS,11)IM,(NAMEIN(I)(1:12),I=1,J)
11	FORMAT(' Stack of',I2,' images: ',4(A,1X),'...')
	CALL JLP_WRITEIMAG(MADRID(IPOUT),NPOUT,NLOUT,NPOUT,
     1	NAMEOUT,COMMENTS)
99	CALL JLP_END

	STOP
	END

C@*********************************************************************
C LINSTK  (Called by STACK)
c@*********************************************************************
      SUBROUTINE LINSTK(IA,NPIXA,NLINEA,INVALA,LINEA,IB,NPIXB,NIMAGE)
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*purpose
*       to copy a specified line from an image into a stack of image
*       lines taken from elsewhere
*
*method
*       copy the specified line to a specified stack location. pad the
*       line with invalid pixels to make up any extra length required.
*       replace it with invalid pixels if the line specified lies
*       outside the input image
*
*arguments (in=input, ou=output)
*       ia (in)             real*4(npixa,nlinea)     the input image
*
*       npixa,nlinea (in)   integer                  dimensions of ia
*
*       invala (in)         integer                  invalid pixel flag for ia
*
*       linea (in)          integer      specifies the line to be copied from ia
*
*       ib (in/out)         real*4(npixb,*)
*				 the stack of image lines, each of length npixb
*       npixb (in)          integer                  the length of lines in ib
*
*       nimage (in)         integer
*                            the location in the stack ib into which the line is
*                            to be copied
*-----------------------------------------------------------------------
*
*
	REAL*4 IA(NPIXA,NLINEA),IB(NPIXB,*)

*
* if the line required lies outside the input image, fill the
* corresponding workspace with invalid pixel values
*

      IF(LINEA.GT.NLINEA) THEN

         DO 1 I=1,NPIXB
            IB(I,NIMAGE)=INVALA
1        CONTINUE

      ELSE
*
* otherwise copy the required line to the workspace
*
         DO 2 I=1,NPIXB
*
* if the workspace has longer lines than the image, pad out with
* invalid pixel values
*
            IF(I.LE.NPIXA) THEN
               IB(I,NIMAGE)=IA(I,LINEA)
            ELSE
               IB(I,NIMAGE)=INVALA
            ENDIF

2        CONTINUE

      ENDIF

      RETURN
      END

C@*********************************************************************
C COMBIN (Called by STACK)
C@*********************************************************************
      SUBROUTINE COMBIN(IA,NPIX,NIMAGE,WEIGHT,METH,INVAL
     : ,NMIN,INVALB,LINEB,IB,WORK)

*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*purpose
*       to combine a number of lines from different images into a
*       single output line
*
*method
*       work along the line, combining the values from each input image
*       using either a weighted mean, the median, the mode, the maximum
*       or the minimum, taking due account of invalid pixels in each image.
*
*arguments
*       ia (in)
*       real*4(npix,nimage)
*               the input stack of image lines
*       npix (in)
*       integer
*               the number of pixels per line
*       nimage (in)
*       integer
*               the number of image lines in the stack ia
*       weight (in)
*       real(nimage)
*               weights to be used with each image if using mean or
*               mode
*       meth (in)
*       integer
*               method to be used in the range 1 to 5
*       inval (in)
*       integer
*               invalid pixel flags for input image lines
*       nmin (in)
*       integer
*               minimum number of images required at each pixel before
*               a valid output value is generated
*       invalb (in)
*       integer
*               invalid pixel flag for output image
*       lineb (in)
*       integer
*               the line in the output image ib into which the result
*               is to be put
*       ib (in/out)
*       real*4(npix,*)
*               the output image into which the resultant line is
*               inserted
*       work (workspace)
*       real(nimage,2)
*               used to store intermediate values in the computations
*
*calls
*               nthmin,wmode
*
*-----------------------------------------------------------------------
*
*
      REAL*4 IA(NPIX,NIMAGE),IB(NPIX,*)
      REAL WEIGHT(NIMAGE),WORK(NIMAGE,2)
      INTEGER INVAL(NIMAGE)

*
* set probability of a pixel being corrupt (used in the routine wmode)
* 0.01 is a safe value for electronographs
*
      PARAMETER (PBAD=0.01)

*
* set accuracy criterion and limit the method to the range 1-5
*
      TOLL=0.5
      METHOD=MIN(MAX(1,METH),5)

*
* count through each pixel in the line
*

      DO 100 I=1,NPIX

*
* initiallise counter for good images at each pixel location
*
         NGOOD=0

*
* if the mean is required:
* ------------------------
*

         IF(METHOD.EQ.1) THEN
            SUM1=0.0
            SUM2=0.0

*
* form weighted mean of valid values
*

            DO 901 J=1,NIMAGE

               IF(IA(I,J).NE.INVAL(J)) THEN
                  NGOOD=NGOOD+1
                  SUM1=SUM1+WEIGHT(J)
                  SUM2=SUM2+WEIGHT(J)*IA(I,J)
               ENDIF

901         CONTINUE

*
* if insufficient images were good, abort and set output invalid
*

            IF(NGOOD.GE.NMIN) THEN
               B=SUM2/SUM1

            ELSE
               GO TO 999

            ENDIF

*
* if the median is required:
* --------------------------
*

         ELSE IF(METHOD.EQ.2) THEN

*
* extract a list of the valid values
*

            DO 902 J=1,NIMAGE

               IF(IA(I,J).NE.INVAL(J)) THEN
                  NGOOD=NGOOD+1
                  WORK(NGOOD,1)=IA(I,J)
               ENDIF

902         CONTINUE

*
* if sufficient points are present, call nthmin to find the median
* value
*

            IF(NGOOD.GE.NMIN) THEN
               NMED=NGOOD/2+1
               CALL NTHMIN(WORK(1,1),NGOOD,NMED,WORK(1,2),IERR)

*
* median lies between two values if the number of values is even
*

               IF(MOD(NGOOD,2).EQ.0) THEN
                  B=(WORK(1,2)+WORK(2,2))*0.5

               ELSE
                  B=WORK(1,2)
               ENDIF

            ELSE
               GO TO 999

            ENDIF

*
* if the mode is required:
* ------------------------
*

         ELSE IF(METHOD.EQ.3) THEN

*
* form a list of the valid values and their weights
*

            DO 903 J=1,NIMAGE

               IF(IA(I,J).NE.INVAL(J)) THEN
                  NGOOD=NGOOD+1
                  WORK(NGOOD,1)=IA(I,J)
                  WORK(NGOOD,2)=WEIGHT(J)
               ENDIF

903         CONTINUE

*
* if sufficient values are valid, call wmode to find the maximum
* likelihood value (output in real b)
*

            IF(NGOOD.GE.NMIN) THEN
               CALL WMODE(WORK(1,1),WORK(1,2),NGOOD,PBAD,3,TOLL,B,
     1	SIGMA)

            ELSE
               GO TO 999

            ENDIF

*
* if the minimum is required:
* ---------------------------
*

         ELSE IF(METHOD.EQ.4) THEN
            B=1.0E20

*
* find the minimum of the valid values
*

            DO 904 J=1,NIMAGE

               IF(IA(I,J).NE.INVAL(J)) THEN
                  NGOOD=NGOOD+1
                  B=MIN(B,IA(I,J))
               ENDIF

904         CONTINUE

            IF(NGOOD.LT.NMIN) GO TO 999

*
* if the maximum is required:
* ---------------------------
*

         ELSE IF(METHOD.EQ.5) THEN
            B=-1.0E20

*
* find the maximum of the valid values
*

            DO 905 J=1,NIMAGE

               IF(IA(I,J).NE.INVAL(J)) THEN
                  NGOOD=NGOOD+1
                  B=MAX(B,IA(I,J))
               ENDIF

905         CONTINUE

            IF(NGOOD.LT.NMIN) GO TO 999
         ENDIF

*
* the output value is now known...
*
	 IB(I,LINEB)=B

         GO TO 100

*
* pixels with insufficient valid input images are here set invalid
* on output:
*
999      IB(I,LINEB)=INVALB
100   CONTINUE

      RETURN

      END
c@*********************************************************************
c wmode
c@*********************************************************************
      SUBROUTINE WMODE(X,W,NX,PBAD,NITER,TOLL,XMODE,SIGMA)

*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*purpose
*       to find the most likely mean value for a set of data points
*       with different normally distributed errors and a constant
*       probability of being corrupt.
*
*method
*       this routine uses the statistical model used by mode to
*       minimise the effect of corrupt data, but includes different
*       weights for each data point, to allow for different intrinsic
*       errors. see mode for a fuller description of the algorithm.
*
*arguments
*       x (in)
*       real(nx)
*               an array of data values
*       w (in)
*       real(nx)
*               an array of weights for each data point in x. the
*               weights are inversely proportional to the square of
*               the relative errors on each data point.
*       nx (in)
*       integer
*               the number of data points
*       pbad (in)
*       real
*               an estimate of the probability that any one data point
*               is corrupt. (this value not very critical)
*       niter (in)
*       integer
*               the maximum number of iterations to be performed
*       toll (in)
*       real
*               the absolute accuracy required in the mean value.
*               iterations cease when two successive iterations agree
*               to within this value.
*       xmode (out)
*       real
*               the estimate of the uncorrupted mean value
*       sigma (out)
*       real
*               an estimate of the uncorrupted normalised standard
*               deviation of the data points. an estimate of the
*               standard deviation of any one data point is:
*                   sigma/sqrt(w) , where w is its weight.
*
*written by
*       r.f. warren-smith
*-----------------------------------------------------------------------
*
*
      REAL X(NX),W(NX)
      DOUBLE PRECISION SUM1,SUM2,SUM3

*
* Form sums for finding the mean and standard deviation of the
* data
*
      SUM1=0.0D0
      SUM2=0.0D0
      SUM3=0.0D0

      DO 1 I=1,NX
         DATA=W(I)
         SUM1=SUM1+DATA
         DATA=DATA*X(I)
         SUM2=SUM2+DATA
         DATA=DATA*X(I)
         SUM3=SUM3+DATA
1     CONTINUE

*
* Calculate the weighted mean and variance
*
      XMODE=SUM2/SUM1
      SIG2=(SUM3-(SUM2*SUM2)/SUM1)/NX
      SIG2=MAX(0.0,SIG2)

*
* Having formed the initial estimates, the main iteration loop
* starts here
*

      DO 3 ITER=1,NITER

*
* Calculate the standard deviation and normalisation constants
*
         SIG2=MAX(1.0E-20,SIG2)
         SIG1=SQRT(SIG2)
         W1=1.0/SIG1
         W2=0.5/SIG2
         PBNORM=0.707107*PBAD

*
* Set limit at which points are negligible at 10 stand. devs.
*
         DEVLIM=100.0*SIG2

*
* Initiallise sums for finding new estimates
*
         SUMA=0.0
         SUMB=0.0
         SUMC=0.0
         SUMD=0.0

*
* Count through data points
*

         DO 2 I=1,NX

*
* Find deviation from mode, normalised to that expected from
* the weights supplied
*
            DX=X(I)-XMODE
            DX2=DX*DX
            DEV2=DX2*W(I)

*
* If point is not negligible, calculate the fractional probability
* that it is good
*

            IF(DEV2.LE.DEVLIM) THEN
               EX=EXP(-W2*DEV2)
               PROB=EX/(EX+PBNORM)
               SUMA=SUMA+PROB
               SUMB=SUMB+PROB*W(I)
               SUMC=SUMC+DX*PROB*W(I)
               SUMD=SUMD+DEV2*PROB
            ENDIF

2        CONTINUE

*
* Calculate the new estimates
*
         SUMA=MAX(SUMA,1.0E-20)
         SUMB=MAX(SUMB,1.0E-20)
         DXMODE=SUMC/SUMB
         XMODE=XMODE+DXMODE
         SIG2=SUMD/SUMA

*
* If required accuracy has been achieved, exit iteration loop
*

         IF(ABS(DXMODE).LE.TOLL) THEN
            GO TO 7

         ENDIF

3     CONTINUE

7     SIGMA=SQRT(SIG2)
      RETURN

      END

c@*********************************************************************
c nthmin
c@*********************************************************************
      SUBROUTINE NTHMIN(X,NX,N,STAK,IERR)

*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*Purpose
*       to find the n'th smallest number in a set of data values
*
*Method
*       maintain a stack of the smallest values. compare each data
*       point with the top of stack.. if smaller, insert it in the
*       stack at an appropriate level to maintain non-increasing
*       stack order. top of stack is lost when data is inserted.
*
*Arguments
*       x (in)
*       real(nx)
*               array of data values
*       nx (in)
*       integer
*               number of data points
*       n (in)
*       integer
*               specifies the n in 'nth smallest data value'
*       stak (out)
*       real(n)
*               stack of n smallest values.. n'th smallest in stak(1)
*       ierr (out)
*       integer
*               error flag: zero for success
*
*Written by
*       r.f. warren-smith
*-----------------------------------------------------------------------
*
*
      REAL X(NX),STAK(N)
      PARAMETER (EXTREM=1.0E20)

*
* Check argument validity
*

      IF(N.GT.NX) THEN
         IERR=1

      ELSE
         IERR=0

*
* Initiallise the stack of smallest values
*

         DO 1 I=1,N
            STAK(I)=EXTREM
1        CONTINUE

*
* Compare each data value with the top of stack
*

         DO 6 J=1,NX

            IF(X(J).LT.STAK(1)) THEN

*
* If less than top of stack, it belongs in the stack.. search down
* the stack, moving contents up.
*

               DO 2 LOCN=2,N

                  IF(X(J).LT.STAK(LOCN)) THEN

                     STAK(LOCN-1)=STAK(LOCN)

                   ELSE
* When correct level is found, insert data in stack
*
                     STAK(LOCN-1)=X(J)
                     GO TO 61

                  ENDIF

2              CONTINUE

*
* Arrive here if data point belongs on bottom of stack
*
               STAK(N)=X(J)
61             CONTINUE
            ENDIF

6        CONTINUE

      ENDIF

      RETURN
      END


!=========================================================================
elemental function gsw_ct_from_entropy (sa, entropy)
!=========================================================================
!
!  Calculates Conservative Temperature with entropy as an input variable.
!
!  SA       =  Absolute Salinity                                   [ g/kg ]
!  entropy  =  specific entropy                                   [ deg C ]
!
!  CT  =  Conservative Temperature (ITS-90)                       [ deg C ]
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_ct_from_pt, gsw_pt_from_entropy

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, entropy

real (r8) :: gsw_ct_from_entropy

real (r8) :: pt

pt = gsw_pt_from_entropy(sa,entropy)
gsw_ct_from_entropy = gsw_ct_from_pt(sa,pt)

return
end function

!--------------------------------------------------------------------------


module  SmoothDeltaDirectSumModule

use NumberKindsModule
use LoggerModule
use ParticlesModule
use EdgesModule, only : MaxEdgeLength
use PolyMesh2dModule
use FieldModule
use MPISetupModule
use SphereGeomModule, only : ChordDistance, SphereDistance, SphereProjection

implicit none

include 'mpif.h'

private

public SphereDelta, New, Delete
public InterpolateScalar

type SphereDelta
	real(kreal) :: eps
end type

interface New
	module procedure newPrivate
end interface

interface Delete
	module procedure deletePrivate
end interface

interface InterpolateScalar
	module procedure interpolateScalarPrivate
end interface

!
!----------------
! Logging
!----------------
!
logical(klog), save :: logInit = .FALSE.
type(Logger) :: log
character(len=28), save :: logKey = 'SphereDelta'
integer(kint), parameter :: logLevel = DEBUG_LOGGING_LEVEL

contains
!----------------
!
! Public functions
!
!----------------
subroutine newPrivate(self, sphereMesh, smoothRadiusMultplier)
	type(SphereDelta), intent(out) :: self
	type(PolyMesh2D), intent(in) :: sphereMesh
	real(kreal), intent(in) :: smoothRadiusMultplier
	real(kreal) :: mult

	if ( .NOT. logInit ) call InitLogger(log, procRank)

	if ( present(smoothRadiusMultplier) ) then
		mult = smoothRadiusMultplier
	else
		mult = 2.0_kreal
	endif

	self%eps = mult * MaxEdgeLength( sphereMesh%edges, sphereMesh%particles)
end subroutine

subroutine deletePrivate(self)
	type(SphereDelta), intent(inout) :: self
end subroutine

function interpolateScalarPrivate(self, sphereMesh, scalarField, interpLoc )
	real(kreal) :: interpolateScalarPrivate
	type(SphereDelta), intent(in) :: self
	type(PolyMesh2D), intent(in) :: sphereMesh
	type(Field), intent(in) :: scalarField
	real(kreal), dimension(3), intent(in) :: interpLoc
	!
	integer(kint) :: j
	real(kreal) :: dotProd

	interpolateScalarPrivate = 0.0_kreal
	do j = 1, sphereMesh%particles%N
		if ( sphereMEsh%particles%isActive(j) then
			dotProd = sphereMesh%particles%x(j) * interpLoc(1) + sphereMesh%particles%y(j) * interpLoc(2) + &
					  sphereMesh%particles%z(j) * interpLoc(3)
			interpolateScalarPrivate = interpolateScalarPrivate + scalarField%scalar(j) * sphereMesh%particles%area(j) * &
				(-(1.0_kreal - dotProd)*(1.0_kreal - dotProd) + 2.0_kreal * self%eps*self%eps* dotProd) / &
				( 4.0_kreal * PI * ( 1.0_kreal - dotProd + self%eps*self%eps) * (1.0_kreal - dotProd + self%eps*self%eps))
		endif
	enddo
end function

subroutine interpolateScalarParallel(self, sphereMesh, scalarField, scalarInterp, xPts, yPts, zPts, interpMPI)
	type(SphereDelta), intent(in) :: self
	type(PolyMesh2d), intent(in) :: sphereMesh
	type(Field), intent(in) :: scalarField
	real(kreal), dimension(:), intent(out) :: scalarInterp
	real(kreal), dimension(:), intent(in) :: xPts, yPts, zPts
	type(MPISetup), intent(in) :: interpMPI
	!
	integer(kint) :: i, j, mpiErrCode
	real(kreal) :: dotProd, avg

	avg = ScalarAverage(scalarField, sphereMesh%particles)

	do i = interpMPI%indexStart(procRank), interpMPI%indexEnd(procRank)
		scalarInterp(i) = 0.0_kreal
		do j = 1, sphereMesh%particles%N
			if ( sphereMesh%particles%isActive(j) ) then
				dotProd = xPts(i) * sphereMesh%particles%x(j) + yPts(i) * sphereMesh%particles%y(j) + &
						  zPts(i) * sphereMesh%particles%z(j)
				scalarInterp(i) = scalarInterp(i) + scalarField%scalar(j) * sphereMesh%particles%area(j) * &
					(-(1.0_kreal - dotProd)*(1.0_kreal - dotProd) + 2.0_kreal * self%eps * self%eps * dotProd ) / &
					( 4.0_kreal * PI * (1.0_kreal - dotProd + self%eps*self%eps)*(1.0_kreal - dotProd + self%eps*self%eps))
			endif
		enddo
		scalarInterp(i) = scalarInterp(i) - avg
	enddo

	do i = 0, numProcs - 1
		call MPI_BCAST(scalarInterp(interpMPI%indexStart(i):interpMPI%indexEnd(i)), interpMPI%messageLength(i), &
				MPI_DOUBLE_PRECISION, i, MPI_COMM_WORLD, mpiErrCode)
	enddo
end subroutine

subroutine interpolateScalarToLatLonParallel(self, sphereMesh, scalarField, scalarInterp, lons, lats, interpMPI )
	type(SphereDelta), intent(in) :: self
	type(PolyMesh2d), intent(in) :: sphereMesh
	type(Field), intent(in) :: scalarField
	real(kreal), dimension(:,:), intent(out) :: scalarInterp
	real(kreal), dimension(:), intent(in) :: lons
	real(kreal), dimension(:), intent(in) :: lats
	type(MPISetup), intent(in) :: interpMPI
	!
	integer(kint) :: i, j, k, mpiErrCode
	real(kreal) :: dotProd, avg

	avg = ScalarAverage(scalarField, sphereMesh%particles)

	do j = interpMPI%indexStart(procRank), interpMPI%indexEnd(procRank)
		do i = 1, size(lats)
			scalarInterp(i,j) = 0.0_kreal
			do k = 1, sphereMesh%particles%N
				if ( sphereMesh%particles%isActive(k)) then
					dotProd = cos(lons(j))*cos(lats(i)) * sphereMesh%particles%x(k) + &
							  sin(lons(j))*cos(lats(i)) * sphereMesh%particles%y(k) + &
							  sin(lats(i)) * sphereMesh%particles%z(k)
					scalarInterp(i,j) = scalarInterp(i,j) + scalarField%scalar(k) * sphereMesh%particles%area(k) * &
					(-(1.0_kreal - dotProd)*(1.0_kreal - dotProd) + 2.0_kreal * self%eps * self%eps * dotProd ) / &
					( 4.0_kreal * PI * (1.0_kreal - dotProd + self%eps*self%eps)*(1.0_kreal - dotProd + self%eps*self%eps))
				endif
			enddo
			scalarInterp(i,j) = scalarInterp(i,j) - avg
		enddo
	enddo

	do i = 0, numProcs - 1
		call MPI_BCAST( scalarInterp(:,interpMPI%indexStart(i):interpMPI%indexEnd(i)), size(lats)*interpMPI%messageLength(i), &
			MPI_DOUBLE_PRECISION, i, MPI_COMM_WORLD, mpiErrCode)
	enddo
end subroutine

!----------------
!
! Private functions
!
!----------------

pure function deltaKernel( x, y, z, xt, yt, zt, eps )
	real(kreal) :: deltaKernel
	real(kreal), intent(in) :: x, y, z
	real(kreal), intent(in) :: xt, yt, zt
	real(kreal), intent(in) :: eps
	!
	real(kreal) :: dotProd

	dotProd = x * xt + y * yt + z * zt

	deltaKernel = ( -(1.0_kreal - dotProd) * (1.0_kreal - dotProd) + 2.0_kreal * eps * dotProd ) / &
				  ( 4.0_kreal * PI * ( 1.0_kreal - dotProd + eps*eps) * (1.0_kreal - dotProd + eps*eps))
end function

subroutine InitLogger(aLog,rank)
! Initialize a logger for this module and processor
	type(Logger), intent(out) :: aLog
	integer(kint), intent(in) :: rank
	write(logKey,'(A,A,I0.2,A)') trim(logKey),'_',rank,' : '
	if ( rank == 0 ) then
		call New(aLog,logLevel)
	else
		call New(aLog,ERROR_LOGGING_LEVEL)
	endif
	logInit = .TRUE.
end subroutine

end module


      subroutine coor(solarGalactocentricDistance)
C     This subroutine calculates the coordinates and proper motions
C     in the Galactic coordinate system.
C     Also it calculates the right ascension and the declination.
C     Galactic coordinates in radians
C     Distances in Kpc
C     Velocities in Km/s
C     Proper motions in  arcsec/yr
C     More info on conversions at: https://physics.stackexchange.com/questions/88663/converting-between-galactic-and-ecliptic-coordinates
      implicit none

      integer, parameter :: MAX_STARS_COUNT = 6000000
C     TODO: find out the meaning of alfag(ALPHA_G)
C     transverse_velocity = KAPPA * proper_motion * distance
C     4.74 km/sec is one au/year
      real, parameter :: KAPPA = 4.74,
     &                   ALPHA_G = 3.35,
     &                   PI = 4.0 * atan(1.0),
     &                   FI = 180.0 / PI,
     &                   PC_PER_KPC = 1e3
C     THETA - see BK angle at the link above
C     DELTA_G - declination of NGP
C     TODO: find out why we use NGP declination for J1950 and not J2000
      double precision, parameter :: DELTA_G = 0.478d0,
     &                               THETA = 2.147d0,
     &                               SIN_DELTA_G = dsin(DELTA_G),
     &                               COS_DELTA_G = dcos(DELTA_G)
      integer :: wd_index,
     &           numberOfWDs
      real :: sin_lgac,
     &        cos_lgac,
     &        zkr,
     &        zkri,
     &        xc,
     &        xs,
     &        velocity_by_prop_motion
      double precision :: solarGalactocentricDistance,
     &                    ros,
     &                    cos_bgac,
     &                    sin_bgac,
     &                    zzx
      double precision :: coordinate_R(MAX_STARS_COUNT),
     &                    coordinate_Theta(MAX_STARS_COUNT),
     &                    coordinate_Zcylindr(MAX_STARS_COUNT),
     &                    lgac(MAX_STARS_COUNT),
     &                    bgac(MAX_STARS_COUNT),
     &                    properMotion(MAX_STARS_COUNT),
     &                    rightAscension(MAX_STARS_COUNT),
     &                    declination(MAX_STARS_COUNT)
      real :: rgac(MAX_STARS_COUNT),
     &        latitude_proper_motion(MAX_STARS_COUNT),
     &        longitude_proper_motion(MAX_STARS_COUNT),
     &        radial_velocity(MAX_STARS_COUNT),
     &        uu(MAX_STARS_COUNT),
     &        vv(MAX_STARS_COUNT),
     &        ww(MAX_STARS_COUNT)
      integer ::  disk_belonging(MAX_STARS_COUNT),
     &            flagOfWD(MAX_STARS_COUNT)

      common /coorcil/ coordinate_R,
     &                 coordinate_Theta,
     &                 coordinate_Zcylindr
      common /vel/ uu, vv, ww
      common /paral/ rgac
      common /mad/ properMotion,
     &             rightAscension,
     &             declination
      common /index/ flagOfWD,
     &               numberOfWDs,
     &               disk_belonging
      common /mopro/ latitude_proper_motion,
     &               longitude_proper_motion,
     &               radial_velocity
      common /lb/ lgac, bgac

      do wd_index = 1, numberOfWDs
C         TODO: rename ros to projection of distance from Sun to WD on
C               galactic plane
          ros = solarGalactocentricDistance
     &          * solarGalactocentricDistance
     &          + coordinate_R(wd_index) * coordinate_R(wd_index)
     &          - 2.d0 * coordinate_R(wd_index)
     &            * solarGalactocentricDistance
     &            * dcos(coordinate_Theta(wd_index))
          rgac(wd_index) = real(
     &        sqrt(ros + coordinate_Zcylindr(wd_index)
     &                   * coordinate_Zcylindr(wd_index)))
C         Galactic coordinate lgac
          ros = dsqrt(ros)

C         TODO: check if precision can be lost here
          if ((solarGalactocentricDistance ** 2 + ros**2
     &         - coordinate_R(wd_index) ** 2)
     &        / (2.d0 * solarGalactocentricDistance * ros) > 1.0) then
              lgac(wd_index) = 0.0
          else
              lgac(wd_index) = dacos((solarGalactocentricDistance ** 2
     &                         + ros**2
     &                         - coordinate_R(wd_index) ** 2)
     &                               / (2.0d0 * ros
     &                                  * solarGalactocentricDistance))
          end if

C         Unfolding from 0-180 to 0-360
          if (coordinate_Theta(wd_index) > PI) then
              lgac(wd_index) = 2.0 * PI - lgac(wd_index)
          end if

C         Galactic coordinate bgac ---
C         TODO: find out the meaning of zzx
          zzx = dabs(coordinate_Zcylindr(wd_index) / ros)
          bgac(wd_index) = datan(zzx)
          if (coordinate_Zcylindr(wd_index) < 0.0) then
              bgac(wd_index)=-bgac(wd_index)
          else
              continue
          end if

C         Calculating the proper motions in galactic coordinates
          sin_lgac = real(sin(lgac(wd_index)))
          cos_lgac = real(cos(lgac(wd_index)))
          sin_bgac = dsin(bgac(wd_index))
          cos_bgac = dcos(bgac(wd_index))

          zkr = KAPPA * rgac(wd_index) * PC_PER_KPC
          zkri = 1.0 / zkr
          velocity_by_prop_motion = 1.0 / (KAPPA * rgac(wd_index)
     &                                     * PC_PER_KPC)

C         TODO: find out why dividing by cos_bgac
          longitude_proper_motion(wd_index) = real(
     &        velocity_by_prop_motion
     &        * (-uu(wd_index) * sin_lgac / cos_bgac
     &           + vv(wd_index) * cos_lgac / cos_bgac))
          latitude_proper_motion(wd_index) = real(
     &        velocity_by_prop_motion
     &        * (-uu(wd_index) * cos_lgac * sin_bgac
     &           -vv(wd_index) * sin_bgac * sin_lgac
     &           + ww(wd_index) * cos_bgac))
          radial_velocity(wd_index) = real(
     &        uu(wd_index) * cos_bgac * cos_lgac
     &        + vv(wd_index) * cos_bgac * sin_lgac
     &        + ww(wd_index) * sin_bgac)
C         FIXME: Looks like this formula is wrong
          properMotion(wd_index) = sqrt(
     &        longitude_proper_motion(wd_index)
     &        * longitude_proper_motion(wd_index)
     &        + latitude_proper_motion(wd_index)
     &          * latitude_proper_motion(wd_index))

C         More info at the link above
          declination(wd_index) = dasin(
     &        SIN_DELTA_G * sin_bgac + COS_DELTA_G * cos_bgac
     &                                 * dcos(THETA - lgac(wd_index)))

C         TODO: give better names for xs and xc
C         These vars are used for conversion from galactic to equatorial
C         coordinates. More info at the link above
          xs = real((cos_bgac * dsin(THETA - lgac(wd_index)))
     &              / dcos(declination(wd_index)))
          xc =  real((COS_DELTA_G * sin_bgac
     &                - SIN_DELTA_G * cos_bgac
     &                  * cos(THETA - lgac(wd_index)))
     &               / cos(declination(wd_index)))

C         Looking at the sign that corresponds to the right ascension
C         TODO: find out what is going on here
          if (xs >= 0.0) then
              if (xc >= 0.0) then
                  rightAscension(wd_index) = asin(xs) + ALPHA_G
              else
                  rightAscension(wd_index) = acos(xc) + ALPHA_G
              end if
          else
              if (xc < 0.0) then
                  rightAscension(wd_index) = PI - asin(xs) + ALPHA_G
              else
                  rightAscension(wd_index) = 2 * PI + asin(xs) + ALPHA_G
              end if
          end if
          if (rightAscension(wd_index) * FI > 360.0) then
              rightAscension(wd_index) = rightAscension(wd_index)
     &                                   - 2 * PI
          end if
      end do
      end subroutine


      double precision function Hqaqasq(i1,i2,i3,i4)
      implicit none
C----Interference piece of Matrix element squared for the process
C----q(p1)+q(p3) -> q(p2)+q(p4)+Higgs
C----with identical quarks
C----summed over incoming/outgoing colors and spins.
C    with a factor of gsq*Asq removed.

c      Cf*((s13-s24)^2*(s12*s34-s13*s24+s14*s23)
c         -2*(-s12*s34+s13*s24+s14*s23)*(s12*s34+s13*s24-s14*s23))
c          /(s12*s14*s23*s34)
c       +Cf*e*(s12*s34-s13*s24+s14*s23)
c        *((s23+s14)*(s12+s34)-(s13+s24)^2)
c         /(s12*s14*s23*s34)
c       -Cf*e^2*(s24+s23+s14+s13)*(s34+s24+s13+s12)*(s12*s34-s13*s24+s14*s23)
c       /(s12*s14*s23*s34);

      include 'constants.f'
      include 'sprods_com.f'
      integer i1,i2,i3,i4
      double precision s12,s13,s14,s23,s24,s34
      s12=s(i1,i2)
      s13=s(i1,i3)
      s14=s(i1,i4)
      s23=s(i2,i3)
      s24=s(i2,i4)
      s34=s(i3,i4)

      Hqaqasq=+Cf*((s13-s24)**2*(s12*s34-s13*s24+s14*s23)
     .    -2d0*(-s12*s34+s13*s24+s14*s23)*(s12*s34+s13*s24-s14*s23))
     .     /(s12*s14*s23*s34)

      return
      end


      subroutine ywar(n,nar,data,dphi,x)
c
c  computes ar prewhitening filter
c
c  inputs:
c    n    - number of data values
c    nar  - length of ar filter exclusive of leading 1
c    data - real vector of data
c    dphi - real work vector of length nar+1
c
c  outputs:
c
c   x     - real vector of filter coefficients
c           to convert to an ar filter, a leading 1 is required and
c           the sign of x must be changed
c
c  calls cross
c
      dimension data(1),dphi(0:nar),x(1)
      call cross(n,data,nar,dphi,0.)
      do 20 i=1,nar
   20   dphi(i)=dphi(i)/dphi(0)
      x(1)=dphi(1)
      y=dphi(1)
      s=(1.-y)*(1.+y)
      do 50 k=1,nar-1
        sum=0.
        do 30 j=1,k
   30     sum=sum+x(j)*dphi(k+1-j)
        y=(dphi(k+1)-sum)/s
        x(k+1)=y
        kk=k/2
        if(kk.eq.0)goto 45
        do 40 j=1,kk
          t=x(j)
          x(j)=x(j)-y*x(k+1-j)
  40      x(k+1-j)=x(k+1-j)-y*t
  45    if(2*kk.ne.k)x(kk+1)=x(kk+1)*(1-y)
  50    s=s*(1.-y)*(1.+y)
      return
      end

c $Id$


MODULE random

!
!  Pay attention when using random numbers in parallel simulation (For now only master process uses them, for initialization)
!

CONTAINS

  SUBROUTINE gasdev_s(harvest)
    !
    ! Returns in harvest a normally distributed deviate with zero mean and unit standard deviation
    ! using intrinsic random_number as the source of uniform deviates
    !
    IMPLICIT none

    DOUBLE PRECISION, intent(out) :: harvest

    DOUBLE PRECISION        :: rsq, v1,v2
    DOUBLE PRECISION, SAVE  :: g
    LOGICAL, SAVE           :: gaus_stored = .false.

    IF (gaus_stored) THEN       ! We have an extra deviate,
       harvest = g              ! so return it
       gaus_stored = .false.    ! and unset the flag.
    ELSE
       DO
          v1 = 2.0*ran1() - 1.0     ! move the values to the box -1 to +1
          v2 = 2.0*ran1() - 1.0
          rsq = v1**2 + v2**2   ! see if they are in the unit circle
          if (rsq > 0.0 .and. rsq < 1.0) exit
       END DO                   ! otherwise, try again
       rsq = sqrt(-2.0*log(rsq)/rsq) ! Now make the Box-Muller transformation to
       harvest = v1*rsq         ! get two normal deviates. Return one and
       g = v2*rsq               ! save the other for next time
       gaus_stored = .true.                    ! Set flag.
    END IF

  END SUBROUTINE gasdev_s

  FUNCTION ran1()
    IMPLICIT none
    DOUBLE PRECISION :: ran1, x
    CALL RANDOM_NUMBER(x)
    ran1 = x
  END FUNCTION ran1

  SUBROUTINE init_random_seed()
    INTEGER :: i,n,clock
    INTEGER, DIMENSION(:), ALLOCATABLE :: seed

    CALL RANDOM_SEED(size = n)
    ALLOCATE(seed(n))

    CALL SYSTEM_CLOCK(COUNT=clock)

    seed = clock + 37*(/ (i-1, i=1, n) /)
    CALL RANDOM_SEED(PUT = seed)

    DEALLOCATE(seed)

  END SUBROUTINE init_random_seed

  SUBROUTINE init_default_seed()
    INTEGER :: n
    INTEGER, DIMENSION(:), ALLOCATABLE :: seed

    CALL RANDOM_SEED(size = n)
    ALLOCATE(seed(n))

    seed = 1245

    CALL RANDOM_SEED(PUT = seed)

    DEALLOCATE(seed)

  END SUBROUTINE init_default_seed

END MODULE random


!*****************************************************************************************
!> author: Jacob Williams
!
!  This module contains various routines related to the
!  Circular Restricted Three-Body Problem (CRTBP).

    module crtbp_module

    use kind_module,    only: wp
    use numbers_module

    implicit none

    private

    public :: compute_crtpb_parameter
    public :: compute_jacobi_constant
    public :: crtbp_derivs
    public :: crtbp_derivs_with_stm
    public :: normalize_variables, unnormalize_variables
    public :: compute_libration_points,compute_libration_points_v2
    public :: crtbp_test

    contains
!*******************************************************************************

!*******************************************************************************
!>
!  Compute \( \mu \), the normalized CRTBP parameter.
!  It is equal to \( M_2 / (M_1 + M_2) \).
!
!@note The inputs can either be mu's or masses,
!      as long are they are both the same units.

    pure function compute_crtpb_parameter(mu1,mu2) result(mu)

    implicit none

    real(wp),intent(in) :: mu1  !! grav param for body 1 \([km^3/s^2]\)
    real(wp),intent(in) :: mu2  !! grav param for body 2 \([km^3/s^2]\)
    real(wp)            :: mu   !! CRTBP parameter \(\mu\)

    mu = mu2 / (mu1 + mu2)

    end function compute_crtpb_parameter
!*******************************************************************************

!*******************************************************************************
!>
!  Convert state in km, km/s units to normalized CRTBP state.

    subroutine normalize_variables(mu1,mu2,d12,x,m,t,x_crtbp,m_crtbp,t_crtbp)

    implicit none

    real(wp),intent(in)                        :: mu1      !! grav. param. of body 1 \( [\mathrm{km}^3/\mathrm{s}^2] \)
    real(wp),intent(in)                        :: mu2      !! grav. param. of body 2 \( [\mathrm{km}^3/\mathrm{s}^2] \)
    real(wp),intent(in)                        :: d12      !! distance between body 1 and body 2 [km]
    real(wp),dimension(6),intent(in),optional  :: x        !! unnormalized state w.r.t. barycenter [km,km/s]
    real(wp),intent(in),optional               :: m        !! unnormalized mass [kg]
    real(wp),intent(in),optional               :: t        !! unnormalized time [sec]
    real(wp),dimension(6),intent(out),optional :: x_crtbp  !! CRTBP normalized state
    real(wp),intent(out),optional              :: m_crtbp  !! CRTBP normalized mass
    real(wp),intent(out),optional              :: t_crtbp  !! CRTBP normalized time

    real(wp) :: tp,n,tu,du,mu

    n = sqrt((mu1+mu2)/d12**3)  !mean motion (rad/sec)
    tu = one/n  !time unit
    du = d12    !distance unit

    if (present(x) .and. present(x_crtbp)) then
        x_crtbp(1:3) = x(1:3) / du       !scale distance
        x_crtbp(4:6) = x(4:6) / (du/tu)  !scale velocity
    end if

    if (present(m) .and. present(m_crtbp)) then
        mu = (mu1 + mu2)/universal_grav_constant    !mass unit
        m_crtbp = m / mu  !scale mass
    end if

    if (present(t) .and. present(t_crtbp)) t_crtbp = t / tu  !scale time

    end subroutine normalize_variables
!*******************************************************************************

!*******************************************************************************
!>
!  Convert normalized CRTBP state to km, km/s units.
!
!# Notes:
!
! See also: http://www.spaceatdia.org/uploads/mariano/ss1/2012SSLecture3.pdf
!
!   m1 = one - mu   ! mass of body 1
!   m2 = mu         ! mass of body 2
!
!   x1 = -mu        ! location of body 1
!   x2 = one - mu   ! location of body 2
!
!   normalized mass     : 1 MU -> (m1 + m2) kg
!   normalized position : 1 DU -> d12 km
!   normalized time     : 1 TU -> 1/n sec
!                         (2pi TU -> 1 rev -> 2pi/n)

    subroutine unnormalize_variables(mu1,mu2,d12,x_crtbp,m_crtbp,t_crtbp,x,m,t)

    implicit none

    real(wp),intent(in)                        :: mu1      !! grav. param. of body 1 \( [\mathrm{km}^3/\mathrm{s}^2] \)
    real(wp),intent(in)                        :: mu2      !! grav. param. of body 2 \( [\mathrm{km}^3/\mathrm{s}^2] \)
    real(wp),intent(in)                        :: d12      !! distance between body 1 and body 2 [km]
    real(wp),dimension(6),intent(in),optional  :: x_crtbp  !! CRTBP normalized state
    real(wp),intent(in),optional               :: m_crtbp  !! CRTBP normalized mass
    real(wp),intent(in),optional               :: t_crtbp  !! CRTBP normalized time
    real(wp),dimension(6),intent(out),optional :: x        !! unnormalized state w.r.t. barycenter [km,km/s]
    real(wp),intent(out),optional              :: m        !! unnormalized mass [kg]
    real(wp),intent(out),optional              :: t        !! unnormalized time [sec]

    real(wp) :: tp,n,tu,du,mu

    n = sqrt((mu1+mu2)/d12**3)  !mean motion (rad/sec)
    tu = one/n  !time unit
    du = d12    !distance unit

    if (present(x) .and. present(x_crtbp)) then
        x(1:3) = x_crtbp(1:3) * du       !unscale distance
        x(4:6) = x_crtbp(4:6) * (du/tu)  !unscale velocity
    end if

    if (present(m) .and. present(m_crtbp)) then
        mu = (mu1 + mu2)/universal_grav_constant    !mass unit
        m = m_crtbp * mu  !unscale mass
    end if

    if (present(t) .and. present(t_crtbp)) t = t_crtbp * tu  !unscale time

    end subroutine unnormalize_variables
!*******************************************************************************

!*******************************************************************************
!>
!  Compute the CRTBP Jacobi constant, given the state.

    pure function compute_jacobi_constant(mu,x) result(c)

    implicit none

    real(wp),intent(in)              :: mu   !! CRTBP parameter (See [[compute_crtpb_parameter]])
    real(wp),dimension(6),intent(in) :: x    !! normalized state vector
    real(wp)                         :: c    !! Jacobi constant

    !local variables:
    real,dimension(3) :: r,v,rb1,rb2
    real(wp) :: omm,r1,r2

    !extract variables from x vector:
    r = x(1:3)    ! position
    v = x(4:6)    ! velocity

    !other parameters:
    omm = one - mu
    rb1 = [-mu,zero,zero]   ! location of body 1
    rb2 = [omm,zero,zero]   ! location of body 2
    r1  = norm2(r - rb1)    ! body1 -> sc distance
    r2  = norm2(r - rb2)    ! body2 -> sc distance

    !compute Jacobi integral:
    ! [ See: http://cosweb1.fau.edu/~jmirelesjames/hw4Notes.pdf ]
    if (r1==zero .or. r2==zero) then
        c = huge(one)   ! a large value
    else
        c = r(1)**2 + r(2)**2 + &
            two*omm/r1 + two*mu/r2 - &
            (v(1)**2 + v(2)**2 + v(3)**2)
    end if

    end function compute_jacobi_constant
!*******************************************************************************

!*******************************************************************************
!>
!  Compute the coordinates of the libration points (L1,L2,L3,L4,L5).
!  L1-L3 are computed using Newton's method. L4-L5 are known analytically.
!
!@note The coordinate are w.r.t. the barycenter of the system.

    subroutine compute_libration_points(mu,r1,r2,r3,r4,r5)

    use math_module, only: cube_root

    implicit none

    real(wp),intent(in)                        :: mu  !! CRTBP parameter
    real(wp),intent(out),optional              :: r1  !! L1 x coordinate
    real(wp),intent(out),optional              :: r2  !! L2 x coordinate
    real(wp),intent(out),optional              :: r3  !! L3 x coordinate
    real(wp),dimension(2),intent(out),optional :: r4  !! L4 [x,y] coordinates
    real(wp),dimension(2),intent(out),optional :: r5  !! L5 [x,y] coordinates

    integer  :: i  !! counter
    real(wp) :: f  !! quintic function value (to be driven to zero)
    real(wp) :: fp !! derivative of quintic function
    real(wp) :: x  !! indep. variable in the quintic functions

    integer,parameter  :: maxiter = 100    !! maximum number of
                                           !! iterations for newton's method
    real(wp),parameter :: tol = 1.0e-12_wp !! convergence tolerance for
                                           !! newton's method

    !L1, L2, and L3 are solved using iterative Newton method:

    if (present(r1)) then
        x = cube_root(mu / (3.0_wp - 3.0_wp * mu)) !initial guess
        do i = 1,maxiter
           f = x**5 - &
               (3.0_wp - mu) * x**4 + &
               (3.0_wp - 2.0_wp * mu) * x**3 - &
               mu * x**2 + &
               2.0_wp * mu * x - &
               mu
           if (abs(f) <= tol) exit
           fp = 5.0_wp * x**4 - &
                4.0_wp * x**3 * (3.0_wp - mu) + &
                3.0_wp * x**2 * (3.0_wp - 2.0_wp * mu) - &
                2.0_wp * x * mu + &
                2.0_wp * mu
           x = x - f / fp
        end do
        r1 = 1.0_wp - x  ! wrt primary body
        r1 = r1 - mu     ! wrt barycenter
    end if

    if (present(r2)) then
        x = cube_root(mu / (3.0_wp - 3.0_wp * mu)) !initial guess
        do i = 1,maxiter
           f = x**5 + &
               (3.0_wp - mu) * x**4 + &
               (3.0_wp - 2.0_wp * mu) * x**3 - &
               mu * x**2 - &
               2.0_wp * mu * x - &
               mu
           if (abs(f) <= tol) exit
           fp = 5.0_wp * x**4 + &
                4.0_wp * x**3 * (3.0_wp - mu) + &
                3.0_wp * x**2 * (3.0_wp - 2.0_wp * mu) - &
                2.0_wp * x * mu - &
                2.0_wp * mu
           x = x - f / fp
        end do
        r2 = 1.0_wp + x  ! wrt primary body
        r2 = r2 - mu     ! wrt barycenter
    end if

    if (present(r3)) then
        x = -(7.0_wp / 12.0_wp) * mu !initial guess
        do i = 1,maxiter
           f = x**5 + &
               (7.0_wp + mu) * x**4 + &
               (19.0_wp + 6.0_wp * mu) * x**3 + &
               (24.0_wp + 13.0_wp * mu) * x**2 + &
               2.0_wp * (6.0_wp + 7.0_wp * mu) * x + &
               7.0_wp * mu
           if (abs(f) <= tol) exit
           fp = 5.0_wp * x**4 + &
                4.0_wp * x**3 * (7.0_wp + mu) + &
                3.0_wp * x**2 * (19.0_wp + 6.0_wp * mu) + &
                2.0_wp * x * (24.0_wp + 13.0_wp * mu) + &
                2.0_wp * (6.0_wp + 7.0_wp * mu)
           x = x - f / fp
        end do
        r3 = -(x + 1.0_wp)  ! wrt primary body
        r3 = r3 - mu        ! wrt barycenter
    end if

    ! L4 and L5 are analytic:
    if (present(r4)) r4 = [0.5_wp - mu,  sqrt(3.0_wp)/2.0_wp]
    if (present(r5)) r5 = [0.5_wp - mu, -sqrt(3.0_wp)/2.0_wp]

    end subroutine compute_libration_points
!*******************************************************************************

!*******************************************************************************
!>
!  Compute the coordinates of the libration points (L1,L2,L3,L4,L5).
!
!  This is just an alternate version of [[compute_libration_points]].
!
!### Reference
!  * J.S. Parker, R.L. Anderson, "Low-Energy Lunar Trajectory Design", 2014.
!   (Appendix A.5)
!
!@note The coordinate are w.r.t. the barycenter of the system.

    subroutine compute_libration_points_v2(mu,r1,r2,r3,r4,r5)

    use math_module, only: cube_root

    implicit none

    real(wp),intent(in)                        :: mu  !! CRTBP parameter
    real(wp),intent(out),optional              :: r1  !! L1 x coordinate
    real(wp),intent(out),optional              :: r2  !! L2 x coordinate
    real(wp),intent(out),optional              :: r3  !! L3 x coordinate
    real(wp),dimension(2),intent(out),optional :: r4  !! L4 [x,y] coordinates
    real(wp),dimension(2),intent(out),optional :: r5  !! L5 [x,y] coordinates

    integer,parameter  :: maxiter = 100    !! maximum number of iterations
    real(wp),parameter :: tol = 1.0e-12_wp !! convergence tolerance

    real(wp) :: gamma, gamma0
    integer :: i !! counter

    if (present(r1)) then
        gamma0 = cube_root(mu * (one-mu) / three)
        gamma = gamma0 + one
        do i = 1,maxiter
            if (abs(gamma-gamma0)<=tol) exit
            gamma0 = gamma
            gamma = cube_root((mu*(gamma0-one)**2)/(three-two*mu-gamma0*(three-mu-gamma0)))
        end do
        r1 = one - mu - gamma
    end if

    if (present(r2)) then
        gamma0 = cube_root(mu * (one-mu) / three)
        gamma = gamma0 + one
        do i = 1,maxiter
            if (abs(gamma-gamma0)<=tol) exit
            gamma0 = gamma
            gamma = cube_root((mu*(gamma0+one)**2)/(three-two*mu+gamma0*(three-mu+gamma0)))
        end do
        r2 = one - mu + gamma
    end if

    if (present(r3)) then
        gamma0 = cube_root(mu * (one-mu) / three)
        gamma = gamma0 + one
        do i = 1,maxiter
            if (abs(gamma-gamma0)<=tol) exit
            gamma0 = gamma
            gamma = cube_root((one-mu)*(gamma0+one)**2/(one+two*mu+gamma0*(two+mu+gamma0)))
        end do
        r3 = - mu - gamma
    end if

    call compute_libration_points(mu,r4=r4,r5=r5)

    end subroutine compute_libration_points_v2
!*******************************************************************************

!*******************************************************************************
!>
!  CRTBP derivatives: state only.

    subroutine crtbp_derivs(mu,x,dx)

    implicit none

    real(wp),intent(in)               :: mu   !! CRTBP parameter (See [[compute_crtpb_parameter]])
    real(wp),dimension(6),intent(in)  :: x    !! normalized state \([\mathbf{r},\mathbf{v}]\)
    real(wp),dimension(6),intent(out) :: dx   !! normalized state derivative \([\dot{\mathbf{r}},\dot{\mathbf{v}}]\)

    !local variables:
    real(wp),dimension(3) :: r1,r2,rb1,rb2,r,v,g
    real(wp) :: r13,r23,omm,c1,c2

    !extract variables from x vector:
    r = x(1:3) ! position
    v = x(4:6) ! velocity

    !other parameters:
    omm = one - mu
    rb1 = [-mu,zero,zero] ! location of body 1
    rb2 = [omm,zero,zero] ! location of body 2
    r1  = r - rb1         ! body1 -> sc vector
    r2  = r - rb2         ! body2 -> sc vector
    r13 = norm2(r1)**3
    r23 = norm2(r2)**3
    c1  = omm/r13
    c2  = mu/r23

    !normalized gravity from both bodies:
    g(1) = -c1*(r(1) + mu) - c2*(r(1)-one+mu)
    g(2) = -c1*r(2)        - c2*r(2)
    g(3) = -c1*r(3)        - c2*r(3)

    ! derivative of x:
    dx(1:3) = v                       ! rdot
    dx(4)   =  two*v(2) + r(1) + g(1) ! vdot
    dx(5)   = -two*v(1) + r(2) + g(2) !
    dx(6)   =                    g(3) !

    end subroutine crtbp_derivs
!*******************************************************************************

!*******************************************************************************
!>
!  CRTBP derivatives: state + state transition matrix.

    subroutine crtbp_derivs_with_stm(mu,x,dx)

    implicit none

    real(wp),intent(in)                :: mu   !! CRTBP parameter (See [[compute_crtpb_parameter]])
    real(wp),dimension(42),intent(in)  :: x    !! normalized state and STM \([\mathbf{r},\mathbf{v},\mathbf{\Phi}]\)
    real(wp),dimension(42),intent(out) :: dx   !! normalized state and STM derivative \([\dot{\mathbf{r}},\dot{\mathbf{v}},\dot{\mathbf{\Phi}}]\)

    !local variables:
    real,dimension(3) :: rb1,rb2,r,v,g
    real(wp),dimension(6,6) :: A, phi, phi_dot
    real(wp) :: r1,r2,r13,r23,r15,r25,omm,tmu,tomm,c1,c2
    real(wp) :: Uxx,Uxy,Uxz,Uyx,Uyy,Uyz,Uzx,Uzy,Uzz

    !extract variables from x vector:
    r = x(1:3) ! position
    v = x(4:6) ! velocity

    !other parameters:
    omm  = one - mu
    rb1  = [-mu,zero,zero] ! location of body 1
    rb2  = [omm,zero,zero] ! location of body 2
    r1   = norm2(r - rb1)  ! body1 -> sc distance
    r2   = norm2(r - rb2)  ! body2 -> sc distance
    r13  = r1**3
    r23  = r2**3
    r15  = r1**5
    r25  = r2**5
    c1   = omm/r13
    c2   = mu/r23
    tmu  = three*mu
    tomm = three*omm

    !normalized gravity from both bodies:
    g(1) = -c1*(r(1) + mu) - c2*(r(1)-one+mu)
    g(2) = -c1*r(2)        - c2*r(2)
    g(3) = -c1*r(3)        - c2*r(3)

    !STM terms:
    Uxx = one - c1 - c2 + tomm*(r(1)+mu)**2/r15 + tmu*(r(1)-one+mu)**2/r25
    Uyy = one - c1 - c2 + tomm*r(2)**2/r15 + tmu*r(2)**2/r25
    Uzz =       c1 - c2 + tomm*r(3)**2/r15 + tmu*r(3)**2/r25
    Uxy = tomm*(r(1)+mu)*r(2)/r15 + tmu*(r(1)-one+mu)*r(2)/r25
    Uxz = tomm*(r(1)+mu)*r(3)/r15 + tmu*(r(1)-one+mu)*r(3)/r25
    Uyz = tomm* r(2)*r(3)/r15 + tmu*r(2)*r(3)/r25
    Uyx = Uxy
    Uzx = Uxz
    Uzy = Uyz

    !columns of A matrix:
    A(:,1) = [zero,zero,zero,Uxx,Uyx,Uzx]
    A(:,2) = [zero,zero,zero,Uxy,Uyy,Uzy]
    A(:,3) = [zero,zero,zero,Uxz,Uyz,Uzz]
    A(:,4) = [one,zero,zero,zero,-two,zero]
    A(:,5) = [zero,one,zero,two,zero,zero]
    A(:,6) = [zero,zero,one,zero,zero,zero]

    !unpack phi into matrix:
    phi = reshape(x(7:42), shape=[6,6])

    !derivative of phi matrix:
    phi_dot = matmul(A,phi)

    !derivative of x vector:
    dx(1:3)  = v                           ! r_dot
    dx(4)    =  two*v(2) + r(1) + g(1)     ! v_dot
    dx(5)    = -two*v(1) + r(2) + g(2)     !
    dx(6)    =                    g(3)     !
    dx(7:42) = pack (phi_dot, mask=.true.) ! phi_dot

    end subroutine crtbp_derivs_with_stm
!*******************************************************************************

!*******************************************************************************
!>
!  Unit tests for CRTBP routines.

    subroutine crtbp_test()

    use celestial_body_module

    implicit none

    real(wp),parameter :: mu_earth = body_earth%mu !! \( \mu_{Earth} ~ (\mathrm{km}^3/\mathrm{s}^2) \)
    real(wp),parameter :: mu_moon  = body_moon%mu  !! \( \mu_{Moon}  ~ (\mathrm{km}^3/\mathrm{s}^2) \)
    real(wp),parameter :: mu_sun   = body_sun%mu   !! \( \mu_{Sun}   ~ (\mathrm{km}^3/\mathrm{s}^2) \)

    !< sample state (normalized)
    !< see: [Celestial Mechanics Notes Set 4: The Circular Restricted
    !< Three Body Problem](http://cosweb1.fau.edu/~jmirelesjames/hw4Notes.pdf), p.40.
    real(wp),dimension(6),parameter :: x = [  0.30910452642073_wp, &
                                              0.07738174525518_wp, &
                                              0.0_wp,              &
                                             -0.72560796964234_wp, &
                                              1.55464233412773_wp, &
                                              0.0_wp               ]

    integer                 :: i       !! counter
    real(wp)                :: mu      !! CRTPB parameter
    real(wp)                :: mu1     !! primary body mu
    real(wp)                :: mu2     !! secondary body mu
    real(wp)                :: c       !! Jacobi constant
    real(wp),dimension(6)   :: xd      !! derivative vector: state
    real(wp),dimension(42)  :: x_phi   !! initial state + phi (identity)
    real(wp),dimension(42)  :: x_phi_d !! derivative vector: state + phi
    real(wp),dimension(6,6) :: eye     !! 6x6 identity matrix
    real(wp)                :: r1      !! L1 x coordinate (normalized)
    real(wp)                :: r2      !! L2 x coordinate (normalized)
    real(wp)                :: r3      !! L3 x coordinate (normalized)
    real(wp),dimension(2)   :: r4      !! L4 x coordinate (normalized)
    real(wp),dimension(2)   :: r5      !! L5 x coordinate (normalized)

    !create an identity matrix for stm initial condition:
    eye = zero
    do i = 1, 6
        eye(i,i) = one
    end do
    x_phi = [x, pack(eye,mask=.true.)]

    write(*,*) ''
    write(*,*) '---------------'
    write(*,*) ' crtbp_test'
    write(*,*) '---------------'
    write(*,*) ''

    do i=1,3

        select case (i)
        case(1)
            mu1 = mu_earth
            mu2 = mu_moon
        case(2)
            mu1 = mu_earth
            mu2 = mu_earth
        case(3)
            mu1 = mu_earth + mu_moon/four
            mu2 = mu_earth
        end select

        write(*,*) ''
        mu = compute_crtpb_parameter(mu1,mu2)
        c = compute_jacobi_constant(mu,x)
        call crtbp_derivs(mu,x,xd)
        call crtbp_derivs_with_stm(mu,x_phi,x_phi_d)
        call compute_libration_points(mu,r1,r2,r3,r4,r5)

        write(*,'(A,1X,*(F30.16,1X))')  'mu:         ', mu
        write(*,'(A,1X,*(F30.16,1X))' ) 'L1 x:       ', r1
        write(*,'(A,1X,*(F30.16,1X))' ) 'L2 x:       ', r2
        write(*,'(A,1X,*(F30.16,1X))' ) 'L3 x:       ', r3
        write(*,'(A,1X,*(F30.16,1X))' ) 'L4 x:       ', r4
        write(*,'(A,1X,*(F30.16,1X))' ) 'L5 x:       ', r5
        write(*,'(A,1X,*(F30.16,1X))' ) 'x:          ', x
        write(*,'(A,1X,*(F30.16,1X))' ) 'c:          ', c
        write(*,'(A,1X,*(F30.16,1X))' ) 'xd:         ', xd
        write(*,'(A,1X,*(F30.16,1X))' ) 'x+phi:      ', x_phi
        write(*,'(A,1X,*(F30.16,1X))' ) 'xd+phi_dot: ', x_phi_d
        write(*,*) ''

        call compute_libration_points_v2(mu,r1,r2,r3,r4,r5)
        write(*,*) ''
        write(*,*) 'alternate formulation:'
        write(*,'(A,1X,*(F30.16,1X))' ) 'L1 x:       ', r1
        write(*,'(A,1X,*(F30.16,1X))' ) 'L2 x:       ', r2
        write(*,'(A,1X,*(F30.16,1X))' ) 'L3 x:       ', r3
        write(*,'(A,1X,*(F30.16,1X))' ) 'L4 x:       ', r4
        write(*,'(A,1X,*(F30.16,1X))' ) 'L5 x:       ', r5
        write(*,*) ''

    end do

    end subroutine crtbp_test
!*******************************************************************************

!*******************************************************************************
    end module crtbp_module
!*******************************************************************************


module rhs
  implicit none

  contains

    integer(c_int) function RhsFn(tn, sunvec_y, sunvec_f, user_data) &
           result(ierr) bind(C,name='RhsFn')

      use, intrinsic :: iso_c_binding
      use fnvector_serial
      use cvode_interface
      implicit none

      real(c_double), value :: tn
      type(c_ptr), value    :: sunvec_y
      type(c_ptr), value    :: sunvec_f
      type(c_ptr), value    :: user_data

      ! pointers to data in SUNDAILS vectors
      real(c_double), pointer :: yvec(:)
      real(c_double), pointer :: fvec(:)

      real(c_double) :: energy

      integer(c_long), parameter :: neq = 1

      ! get data arrays from SUNDIALS vectors
      call N_VGetData_Serial(sunvec_y, neq, yvec)
      call N_VGetData_Serial(sunvec_f, neq, fvec)

      call f_rhs(1, tn, yvec(1), energy, 0.0, 0)

      fvec(1) = energy

      ierr = 0
    end function RhsFn
end module rhs


c     program DRSPFIT
c>> 1996-06-21 DRSPFIT Krogh Special code for C conversion.
c>> 1996-05-28 DRSPFIT Krogh Added external statement.
c>> 1995-09-15 DRSPFIT Krogh Declare all variables.
c>> 1995-09-15 DRSPFIT Krogh Remove '0' in format (again?)
c>> 1994-10-19 DRSPFIT Krogh  Changes to use M77CON
c>> 1994-08-09 DRSPFIT WVS Remove '0' in format
c>> 1991-11-20 DRSPFIT CLL Edited for Fortran 90
c>> 1987-12-09 DRSPFIT  Lawson  Initial Code.
c--S replaces "?": DR?PFIT, ?PFIT, ?CPVAL
c     Demonstration driver for SPFIT.
c++ Code for .C. is inactive
c%%   long int j;
c++ End
      external SCPVAL
      real              X(12),Y(12),P(11),SIGFAC,W(121),SCPVAL
      real              SIG(1), R, YFIT
      integer I, M, NDEG, NP3
c
      data X / 2.E0, 4.E0, 6.E0, 8.E0, 10.E0, 12.E0, 14.E0,
     *         16.E0, 18.E0, 20.E0, 22.E0, 24.E0 /
      data Y / 2.2E0, 4.0E0, 5.0E0, 4.6E0, 2.8E0, 2.7E0,
     *         3.8E0, 5.1E0, 6.1E0, 6.3E0, 5.0E0, 2.0E0 /
      data SIG(1) / -1.E0 /
      data M      / 12 /
c
 1001 format(1X/'  I    X       Y       YFIT  R=Y-YFIT'/1X)
 1002 format(1X,I2,F6.0,2F9.3,F10.3)
c
      call SPFIT(M,X,Y,SIG,8, .TRUE. , .TRUE. , .TRUE. ,P,NDEG,SIGFAC,W)
      NP3 = NDEG+3

c++ Code for ~.C. is active
      print
     *'(/'' NDEG ='',I2,10X,''SIGFAC ='',F8.4//
     *'' P(1),P(2) ='',9X,2F15.8//'' P(3),...,P(NDEG+3) ='',3F15.8/
     *(21X,3F15.8))', NDEG,SIGFAC,(P(I),I=1,NP3)
c++ Code for .C. is inactive
c%%   printf(
c%%      "\n NDEG =%2ld          SIGFAC =%8.4f",ndeg,sigfac );
c%%   printf(
c%%   "\n\n P(1),P(2) =         %15.8f%15.5f\n\n P(3),...,P(NDEG+3) =",
c%%   p[0], p[1]);
c%%   for (i = 2; i < (np3); i+=3){
c%%      for (j = i; j < (i < np3-2 ? i+3 : np3); j++)
c%%           printf("%15.8f", p[j] );
c%%           if (i < np3-2) printf("\n                     ");}
c%%   printf( "\n" );
c++ End
      write(*,1001)
      do 20 I = 1,M
        YFIT = SCPVAL(P,NDEG,X(I))
        R = Y(I) - YFIT
        write(*,1002)I,X(I),Y(I),YFIT,R
   20 continue
      stop
      end


! libRQZ
! daan. camps@cs.kuleuven.be
! Description:
!   Commonly used parameters
! ___________________________________________________________________
module u_parameters
  implicit none

  integer, parameter  ::  dp = SELECTED_REAL_KIND(15)
  real(kind=dp), parameter  ::  mp = epsilon(1.0_dp)
  real(kind=dp)  ::  sfmin

  ! For complex immplementation (z)
  complex(kind=dp), parameter :: czero = (0.0_dp,0.0_dp)
  complex(kind=dp), parameter :: cone = (1.0_dp,0.0_dp)
  integer, parameter          :: ssth = 80

  ! For double implementation (d)
  real(kind=dp), parameter :: dzero = 0.0_dp
  real(kind=dp), parameter :: done = 1.0_dp

end module


c     program DRCWOFZ
c>> 2009-10-27 DRCWOFZ Krogh/Snyder Added equivalence for Nag compiler.
c>> 1996-06-10 DRCWOFZ Krogh Moved formats up, M77CON changes for C.
c>> 1992-03-13 DRCWOFZ WV Snyder Created from DRCWOFZ
c>> 1991-11-20 DRCWOFZ WV Snyder Original Code
c Conversion should only be done from "Z" to "C" for processing to C.
c--C replaces "?": DR?WOFZ, ?WOFZ
c
c     Demonstration driver for CWOFZ.
c
      complex Z, W
      real ZR(2), WR(2)
      equivalence ( Z, ZR ), ( W, WR )
      integer IFLAG
      integer IX, IY
c
c     Evaluate w(z) at 25 points [0..4] x [0..4] in the complex plane.
c
10    format ('  x  y        Re w            Im w')
20    format (2i3, 2g17.8)
      print 10
      do 40 ix = 0, 4
         do 30 iy = 0, 4
            z = cmplx(real(ix), real(iy))
            call cwofz (zr, wr, iflag)
            print 20, ix, iy, w
30       continue
40    continue
      stop
      end


      SUBROUTINE LSEI(C,D,E,F,G,H,LC,MC,LE,ME,LG,MG,N,X,XNRM,W,JW,MODE)

C     FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF
C     EQUALITY & INEQUALITY CONSTRAINED LEAST SQUARES PROBLEM LSEI :

C                MIN ||E*X - F||
C                 X

C                S.T.  C*X  = D,
C                      G*X >= H.

C     USING QR DECOMPOSITION & ORTHOGONAL BASIS OF NULLSPACE OF C
C     CHAPTER 23.6 OF LAWSON & HANSON: SOLVING LEAST SQUARES PROBLEMS.

C     THE FOLLOWING DIMENSIONS OF THE ARRAYS DEFINING THE PROBLEM
C     ARE NECESSARY
C     DIM(E) :   FORMAL (LE,N),    ACTUAL (ME,N)
C     DIM(F) :   FORMAL (LE  ),    ACTUAL (ME  )
C     DIM(C) :   FORMAL (LC,N),    ACTUAL (MC,N)
C     DIM(D) :   FORMAL (LC  ),    ACTUAL (MC  )
C     DIM(G) :   FORMAL (LG,N),    ACTUAL (MG,N)
C     DIM(H) :   FORMAL (LG  ),    ACTUAL (MG  )
C     DIM(X) :   FORMAL (N   ),    ACTUAL (N   )
C     DIM(W) :   2*MC+ME+(ME+MG)*(N-MC)  for LSEI
C              +(N-MC+1)*(MG+2)+2*MG     for LSI
C     DIM(JW):   MAX(MG,L)
C     ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS C, D, E, F, G, AND H.
C     ON RETURN, ALL ARRAYS WILL BE CHANGED BY THE SUBROUTINE.
C     X     STORES THE SOLUTION VECTOR
C     XNORM STORES THE RESIDUUM OF THE SOLUTION IN EUCLIDIAN NORM
C     W     STORES THE VECTOR OF LAGRANGE MULTIPLIERS IN ITS FIRST
C           MC+MG ELEMENTS
C     MODE  IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS:
C          MODE=1: SUCCESSFUL COMPUTATION
C               2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1)
C               3: ITERATION COUNT EXCEEDED BY NNLS
C               4: INEQUALITY CONSTRAINTS INCOMPATIBLE
C               5: MATRIX E IS NOT OF FULL RANK
C               6: MATRIX C IS NOT OF FULL RANK
C               7: RANK DEFECT IN HFTI

C     18.5.1981, DIETER KRAFT, DFVLR OBERPFAFFENHOFEN
C     20.3.1987, DIETER KRAFT, DFVLR OBERPFAFFENHOFEN

      INTEGER          JW(*),I,IE,IF,IG,IW,J,K,KRANK,L,LC,LE,LG,
     .                 MC,MC1,ME,MG,MODE,N
      DOUBLE PRECISION C(LC,N),E(LE,N),G(LG,N),D(LC),F(LE),H(LG),X(N),
     .                 W(*),T,DDOT,XNRM,DNRM2,EPMACH,ZERO
      DATA             EPMACH/2.22D-16/,ZERO/0.0D+00/

      MODE=2
      IF(MC.GT.N)                      GOTO 75
      L=N-MC
      MC1=MC+1
      IW=(L+1)*(MG+2)+2*MG+MC
      IE=IW+MC+1
      IF=IE+ME*L
      IG=IF+ME

C  TRIANGULARIZE C AND APPLY FACTORS TO E AND G

      DO 10 I=1,MC
          J=MIN(I+1,LC)
          CALL H12(1,I,I+1,N,C(I,1),LC,W(IW+I),C(J,1),LC,1,MC-I)
          CALL H12(2,I,I+1,N,C(I,1),LC,W(IW+I),E     ,LE,1,ME)
   10     CALL H12(2,I,I+1,N,C(I,1),LC,W(IW+I),G     ,LG,1,MG)

C  SOLVE C*X=D AND MODIFY F

      MODE=6
      DO 15 I=1,MC
          IF(ABS(C(I,I)).LT.EPMACH)    GOTO 75
          X(I)=(D(I)-DDOT(I-1,C(I,1),LC,X,1))/C(I,I)
   15 CONTINUE
      MODE=1
      W(MC1) = ZERO
      CALL DCOPY (MG-MC,W(MC1),0,W(MC1),1)

      IF(MC.EQ.N)                      GOTO 50

      DO 20 I=1,ME
   20     W(IF-1+I)=F(I)-DDOT(MC,E(I,1),LE,X,1)

C  STORE TRANSFORMED E & G

      DO 25 I=1,ME
   25     CALL DCOPY(L,E(I,MC1),LE,W(IE-1+I),ME)
      DO 30 I=1,MG
   30     CALL DCOPY(L,G(I,MC1),LG,W(IG-1+I),MG)

      IF(MG.GT.0)                      GOTO 40

C  SOLVE LS WITHOUT INEQUALITY CONSTRAINTS

      MODE=7
      K=MAX(LE,N)
      T=SQRT(EPMACH)
      CALL HFTI (W(IE),ME,ME,L,W(IF),K,1,T,KRANK,XNRM,W,W(L+1),JW)
      CALL DCOPY(L,W(IF),1,X(MC1),1)
      IF(KRANK.NE.L)                   GOTO 75
      MODE=1
                                       GOTO 50
C  MODIFY H AND SOLVE INEQUALITY CONSTRAINED LS PROBLEM

   40 DO 45 I=1,MG
   45     H(I)=H(I)-DDOT(MC,G(I,1),LG,X,1)
      CALL LSI
     . (W(IE),W(IF),W(IG),H,ME,ME,MG,MG,L,X(MC1),XNRM,W(MC1),JW,MODE)
      IF(MC.EQ.0)                      GOTO 75
      T=DNRM2(MC,X,1)
      XNRM=SQRT(XNRM*XNRM+T*T)
      IF(MODE.NE.1)                    GOTO 75

C  SOLUTION OF ORIGINAL PROBLEM AND LAGRANGE MULTIPLIERS

   50 DO 55 I=1,ME
   55     F(I)=DDOT(N,E(I,1),LE,X,1)-F(I)
      DO 60 I=1,MC
   60     D(I)=DDOT(ME,E(1,I),1,F,1)-DDOT(MG,G(1,I),1,W(MC1),1)

      DO 65 I=MC,1,-1
   65     CALL H12(2,I,I+1,N,C(I,1),LC,W(IW+I),X,1,1,1)

      DO 70 I=MC,1,-1
          J=MIN(I+1,LC)
          W(I)=(D(I)-DDOT(MC-I,C(J,I),1,W(J),1))/C(I,I)
   70 CONTINUE

C  END OF SUBROUTINE LSEI

   75 END


c
c-----------------------------------------------------------------------
c subroutine: iftohm
c compute idft for real, n-point, odd harmonic sequences using an
c n/2 point fft
c odd harmonic means x(2*k)=0, all k where x(k) is the dft of x(m)
c note: index m is sequence index--not fortran index
c-----------------------------------------------------------------------
c
      subroutine iftohm(x, n)
      dimension x(1)
c
c x = real array which on input contains the n/4 complex values of the
c     odd harmonics of the input--stored in the sequence re(x(1)),
c     im(x(1)),re(x(2)),im(x(2)),...
c     on output x contains the first n/2 points of the input
c ****note: x must be dimensioned to size n/2+2 for fft routine
c n = true size of x sequence
c
c first compute real(x(1)) and real(x(n/2-1)) separately
c also simultaneously multiply original sequence by sin(twopi*(m-1)/n)
c sin and cos are computed recursively
c
c
c for n = 2, assume x(1)=x0, x(2)=-x0, compute idft directly
c
      if (n.gt.2) go to 10
      x(1) = 0.5*x(1)
      x(2) = -x(1)
      return
  10  twopi = 8.*atan(1.0)
      tpn = twopi/float(n)
      no2 = n/2
      no4 = n/4
      nind = no2
c
c solve for x(0)=x0 directly
c
      x0 = 0.
      do 20 i=1,no2,2
        x0 = x0 + 2.*x(i)
  20  continue
      x0 = x0/float(n)
c
c form y(k)=j*(x(2k+1)-x(2k-1))
c overwrite x array with y sequence
c
      xpr = x(1)
      xpi = x(2)
      x(1) = -2.*x(2)
      x(2) = 0.
      if (no4.eq.1) go to 40
      do 30 i=3,nind,2
        ti = x(i) - xpr
        tr = -x(i+1) + xpi
        xpr = x(i)
        xpi = x(i+1)
        x(i) = tr
        x(i+1) = ti
  30  continue
  40  x(no2+1) = 2.*xpi
      x(no2+2) = 0.
c
c take n/2 point (real) ifft of preprocessed sequence x
c
      call fsst(x, no2)
c
c solve for x(m) by dividing by 4*sin(twopi*m/n) for m=1,2,...,n/2-1
c for m=0 substitute precomputed value x0
c
      cosi = 4.
      sini = 0.
      cosd = cos(tpn)
      sind = sin(tpn)
      do 50 i=2,no2
        temp = cosi*cosd - sini*sind
        sini = cosi*sind + sini*cosd
        cosi = temp
        x(i) = x(i)/sini
  50  continue
      x(1) = x0
      return
      end


! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
!
! The ODE Jacobian of Chemical Model File
!
! Generated by KPP-2.2.3 symbolic chemistry Kinetics PreProcessor
!       (http://www.cs.vt.edu/~asandu/Software/KPP)
! KPP is distributed under GPL, the general public licence
!       (http://www.gnu.org/copyleft/gpl.html)
! (C) 1995-1997, V. Damian & A. Sandu, CGRER, Univ. Iowa
! (C) 1997-2005, A. Sandu, Michigan Tech, Virginia Tech
!     With important contributions from:
!        M. Damian, Villanova University, USA
!        R. Sander, Max-Planck Institute for Chemistry, Mainz, Germany
!
! File                 : kpp_achem_gas_Jacobian.f90
! Time                 : Thu Jun 11 11:33:46 2015
! Working directory    : /gpfsm/dnb32/adarmeno/models/geos-5/heracles-UNSTABLE-DEV_MICROPHYSICS/src/GEOSgcs_GridComp/GEOSgcm_GridComp/GEOSagcm_GridComp/GEOSphV
! Equation file        : kpp_achem_gas.kpp
! Output root filename : kpp_achem_gas
!
! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

MODULE kpp_achem_gas_Jacobian

  USE kpp_achem_gas_Parameters
  USE kpp_achem_gas_JacobianSP

  IMPLICIT NONE

CONTAINS

! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
!
! Jac_SP - the Jacobian of Variables in sparse matrix representation
!   Arguments :
!      V         - Concentrations of variable species (local)
!      F         - Concentrations of fixed species (local)
!      RCT       - Rate constants (local)
!      JVS       - sparse Jacobian of variables
!
! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

SUBROUTINE Jac_SP ( V, F, RCT, JVS )

! V - Concentrations of variable species (local)
  REAL(kind=dp) :: V(NVAR)
! F - Concentrations of fixed species (local)
  REAL(kind=dp) :: F(NFIX)
! RCT - Rate constants (local)
  REAL(kind=dp) :: RCT(NREACT)
! JVS - sparse Jacobian of variables
  REAL(kind=dp) :: JVS(LU_NONZERO)

! Local variables
! B - Temporary array
  REAL(kind=dp) :: B(10)

! B(1) = dA(1)/dV(1)
  B(1) = RCT(1)*F(2)
! B(3) = dA(2)/dV(1)
  B(3) = RCT(2)*F(2)
! B(5) = dA(3)/dV(1)
  B(5) = RCT(3)*F(1)
! B(7) = dA(4)/dV(2)
  B(7) = RCT(4)*F(2)
! B(9) = dA(5)/dV(3)
  B(9) = RCT(5)*F(2)

! Construct the Jacobian terms from B's
! JVS(1) = Jac_FULL(1,1)
  JVS(1) = -B(1)-B(3)-B(5)
! JVS(2) = Jac_FULL(2,1)
  JVS(2) = B(1)+0.75*B(3)+B(5)
! JVS(3) = Jac_FULL(2,2)
  JVS(3) = -B(7)
! JVS(4) = Jac_FULL(3,3)
  JVS(4) = -B(9)
! JVS(5) = Jac_FULL(4,1)
  JVS(5) = 0.25*B(3)
! JVS(6) = Jac_FULL(4,4)
  JVS(6) = 0
! JVS(7) = Jac_FULL(5,2)
  JVS(7) = B(7)
! JVS(8) = Jac_FULL(5,5)
  JVS(8) = 0

END SUBROUTINE Jac_SP

! End of Jac_SP function
! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
!
! Jac_SP_Vec - function for sparse multiplication: sparse Jacobian times vector
!   Arguments :
!      JVS       - sparse Jacobian of variables
!      UV        - User vector for variables
!      JUV       - Jacobian times user vector
!
! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

SUBROUTINE Jac_SP_Vec ( JVS, UV, JUV )

! JVS - sparse Jacobian of variables
  REAL(kind=dp) :: JVS(LU_NONZERO)
! UV - User vector for variables
  REAL(kind=dp) :: UV(NVAR)
! JUV - Jacobian times user vector
  REAL(kind=dp) :: JUV(NVAR)

  JUV(1) = JVS(1)*UV(1)
  JUV(2) = JVS(2)*UV(1)+JVS(3)*UV(2)
  JUV(3) = JVS(4)*UV(3)
  JUV(4) = JVS(5)*UV(1)+JVS(6)*UV(4)
  JUV(5) = JVS(7)*UV(2)+JVS(8)*UV(5)

END SUBROUTINE Jac_SP_Vec

! End of Jac_SP_Vec function
! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
!
! JacTR_SP_Vec - sparse multiplication: sparse Jacobian transposed times vector
!   Arguments :
!      JVS       - sparse Jacobian of variables
!      UV        - User vector for variables
!      JTUV      - Jacobian transposed times user vector
!
! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

SUBROUTINE JacTR_SP_Vec ( JVS, UV, JTUV )

! JVS - sparse Jacobian of variables
  REAL(kind=dp) :: JVS(LU_NONZERO)
! UV - User vector for variables
  REAL(kind=dp) :: UV(NVAR)
! JTUV - Jacobian transposed times user vector
  REAL(kind=dp) :: JTUV(NVAR)

  JTUV(1) = JVS(1)*UV(1)+JVS(2)*UV(2)+JVS(5)*UV(4)
  JTUV(2) = JVS(3)*UV(2)+JVS(7)*UV(5)
  JTUV(3) = JVS(4)*UV(3)
  JTUV(4) = JVS(6)*UV(4)
  JTUV(5) = JVS(8)*UV(5)

END SUBROUTINE JacTR_SP_Vec

! End of JacTR_SP_Vec function
! ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

END MODULE kpp_achem_gas_Jacobian


	SUBROUTINE SNSXAX  ( nstn, stns, sltln, ipsdat, nlvls, sloc,
     +			     xmin, xmax, iret )
C************************************************************************
C* SNSXAX								*
C*									*
C* This subroutine defines the x axis for the cross section program.	*
C*									*
C* SNSXAX  ( NSTN, STNS, SLTLN, IPSDAT, NLVLS, SLOC, XMIN, XMAX, IRET )	*
C*									*
C* Input parameters:							*
C*	NSTN		INTEGER		Number of stations		*
C*	STNS (NSTN)	CHAR*		Station ids			*
C*	SLTLN (2,NSTN)	REAL		Station lat/lons		*
C*	IPSDAT (NSTN)	INTEGER		Pointers to station data	*
C*	NLVLS (NSTN)	INTEGER		Number of levels		*
C*									*
C* Output parameters:							*
C*	SLOC (NSTN)	REAL		Station location on x axis	*
C*	XMIN		REAL		Minimum value on x axis		*
C*	XMAX		REAL		Maximum value on x axis		*
C*	IRET		INTEGER		Return code			*
C*					  0 = normal return		*
C**									*
C* Log:									*
C* J. Woytek/GSFC	 9/82	Distance calculation in CSGTDS		*
C* M. desJardins/GSFC	11/85						*
C* M. desJardins/GSFC	 9/90	Remove STNLN; was in wrong place	*
C* M. desJardins/GSFC	 3/91	Added LLTMCX				*
C* S. Jacobs/EAI	10/92	Fixed typo dely->delx in IF block	*
C* S. Jacobs/EAI	 9/93	Changed real exponent to integer;	*
C*				   changed 0.5 exponent to SQRT		*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
C*
	CHARACTER*(*)	stns(*)
	REAL		sltln (2,*), sloc (*)
	INTEGER		ipsdat (*), nlvls (*)
C*
	REAL		alat (LLTMCX), alon (LLTMCX)
	CHARACTER	stnst*8
C------------------------------------------------------------------------
C*	Convert degrees to radians.
C
	DO  i = 1, nstn
	    alat (i) = sltln (1,i) * DTR
	    alon (i) = sltln (2,i) * DTR
	END DO
C
C*	Map all the stations to the line from station 1 to station n.
C
	iret = 0
	rlat1 = alat (1)
	rlatn = alat (nstn)
	rlon1 = alon (1)
	rlonn = alon (nstn)
C*
C*	Get x and y differences of upper and lower stations.
C
	delx = (rlon1 - rlonn) * COS (( rlat1 + rlatn ) / 2.0)
	dely =  rlat1 - rlatn
	IF ( (delx .ne. 0.) .and. (dely .ne. 0.) ) THEN
C*
C*	   Non-zero, non-infinte slope - calculate slope and get the angle.
C
	   slope = (dely / delx)
	   angle = ATAN ( slope )
	   IF (angle .lt. 0.) angle = angle + PI
	   costh = COS (angle)
	   sinth = SIN (angle)
	END IF
C
C*	The distance to the top station is zero.
C
	sloc (1) = 0.
C
C*	Loop through the other stations, calculating the distance.
C
	DO  i = 2, nstn
	  IF ( ( delx .ne. 0. ) .and. ( dely .ne. 0. ) ) THEN
C
C*		First get longitude on line for station latitude.
C*		  Then get difference between longitude of station and
C*		  longitude on the line for the given station latitude.
C
		rlnlin = rlon1 + (alat(i) - rlat1) / ( slope *
     +			 COS (( alat(i) + rlat1 )/2.0) )
		diflon = ( alon(i) - rlnlin ) * COS ( alat(i) )
C
C*		Now get distance between station and intersection of
C*		   perpendicular bisector and the sloped line.
C
		hyp = diflon * sinth
C
C*		Get x-y components of perpendicular bisector(line
C*		  segment connecting stn to line intersection).
C*		  Add to stn lat/lon to get intersection.
C
		ych = hyp * costh
		xch = hyp * sinth
		stlat = alat(i) + ych
		stlon = alon(i) - ( xch / cos( (stlat + alat(i)) /
     +				2.0) )
C*
	      ELSE IF ( dely .eq. 0. ) THEN
C
C*		For end stations with equal latitudes, the
C*		  distance is given by the difference in longitudes.
C
		stlon = alon (i)
		stlat = rlat1
C*
	      ELSE IF ( delx .eq. 0. ) THEN
C
C*		For equal longitudes, the distance is given by the difference
C*		   in latitudes.
C
		stlon = rlon1
		stlat = alat(i)
	   END IF
C
C*	  Calculate the distance from bottom station to intersection of
C*	     perpendicular line.
C
	  sloc (i) = SQRT ( ( stlat - rlat1 ) ** 2 +
     +		( ( stlon - rlon1 ) *
     +			COS ( ( rlat1 + stlat ) / 2.0 )) ** 2 )
 	END DO
C
C*	Sort the distance array from left to right.  All the station
C*	arrays need to be kept in the same order.
C
	DO  i = 1, nstn-1
	    DO  j = i+1, nstn
		IF  ( sloc (j) .lt. sloc (i) ) THEN
		    stnst = stns (i)
		    slatt = sltln (1,i)
		    slont = sltln (2,i)
		    ipst  = ipsdat (i)
		    nlvlst= nlvls (i)
		    sloct = sloc (i)
		    stns (i) = stns (j)
		    sltln (1,i) = sltln (1,j)
		    sltln (2,i) = sltln (2,j)
		    ipsdat (i) = ipsdat (j)
		    nlvls (i)  = nlvls (j)
		    sloc  (i)  = sloc  (j)
		    stns  (j)  = stnst
		    sltln (1,j) = slatt
		    sltln (2,j) = slont
		    ipsdat (j) = ipst
		    nlvls (j) = nlvlst
		    sloc (j) = sloct
		END IF
	    END DO
	END DO
C
C*	Get the maximum and minimum x values from the distance array.
C
	xmin  = sloc (1)
	xmax  = sloc (nstn)
C*
	RETURN
	END


subroutine apply_2ndbc_solid2d(x_solid,nsd,nn_solid,ien_sbc,ne_sbc,nen_solid,&
			ien_solid,ne_solid,solid_bcforce,solid_stress,lambdacf)
!---------------------------------
! Calculate solid surface normal integral
use solid_variables, only: wq_solid, nquad_solid, xq_solid
use r_common, only: h
implicit none

real(8) x_solid(nsd,nn_solid)
integer nsd
integer nn_solid
integer ien_sbc(ne_sbc,nen_solid+2)
integer ne_sbc
integer nen_solid
integer ien_solid(ne_solid,nen_solid)
integer ne_solid
real(8) solid_bcforce(nsd,nn_solid)
real(8) solid_stress(nsd*2,nn_solid)
!---------------------------------
real(8) x(nsd,nen_solid)
real(8) xj(nsd,nsd)
real(8) xji(nsd,nsd)
real(8) det
real(8) rs(nsd)
integer tmp_ien(nen_solid)
real(8) tmpx(nen_solid,nsd)
integer ibs
integer nos
integer ntem
integer ine
integer iq
integer bcnode1
integer bcnode2
real(8) out_norm(nsd)
integer isd
integer jsd
real(8) w
real(8) tot_len
integer snode
real(8) stress_tmp(nsd,nsd)

real(8) lambdacf(nn_solid)

!-------------------------------------

iq=1
tot_len=0.0
solid_bcforce(:,:)=0.0
out_norm(:)=0.0

do ibs=1,ne_sbc

!write(*,*) 'iensbc', ien_sbc(ibs,:)

        ine=ien_sbc(ibs,1)
		do nos=1,nen_solid
	        ntem=ien_solid(ine,nos) !...connectivity
        	x(1:nsd,nos)   = x_solid(1:nsd,ntem)
		end do

if (nsd == 2) then
        if (nen_solid == 3) then ! triangle case
                if (ien_sbc(ibs,2) == 0) then ! node 2,3 on the edge
                        rs(1)=0.0
                        rs(2)=0.5
                        bcnode1=2
                        bcnode2=3
                end if

                if (ien_sbc(ibs,3) == 0) then ! node 1,3 on the edge
                        rs(1)=0.5
                        rs(2)=0.0
                        bcnode1=1
                        bcnode2=3
                end if

                if (ien_sbc(ibs,4) == 0) then ! node 1,2 on the edge
                        rs(1)=0.5
                        rs(2)=0.5
                        bcnode1=1
                        bcnode2=2
                end if
        end if
        if(nen_solid == 4) then ! quad case
                if (ien_sbc(ibs,2)==1 .and. ien_sbc(ibs,3)==1) then ! node 1,2 on the edge
                        rs(1)=0.0
                        rs(2)=-1.0
                        bcnode1=1
                        bcnode2=2
                end if

                if (ien_sbc(ibs,3)==1 .and. ien_sbc(ibs,4)==1) then ! node 2,3 on the edge
                        rs(1)=1.0
                        rs(2)=0.0
                        bcnode1=2
                        bcnode2=3
                end if

                if (ien_sbc(ibs,4)==1 .and. ien_sbc(ibs,5)==1) then ! node 3,4 on the edge
                        rs(1)=0.0
                        rs(2)=1.0
                        bcnode1=3
                        bcnode2=4
                end if

                if (ien_sbc(ibs,5)==1 .and. ien_sbc(ibs,2)==1) then ! node 4,1 on the edge
                        rs(1)=-1.0
                        rs(2)=0.0
                        bcnode1=4
                        bcnode2=1
                end if

        end if

else
write(*,*) '********NOT READY FOR 3-D NOW**************'
end if

        call r_element(rs)
!	call r_jacob(x,xj,xji,det)

        tmp_ien(1:nen_solid)=ien_sbc(ibs,2:nen_solid+1)

        do isd=1,nsd
                do jsd=1,nen_solid
                        tmpx(jsd,isd)=x(isd,jsd)
                end do
        end do

        call outnormal_2d(tmpx,tmp_ien,out_norm,nsd,nen_solid,w)

!write(*,*) 'out_norm', ibs, out_norm(:), w

	tot_len=tot_len+w

if (ien_sbc(ibs,nen_solid+2) == -999) then

	snode=ien_solid(ine,bcnode1)

                stress_tmp(1,1)=solid_stress(1,snode)
                stress_tmp(2,2)=solid_stress(2,snode)
                stress_tmp(1,2)=solid_stress(3,snode)
                stress_tmp(2,1)=solid_stress(3,snode)
continue
	do isd=1,nsd
		do jsd=1,nsd
		solid_bcforce(isd,snode)=solid_bcforce(isd,snode)+&
		stress_tmp(isd,jsd)*out_norm(jsd)*w*h(bcnode1)
		end do
                !------
                solid_bcforce(isd,snode)=solid_bcforce(isd,snode)+&
                lambdacf(snode)*out_norm(isd)*w*h(bcnode1)
                !------
	end do

        snode=ien_solid(ine,bcnode2)

                stress_tmp(1,1)=solid_stress(1,snode)
                stress_tmp(2,2)=solid_stress(2,snode)
                stress_tmp(1,2)=solid_stress(3,snode)
                stress_tmp(2,1)=solid_stress(3,snode)

        do isd=1,nsd
		do jsd = 1,nsd
                solid_bcforce(isd,snode)=solid_bcforce(isd,snode)+&
		stress_tmp(isd,jsd)*out_norm(jsd)*w*h(bcnode2)
		enddo
                !-------
                solid_bcforce(isd,snode)=solid_bcforce(isd,snode)+&
                lambdacf(snode)*out_norm(isd)*w*h(bcnode2)
                !-------
        enddo

end if

end do
!	do ibs=1,nn_solid
!	write(*,*) 'solid norm', ibs, solid_norm(:,ibs)
!	end do

return
end


! material model
module material_model
implicit none
contains

subroutine init_material
use globals
use process
use utilities
use input_output
real :: maxl(14), maxg(14), vmin, vmax, cfl1, cfl2, r, s
integer :: i1(3), i2(3), j1, k1, l1, j2, k2, l2, j, k, l

if (sync) call barrier
if (master) print *, clock(), 'Material model'

! init arrays
call rfill(mr, 0.0, size(mr))
call rfill(lam, 0.0, size(lam))
call rfill(mu, 0.0, size(mu))
call rfill(gam, 0.0, size(gam))

! inputs
call field_io('<', 'rho', mr)
call field_io('<', 'vp',  lam)
call field_io('<', 'vs',  mu)
call field_io('<', 'gam', gam)
call rcopy(lam, s1, size(s1))
call rcopy(mu, s2, size(s2))

! limits
call rlimits(mr, rho_min, rho_max, size(mr))
call rlimits(s1, vp_min, vp_max, size(s1))
call rlimits(s2, vs_min, vs_max, size(s2))

! velocity dependent viscosity
if (vdamp > 0.0) then
    !$omp parallel do schedule(static) private(j, k, l)
    do l = 1, nm(3)
    do k = 1, nm(2)
    do j = 1, nm(1)
        if (s2(j,k,l) == 0.0) then
            gam(j,k,l) = 0.0
        else
            gam(j,k,l) = vdamp / s2(j,k,l)
        end if
    end do
    end do
    end do
    !$omp end parallel do
end if

! limits
call rlimits(gam, gam_min, gam_max, size(gam))

! this should be taken care of inside field_io now
!call scalar_swap_halo(mr,  nhalo)
!call scalar_swap_halo(s1,  nhalo)
!call scalar_swap_halo(s2,  nhalo)
!call scalar_swap_halo(gam, nhalo)

! halos
call set_halo(mr,  0.0, i1cell, i2cell)
call set_halo(s1,  0.0, i1cell, i2cell)
call set_halo(s2,  0.0, i1cell, i2cell)
call set_halo(gam, 0.0, i1cell, i2cell)

! elastic moduli
!$omp parallel do schedule(static) private(j, k, l)
do l = 1, nm(3)
do k = 1, nm(2)
do j = 1, nm(1)
    mu(j,k,l)  = mr(j,k,l) * s2(j,k,l) * s2(j,k,l)
    lam(j,k,l) = mr(j,k,l) * s1(j,k,l) * s1(j,k,l) - 2.0 * mu(j,k,l)
    r = lam(j,k,l) + mu(j,k,l)
    if (r == 0.0) then
        yy(j,k,l) = 0.0
    else
        yy(j,k,l) = 0.5 * lam(j,k,l) / r
    end if
end do
end do
end do
!$omp end parallel do

! non-overlapping cell indices
i1 = max(i1core, i1bc)
i2 = min(i2core, i2bc - 1)
j1 = i1(1); j2 = i2(1)
k1 = i1(2); k2 = i2(2)
l1 = i1(3); l2 = i2(3)

! minima
maxl(1) = -minval(mr(j1:j2,k1:k2,l1:l2))
maxl(2) = -minval(s1(j1:j2,k1:k2,l1:l2))
maxl(3) = -minval(s2(j1:j2,k1:k2,l1:l2))
maxl(4) = -minval(gam(j1:j2,k1:k2,l1:l2))
maxl(5) = -minval(lam(j1:j2,k1:k2,l1:l2))
maxl(6) = -minval(mu(j1:j2,k1:k2,l1:l2))
maxl(7) = -minval(yy(j1:j2,k1:k2,l1:l2))

! maxima
maxl(8)  = maxval(mr(j1:j2,k1:k2,l1:l2))
maxl(9)  = maxval(s1(j1:j2,k1:k2,l1:l2))
maxl(10) = maxval(s2(j1:j2,k1:k2,l1:l2))
maxl(11) = maxval(gam(j1:j2,k1:k2,l1:l2))
maxl(12) = maxval(lam(j1:j2,k1:k2,l1:l2))
maxl(13) = maxval(mu(j1:j2,k1:k2,l1:l2))
maxl(14) = maxval(yy(j1:j2,k1:k2,l1:l2))

! output
call field_io('>', 'rho', mr)
call field_io('>', 'vp',  s1)
call field_io('>', 'vs',  s2)
call field_io('>', 'gam', gam)
call field_io('>', 'lam', lam)
call field_io('>', 'mu',  mu)
call field_io('>', 'nu',  yy)

! hourglass constant
s = sqrt((dx(1) * dx(1) + dx(2) * dx(2) + dx(3) * dx(3)) / 3.0) / 12.0
!$omp parallel do schedule(static) private(j, k, l)
do l = 1, nm(3)
do k = 1, nm(2)
do j = 1, nm(1)
    r = lam(j,k,l) + 2.0 * mu(j,k,l)
    if (r == 0.0) then
        yy(j,k,l) = 0.0
    else
        yy(j,k,l) = mu(j,k,l) * (lam(j,k,l) + mu(j,k,l)) * s / r
    end if
end do
end do
end do
!$omp end parallel do

! global maxima
call rreduce1(maxg, maxl, 'allmax')

! vs harmonic mean for pml
if (vpml <= 0.0) then
    vmin = -maxg(3)
    vmax =  maxg(9)
    vpml = 2.0 * vmin * vmax / (vmin + vmax)
end if

! Courant condition
vmax = maxg(9)
r = dx(1) * dx(1) + dx(2) * dx(2) + dx(3) * dx(3)
cfl1 = dt * vmax * sqrt(3.0 / r)
cfl2 = dt * vmax * 3.0 / sqrt(r)

! output statistics
if (master) then
    open (1, file='material.json', status='replace')
    write (1,'(a)') '{'
    write (1,*) '   "cfl": [', cfl1,     ', ', cfl2,     '],'
    write (1,*) '   "rho": [', -maxg(1), ', ', maxg(8),  '],'
    write (1,*) '   "vp":  [', -maxg(2), ', ', maxg(9),  '],'
    write (1,*) '   "vs":  [', -maxg(3), ', ', maxg(10), '],'
    write (1,*) '   "gam": [', -maxg(4), ', ', maxg(11), '],'
    write (1,*) '   "lam": [', -maxg(5), ', ', maxg(12), '],'
    write (1,*) '   "mu":  [', -maxg(6), ', ', maxg(13), '],'
    write (1,*) '   "nu":  [', -maxg(7), ', ', maxg(14), ']'
    write (1,'(a)') '}'
    close (1)
    if (any(maxg(1:7) > 0.0)) stop 'negative moduli'
    if (cfl2 > 1.0) stop 'Courant condition not satisfied!'
end if

end subroutine

end module


subroutine HeatCapacity(dHeatCapacity)

    USE ModuleThermoIO
    USE ModuleThermo

    implicit none

    integer               :: k, nMaxHeatCapAttempt
    real(8), dimension(2) :: dGibbsEnergies
    real(8), intent(out)  :: dHeatCapacity
    real(8)               :: dTStepSize, dSecondDer
    real(8)               :: dtemp0, dGibbs0, dGibbsDiff, dTargetDiff, dIncrease, dDecrease

    if (INFOThermo /= 0) then
        dHeatCapacity = 0D0
        return
    end if

    dtemp0         = dTemperature
    dTemperatureForLimits = dtemp0
    dGibbs0        = dGibbsEnergySys
    dTStepSize     = 2.5e-1
    dGibbsEnergies = 0D0
    dTargetDiff    = 2.5e-4
    dGibbsDiff     = dTargetDiff
    dIncrease      = 1.5D0
    dDecrease      = 0.6D0
    nMaxHeatCapAttempt = 5

    lReinitRequested = .FALSE.
    if(lReinitRequested) call SaveReinitData

    lHeatCapacityCurrent = .TRUE.

    HeatCapTrial: do k = 1, nMaxHeatCapAttempt
        if (dGibbsDiff > 2D0*dTargetDiff) then
            dTStepSize = dTStepSize * dDecrease
        else if (dGibbsDiff < 1D-1*dTargetDiff) then
            dTStepSize = dTStepSize * dIncrease
        else if (k > 1) then
            exit HeatCapTrial
        end if
        ! Lower T
        dTemperature = dtemp0 - dTStepSize
        call ResetThermo
        call Thermochimica
        if (INFOThermo /= 0) then
            INFOThermo = 0
            dTStepSize = dTStepSize * dDecrease
            cycle HeatCapTrial
        end if
        dGibbsEnergies(1) = dGibbsEnergySys
        ! Higher T
        dTemperature = dtemp0 + dTStepSize
        call ResetThermo
        call Thermochimica
        if (INFOThermo /= 0) then
            INFOThermo = 0
            dTStepSize = dTStepSize * dDecrease
            cycle HeatCapTrial
        end if
        dGibbsEnergies(2)     = dGibbsEnergySys
        dGibbsDiff = ABS(dGibbsEnergies(2) - dGibbsEnergies(1)) / ABS(dGibbs0)
    end do HeatCapTrial

    ! Second derivative from  3-point stencil
    dSecondDer = (dGibbsEnergies(1)+dGibbsEnergies(2)-2D0*dGibbs0)/dTStepSize**2
    ! Second derivative from  5-point stencil
    ! dSecondDer = (-dGibbsEnergies(1)-dGibbsEnergies(5)-30D0*dGibbsEnergies(3)+16*dGibbsEnergies(2)+16*dGibbsEnergies(4)) &
    !               / (12*dTStepSize**2)

    ! Heat capacity from 2nd derivative of G
    dHeatCapacity = -dtemp0*dSecondDer

    lHeatCapacityCurrent = .FALSE.

    call ResetThermo
    dTemperature = dtemp0
    call Thermochimica

end subroutine HeatCapacity


! For Example:
! The change in number density of C12 is
! d(n12)/dt = - 2 * 1/2 (n12)**2 <sigma v>
!
! where <sigma v> is the average of the relative velocity times the cross
! section for the reaction, and the factor accounting for the total number
! of particle pairs has a 1/2 because we are considering a reaction involving
! identical particles (see Clayton p. 293).  Finally, the -2 means that for
! each reaction, we lose 2 carbon nuclei.
!
! The corresponding Mg24 change is
! d(n24)/dt = + 1/2 (n12)**2 <sigma v>
!
! note that no factor of 2 appears here, because we create only 1 Mg nuclei.
!
! Switching over to mass fractions, using n = rho X N_A/A, where N_A is
! Avagadro's number, and A is the mass number of the nucleon, we get
!
! d(X12)/dt = -2 *1/2 (X12)**2 rho N_A <sigma v> / A12
!
! d(X24)/dt = + 1/2 (X12)**2 rho N_A <sigma v> (A24/A12**2)
!
! these are equal and opposite.
!
! The quantity [N_A <sigma v>] is what is tabulated in Caughlin and Fowler.

! we will always refer to the species by integer indices that come from
! the network module -- this makes things robust to a shuffling of the
! species ordering

subroutine rhs(n, t, y, ydot, rpar, ipar)

  use bl_types
  use bl_constants_module
  use network
  use network_indices
  use rpar_indices

  implicit none
  !$acc routine seq
  !$acc routine(screenz) seq

  ! our convention is that y(1:nspec) are the species (in the same
  ! order as defined in network.f90, and y(nspec+1) is the temperature
  integer,         intent(in   ) :: n, ipar
  real(kind=dp_t), intent(in   ) :: y(n), t
  real(kind=dp_t), intent(  out) :: ydot(n)
  real(kind=dp_t), intent(inout) :: rpar(:)

  integer :: k
!  real(kind=dp_t) :: ymass(nspec)

  !real(kind=dp_t) :: dens
  !real(kind=dp_t) :: temp, T9, T9a, dT9dt, dT9adt

  real(kind=dp_t) :: capture_rate, decay_rate

  real(kind=dp_t), PARAMETER :: &
                     one_twelvth = 1.0d0/12.0d0, &
                     five_sixths = 5.0d0/ 6.0d0, &
                       one_third = 1.0d0/ 3.0d0, &
                      two_thirds = 2.0d0/ 3.0d0

  !dens = rpar(irp_dens)
  !temp = rpar(irp_temp)

  ! compute the molar fractions -- needed for the screening
  !ymass(ic12_) = y(1)/aion(ic12_)
  !ymass(io16_) = X_O16/aion(io16_)
  !ymass(img24_) = (ONE - y(1) - X_O16)/aion(img24_)

  ! compute some often used temperature constants
  !T9     = temp/1.e9_dp_t
  !dT9dt  = ONE/1.e9_dp_t
  !T9a    = T9/(1.0e0_dp_t + 0.0396e0_dp_t*T9)
  !dT9adt = (T9a / T9 - (T9a / (1.0e0_dp_t + 0.0396e0_dp_t*T9)) * 0.0396e0_dp_t) * dT9dt

  capture_rate = 0.1_dp_t
  decay_rate   = 0.2_dp_t

  ydot(ine23_) = -decay_rate*y(ine23_) + capture_rate*y(ina23_)
  ydot(ina23_) = -capture_rate*y(ina23_) + decay_rate*y(ine23_)

  return

end subroutine rhs


module mod_poisson_interp
  implicit none

contains

  subroutine interpolate_mc_to_vc( f_mc, f_vc, BCTYPE )
    implicit none

    real, dimension( :, : ), intent(in) :: f_mc
    double precision, dimension( :, : ), intent(inout) :: f_vc
    character ( 4 ) ,intent ( in ) :: BCTYPE
    integer :: nlon, nlat, i, j

    nlon = size( f_mc, 1)
    nlat = size( f_mc, 2)

    do i = 2, nlon
       do j = 2, nlat
          f_vc ( i, j ) = ( f_mc ( i-1, j-1 ) + &
               f_mc ( i-1, j ) + &
               f_mc ( i, j-1 ) + &
               f_mc ( i, j ) ) / 4
       enddo
    enddo

    do j = 2, nlat
       f_vc ( 1, j ) = ( f_mc ( 1, j-1 ) + &
            f_mc ( 1, j ) + &
            f_mc ( nlon, j-1 ) + &
            f_mc ( nlon, j ) ) / 4
       f_vc ( nlon + 1, j ) = f_vc ( 1, j )
    enddo

    select case ( BCTYPE )

    case ( "PPPP" ) ! Periodic y-dimension

       do i = 2, nlon
          f_vc ( i, 1 ) = ( f_mc ( i, 1) + &
               f_mc ( i-1, 1 ) + &
               f_mc ( i, nlat ) + &
               f_mc ( i-1, nlat ) ) / 4
          f_vc ( i, nlat + 1 ) = f_vc ( i, 1)
       enddo

       f_vc ( 1, 1 ) = ( f_mc ( 1, 1 ) + &
            f_mc ( nlon, 1 ) + &
            f_mc ( 1, nlat ) + &
            f_mc ( nlon, nlat ) ) / 4
       f_vc ( nlon +1 , 1 ) = f_vc ( 1, 1 )
       f_vc ( 1, nlat + 1 ) = f_vc ( 1, 1 )
       f_vc ( nlon + 1, nlat + 1 ) = f_vc ( 1, 1 )

    case ( "PPDD", "PPNN" ) ! Diriclet y-dimension

       do i = 2, nlon
          f_vc ( i, 1 ) = ( f_mc ( i, 1 ) + &
               f_mc ( i-1, 1 ) ) / 2
          f_vc ( i, nlat + 1 ) = ( f_mc ( i, nlat ) + &
               f_mc ( i-1, nlat ) ) / 2
       enddo

       f_vc ( 1, 1 ) =  ( f_mc ( 1, 1 ) + &
            f_mc ( nlon, 1 ) ) / 2
       f_vc ( nlon + 1, 1 ) = f_vc ( 1, 1 )
       f_vc ( 1, nlat + 1 ) =  ( f_mc ( 1, nlat ) + &
            f_mc ( nlon, nlat ) ) / 2
       f_vc ( nlon + 1, nlat + 1 ) = f_vc ( 1, nlat + 1 )

    end select

  end subroutine interpolate_mc_to_vc

  subroutine interpolate_vc_to_mc( f_vc, f_mc )
    implicit none

    double precision, dimension ( :, : ), intent ( in ) :: f_vc
    real, dimension ( :, : ), intent ( out ) :: f_mc

    integer :: nlon, nlat, i, j

    nlon = size( f_mc, 1)
    nlat = size( f_mc, 2)

    do i = 1, nlon
       do j = 1, nlat
          f_mc ( i, j ) = ( f_vc ( i, j ) + &
               f_vc ( i + 1, j ) + &
               f_vc ( i, j + 1 ) + &
               f_vc ( i + 1, j + 1 ) ) / 4
       enddo
    enddo

  end subroutine interpolate_vc_to_mc

end module mod_poisson_interp