C
C	$Id$
C
      DOUBLE PRECISION FUNCTION CSSIG1 (N1,N2,N,X,Y,Z,H,LIST,
     .                                LPTR,LEND,GRAD,IFLGB,
     .                                HPBND,TOL,
     .                                IFLGS, SIGMA, IER)
      INTEGER N1, N2, N, LIST(*), LPTR(*), LEND(N), IFLGB,
     .        IFLGS, IER
      DOUBLE PRECISION X(N), Y(N), Z(N), H(N), GRAD(3,N),
     .                 HPBND, TOL, SIGMA(*)
C
C***********************************************************
C
C                                              From SSRFPACK
C                                            Robert J. Renka
C                                  Dept. of Computer Science
C                                       Univ. of North Texas
C                                           renka@cs.unt.edu
C                                                   11/21/96
C
C   Given a triangulation of a set of nodes on the unit
C sphere, along with data values H and gradients GRAD at the
C nodes, this function determines the smallest tension fac-
C tor CSSIG1 such that the first derivative HP(A) of the
C Hermite interpolatory tension spline H(A), defined by CSSIG1
C and the endpoint values and directional derivatives asso-
C ciated with an arc N1-N2, is bounded (either above or
C below) by HPBND for all A in (A1,A2), where (A1,A2) de-
C notes an interval corresponding to the arc and A denotes
C arc-length.
C
C On input:
C
C       N1,N2 = Nodal indexes of the endpoints of an arc for
C               which the tension factor is to be computed.
C               The indexes must be distinct and lie in the
C               range 1 to N, and if IFLGS .GE. 1, they must
C               correspond to adjacent nodes in the triangu-
C               lation.
C
C       N = Number of nodes in the triangulation.  N .GE. 3.
C
C       X,Y,Z = Arrays of length N containing coordinates of
C               the nodes.  X(I)**2 + Y(I)**2 + Z(I)**2 = 1.
C
C       H = Array of length N containing data values at the
C           nodes.  H(I) is associated with (X(I),Y(I),Z(I))
C           for I = 1 to N.
C
C       LIST,LPTR,LEND = Data structure defining the trian-
C                        gulation.  Refer to STRIPACK
C                        Subroutine CSTRMESH.
C
C       GRAD = Array dimensioned 3 by N whose columns con-
C              gradients at the nodes.  GRAD( ,J) must be
C              orthogonal to node J:  GRAD(1,J)*X(J) +
C              GRAD(2,J)*Y(J) + GRAD(3,J)*Z(J) = 0.  Refer
C              to Subroutines CSGRADG, CSGRADL, and CSSMSURF.
C
C       IFLGB = Bound option indicator:
C               IFLGB = -1 if HPBND is a lower bound on HP.
C               IFLGB = 1 if HPBND is an upper bound on HP.
C
C       HPBND = Bound on HP.  HPBND .LE. min(HP1,HP2,S) if
C               IFLGB = -1 and HPBND .GE. max(HP1,HP2,S) if
C               IFLGB = 1, where HP1 and HP2 are the direc-
C               tional derivatives at the endpoints of the
C               arc N1-N2, and S is the slope of the linear
C               interpolant of the endpoint data values.
C
C       TOL = Tolerance whose magnitude determines how close
C             CSSIG1 is to its optimal value when nonzero
C             finite tension is necessary and sufficient to
C             satisfy the constraint.  For a lower bound,
C             CSSIG1 is chosen so that HPBND .LE. HPMIN .LE.
C             HPBND + abs(TOL), where HPMIN is the minimum
C             value of HP on the arc.  For an upper bound,
C             the maximum of HP satisfies HPBND - abs(TOL)
C             .LE. HPMAX .LE. HPBND.  Thus, the constraint
C             is satisfied but possibly with more tension
C             than necessary.
C
C       IFLGS = Tension array option indicator:
C               IFLGS .LE. 0 if SIGMA is not to be used.
C               IFLGS .GE. 1 if SIGMA is to be updated by
C                            storing CSSIG1 in the appropriate
C                            locations.
C
C The above parameters are not altered by this function.
C
C       SIGMA = Dummy parameter (IFLGS .LE. 0) or array
C               containing tension factors associated with
C               arcs in one-to-one correspondence with LIST
C               entries (IFLGS .GE. 1).  Refer to Subroutine
C               CSGETSIG.
C
C On output:
C
C       SIGMA = Tension factor array updated with the new
C               value if and only if IFLGS .GE. 1 and IER
C               .GE. 0.
C
C       IER = Error indicator:
C             IER = 0 if no errors were encountered and the
C                     constraint can be satisfied with fin-
C                     ite tension.
C             IER = 1 if no errors were encountered but in-
C                     finite tension is required to satisfy
C                     the constraint (e.g., IFLGB = -1,
C                     HPBND = S, and HP1 > S).
C             IER = -1 if N1, N2, N, or IFLGB is outside its
C                      valid range.
C             IER = -2 if nodes N1 and N2 coincide or IFLGS
C                      .GE. 1 and the nodes are not adja-
C                      cent.
C             IER = -3 if HPBND is outside its valid range.
C
C       CSSIG1 = Minimum tension factor defined above unless
C              IER < 0, in which case CSSIG1 = -1.  If IER
C              = 1, CSSIG1 is set to 85, resulting in an
C              approximation to the linear interpolant of
C              the endpoint values.
C
C STRIPACK module required by CSSIG1:  CSSTORE
C
C SSRFPACK modules required by CSSIG1:  CSARCLEN, CSSNHCSH
C
C Intrinsic functions called by CSSIG1:   ABS, EXP, MAX, MIN,
C                                         DBLE, SIGN, SQRT
C
C***********************************************************
C
      DOUBLE PRECISION CSARCLEN, CSSTORE
      INTEGER LP1, LP2, LPL, LUN, NIT
      DOUBLE PRECISION A, A0, AL, B0, BND, C0, C1, C2,
     .                 COSHM, COSHMM, D0, D1, D1PD2, D2,
     .                 DMAX, DSIG, E, EMS, EMS2, F, F0,
     .                 FMAX, FNEG, FTOL, P1(3), P2(3), RF,
     .                 RTOL, S, S1, S2, SBIG, SIG, SINH,
     .                 SINHM, STOL, T0, T1, T2, TM, UN(3),
     .                 UNORM
C
      DATA SBIG/85./,  LUN/-1/
      RF = DBLE(IFLGB)
      BND = HPBND
C
C Print a heading.
C
      IF (LUN .GE. 0  .AND.  RF .LT. 0.) WRITE (LUN,100) N1,
     .                                   N2, BND
      IF (LUN .GE. 0  .AND.  RF .GT. 0.) WRITE (LUN,110) N1,
     .                                   N2, BND
  100 FORMAT (//1X,'CSSIG1 -- N1 =',I4,', N2 =',I4,
     .        ', LOWER BOUND = ',E15.8)
  110 FORMAT (//1X,'CSSIG1 -- N1 =',I4,', N2 =',I4,
     .        ', UPPER BOUND = ',E15.8)
C
C Test for errors and store local parameters.
C
      IER = -1
      IF (MIN(N1,N2) .LT. 1  .OR.  N1 .EQ. N2  .OR.
     .    MAX(N1,N2,3) .GT. N  .OR.  ABS(RF) .NE. 1.)
     .   GO TO 11
      IER = -2
      IF (IFLGS .GT. 0) THEN
C
C   Set LP1 and LP2 to the pointers to N2 as a neighbor of
C     N1 and N1 as a neighbor of N2, respectively.
C
        LPL = LEND(N1)
        LP1 = LPTR(LPL)
    1   IF (LIST(LP1) .EQ. N2) GO TO 2
          LP1 = LPTR(LP1)
          IF (LP1 .NE. LPL) GO TO 1
        IF (ABS(LIST(LP1)) .NE. N2) GO TO 11
C
    2   LPL = LEND(N2)
        LP2 = LPTR(LPL)
    3   IF (LIST(LP2) .EQ. N1) GO TO 4
          LP2 = LPTR(LP2)
          IF (LP2 .NE. LPL) GO TO 3
        IF (ABS(LIST(LP2)) .NE. N1) GO TO 11
      ENDIF
C
C Store nodal coordinates P1 and P2, compute arc-length AL
C   and unit normal UN = (P1 X P2)/UNORM, and test for
C   coincident nodes.
C
    4 P1(1) = X(N1)
      P1(2) = Y(N1)
      P1(3) = Z(N1)
      P2(1) = X(N2)
      P2(2) = Y(N2)
      P2(3) = Z(N2)
      AL = CSARCLEN(P1,P2)
      UN(1) = P1(2)*P2(3) - P1(3)*P2(2)
      UN(2) = P1(3)*P2(1) - P1(1)*P2(3)
      UN(3) = P1(1)*P2(2) - P1(2)*P2(1)
      UNORM = SQRT(UN(1)*UN(1) + UN(2)*UN(2) + UN(3)*UN(3))
      IF (UNORM .EQ. 0.  .OR.  AL .EQ. 0.) GO TO 11
C
C Compute first difference S and scaled directional deriva-
C   tives S1,S2 at the endpoints (for the direction N1->N2).
C
      S = H(N2) - H(N1)
      S1 = AL*(GRAD(1,N1)*P2(1) + GRAD(2,N1)*P2(2) +
     .         GRAD(3,N1)*P2(3))/UNORM
      S2 = -AL*(GRAD(1,N2)*P1(1) + GRAD(2,N2)*P1(2) +
     .          GRAD(3,N2)*P1(3))/UNORM
C
C Test for a valid constraint.
C
      IER = -3
      IF ((RF .LT. 0.  .AND.  MIN(S1,S2,S) .LT. BND)  .OR.
     .    (RF .GT. 0.  .AND.  BND .LT. MAX(S1,S2,S)))
     .   GO TO 11
C
C Test for infinite tension required.
C
      IER = 1
      SIG = SBIG
      IF (S .EQ. BND  .AND.  (S1 .NE. S  .OR.  S2 .NE. S))
     .   GO TO 10
C
C Test for SIG = 0 sufficient.  The Hermite cubic interpo-
C   lant H0 has derivative HP0(T) = (S2 + 2*B0*R + A0*R**2)/
C   AL, where R = (T2-T)/AL.
C
      IER = 0
      SIG = 0.
      T0 = 3.*S - S1 - S2
      B0 = T0 - S2
      C0 = T0 - S1
      A0 = -B0 - C0
C
C   HP0(R) has an extremum (at R = -B0/A0) in (0,1) iff
C     B0*C0 > 0 and the third derivative of H0 has the
C     sign of A0.
C
      IF (B0*C0 .LE. 0.  .OR.  A0*RF .GT. 0.) GO TO 10
C
C   A0*RF < 0 and HP0(R) = -D0/(DT*A0) at R = -B0/A0.
C
      D0 = T0*T0 - S1*S2
      F0 = (BND + D0/(A0*AL))*RF
      IF (F0 .GE. 0.) GO TO 10
C
C Find a zero of F(SIG) = (BND-HP(R))*RF, where HP has an
C   extremum at R.  F has a unique zero, F(0) = F0 < 0, and
C   F = (BND-S)*RF > 0 for SIG sufficiently large.
C
C Initialize parameters for the secant method.  The method
C   uses three points:  (SG0,F0), (SIG,F), and (SNEG,FNEG),
C   where SG0 and SNEG are defined implicitly by DSIG = SIG
C   - SG0 and DMAX = SIG - SNEG.  SG0 is initially zero and
C   SIG is initialized to the zero of (BND - (SIG*S-S1-S2)/
C   (AL*(SIG-2.)))*RF -- a value for which F(SIG) .GE. 0 and
C   F(SIG) = 0 for SIG sufficiently large that 2*SIG is in-
C   significant relative to exp(SIG).
C
      FMAX = (BND-S/AL)*RF
      SIG = 2. - A0/(3.*(AL*BND-S))
      IF (LUN .GE. 0) WRITE (LUN,120) F0, FMAX, SIG
  120 FORMAT (1X,9X,'F0 = ',E15.8,', FMAX = ',E15.8/
     .        1X,8X,'SIG = ',E15.8/)
      IF (CSSTORE(SIG*EXP(-SIG)+.5) .EQ. .5) GO TO 10
      DSIG = SIG
      DMAX = -2.*SIG
      FNEG = FMAX
      D1 = S - S1
      D2 = S2 - S
      D1PD2 = D1 + D2
      NIT = 0
C
C Compute an absolute tolerance FTOL = abs(TOL), and a
C   relative tolerance RTOL = 100*Macheps.
C
      FTOL = ABS(TOL)
      RTOL = 1.
    5 RTOL = RTOL/2.
        IF (CSSTORE(RTOL+1.) .GT. 1.) GO TO 5
      RTOL = RTOL*200.
C
C Top of loop:  compute F.
C
    6 IF (SIG .LE. .5) THEN
C
C   Use approximations designed to avoid cancellation
C     error (associated with small SIG) in the modified
C     hyperbolic functions.
C
        CALL CSSNHCSH (SIG, SINHM,COSHM,COSHMM)
        C1 = SIG*COSHM*D2 - SINHM*D1PD2
        C2 = SIG*(SINHM+SIG)*D2 - COSHM*D1PD2
        A = C2 - C1
        E = SIG*SINHM - COSHMM - COSHMM
      ELSE
C
C   Scale SINHM and COSHM by 2*exp(-SIG) in order to avoid
C     overflow.
C
        EMS = EXP(-SIG)
        EMS2 = EMS + EMS
        TM = 1. - EMS
        SINH = TM*(1.+EMS)
        SINHM = SINH - SIG*EMS2
        COSHM = TM*TM
        C1 = SIG*COSHM*D2 - SINHM*D1PD2
        C2 = SIG*SINH*D2 - COSHM*D1PD2
        A = EMS2*(SIG*TM*D2 + (TM-SIG)*D1PD2)
        E = SIG*SINH - COSHM - COSHM
      ENDIF
C
C   The second derivative HPP of H(R) has a zero at exp(SIG*
C     R) = SQRT((C2+C1)/A) and R is in (0,1) and well-
C     defined iff HPP(T1)*HPP(T2) < 0.
C
      F = FMAX
      T1 = A*(C2+C1)
      IF (T1 .GE. 0.) THEN
        IF (C1*(SIG*COSHM*D1 - SINHM*D1PD2) .LT. 0.) THEN
C
C   HP(R) = (B+SIGN(A)*SQRT(A*C))/(AL*E) at the critical
C     value of R, where A = C2-C1, B = E*S2-C2, and C = C2 +
C     C1.  Note that RF*A < 0.
C
          F = (BND - (E*S2-C2 - RF*SQRT(T1))/(AL*E))*RF
        ENDIF
      ENDIF
C
C   Update the number of iterations NIT.
C
      NIT = NIT + 1
      IF (LUN .GE. 0) WRITE (LUN,130) NIT, SIG, F
  130 FORMAT (1X,3X,I2,' -- SIG = ',E15.8,', F = ',
     .        E15.8)
      IF (F0*F .LT. 0.) THEN
C
C   F0*F < 0.  Update (SNEG,FNEG) to (SG0,F0) so that F
C     and FNEG always have opposite signs.  If SIG is closer
C     to SNEG than SG0 and abs(F) < abs(FNEG), then swap
C     (SNEG,FNEG) with (SG0,F0).
C
        T1 = DMAX
        T2 = FNEG
        DMAX = DSIG
        FNEG = F0
        IF ( ABS(DSIG) .GT. ABS(T1)  .AND.
     .       ABS(F) .LT. ABS(T2) ) THEN
C
          DSIG = T1
          F0 = T2
        ENDIF
      ENDIF
C
C   Test for convergence.
C
      STOL = RTOL*SIG
      IF (ABS(DMAX) .LE. STOL  .OR.  (F .GE. 0.  .AND.
     .    F .LE. FTOL)  .OR.  ABS(F) .LE. RTOL) GO TO 10
      IF (F0*F .LT. 0.  .OR.  ABS(F) .LT. ABS(F0)) GO TO 8
C
C   F*F0 > 0 and the new estimate would be outside of the
C     bracketing interval of length abs(DMAX).  Reset
C     (SG0,F0) to (SNEG,FNEG).
C
    7 DSIG = DMAX
      F0 = FNEG
C
C   Compute the change in SIG by linear interpolation
C     between (SG0,F0) and (SIG,F).
C
    8 DSIG = -F*DSIG/(F-F0)
      IF (LUN .GE. 0) WRITE (LUN,140) DSIG
  140 FORMAT (1X,8X,'DSIG = ',E15.8)
      IF ( ABS(DSIG) .GT. ABS(DMAX)  .OR.
     .     DSIG*DMAX .GT. 0. ) GO TO 7
C
C   Restrict the step-size such that abs(DSIG) .GE. STOL/2.
C     Note that DSIG and DMAX have opposite signs.
C
      IF (ABS(DSIG) .LT. STOL/2.) DSIG = -SIGN(STOL/2.,DMAX)
C
C   Bottom of loop:  update SIG, DMAX, and F0.
C
      SIG = SIG + DSIG
      DMAX = DMAX + DSIG
      F0 = F
      GO TO 6
C
C No errors encountered.
C
   10 CSSIG1 = SIG
      IF (IFLGS .LE. 0) RETURN
      SIGMA(LP1) = SIG
      SIGMA(LP2) = SIG
      RETURN
C
C Error termination.
C
   11 CSSIG1 = -1.
      RETURN
      END


module knapsack
	use constants, only : dbl, bigint, print_frequency
	use ioutil, only : ncombinations, sort_list
	use sample, only : differences
	use nonradiative
	implicit none
contains
	subroutine knap_01(capacity, n, values, res)
		integer, intent(in)					:: n
		real(dbl), intent(in) 				:: capacity
		real(dbl), dimension(n), intent(in)	:: values
		integer(smallint), dimension(n), intent(out)  :: res

		integer									:: maxcap, w, ix, jx, err
		real(dbl)								:: wt
		logical									:: was_added
		real(dbl), allocatable, dimension(:, :)	:: table

		maxcap = ceiling(capacity)+1
		allocate(table(maxcap, n+1), stat=err)
		res = 0
		table = 0d0

		do jx=2,n+1
			wt = values(jx-1)
			do ix=2,maxcap
				if (wt > ix-1) then
					table(ix, jx) = table(ix, jx-1)
				else
					table(ix, jx) = max(table(ix, jx-1), table(ix-int(wt), jx-1) + wt)
				end if
			end do
		end do

		w = maxcap
		do jx=n+1,2,-1
			was_added = (table(w, jx) .ne. table(w, jx-1))
			if (was_added) then
				res(jx-1) = 1
				wt = values(jx-1)
				w = w - int(wt)
			end if
			if (w .le. 0d0) then
				exit
			end if
		end do

		deallocate(table)
	end subroutine knap_01

	subroutine fptas(capacity, n, values, epsilon, res, sumres)
		real(dbl), intent(in)				:: capacity, epsilon
		integer, intent(in)					:: n
		real(dbl), dimension(n), intent(in) :: values
		integer(smallint), dimension(n), intent(out)  :: res
		real(dbl), intent(out)				:: sumres

		real(dbl)				:: K
		real(dbl), dimension(n)	:: new_values
		integer					:: ix

		K = epsilon * maxval(values) / real(n)
		new_values = values / K

		call knap_01(capacity/K, n, new_values, res)
		sumres = dot_product(values, res)
	end subroutine fptas

	subroutine expand(n, energies, occs, res)
		integer, intent(in)								  :: n
		real(dbl), dimension(n), intent(in) 			  :: energies
		integer(smallint), dimension(n), intent(in)  				  :: occs
		real(dbl), allocatable, dimension(:), intent(out) :: res

		integer	:: ix, jx, ctr, ntot
		ntot = sum(occs)
		allocate(res(ntot))

		ctr = 1
		do ix=1,n
			do jx=1,occs(ix)
				res(ctr) = energies(ix)
				ctr = ctr + 1
			end do
		end do
	end subroutine expand

	subroutine contract(nbase, ntot, base, total, res)
		integer, intent(in) 			  	   :: nbase, ntot
		integer(smallint), dimension(nbase), intent(in)  :: base
		integer(smallint), dimension(ntot), intent(in)   :: total
		integer(smallint), dimension(nbase), intent(out) :: res

		integer	:: ctr, ix, jx
		ctr = 1
		do ix=1,nbase
			res(ix) = sum(total(ctr:ctr+base(ix)-1))
			ctr = ctr + base(ix)
		end do
	end subroutine contract

	subroutine adjust(capacity, n, energies, epsilon, nmax, res, sumres)
		integer, intent(in)								:: n, nmax
		real(dbl), intent(in) 							:: capacity, epsilon
		real(dbl), dimension(n), intent(in)				:: energies
		integer(smallint), dimension(n), intent(inout)  :: res
		real(dbl), intent(out)							:: sumres

		integer	:: ntot, ndelta, ix, jx, best_ix, best_jx, iter
		integer(sint) 	  :: indices(n)
		integer(smallint) :: res_sorted(n)
		logical :: empty
		real(dbl) :: ens_sorted(n), edelta, en_diffs(n, n), best_edelta, de
		ntot = sum(res)
		ndelta = nmax - ntot

		if (ndelta .ne. 0) then
			call sort_list(n, energies, ens_sorted, indices)
			call differences(n, ens_sorted, en_diffs)
			do ix=1,n
				res_sorted(ix) = res(indices(ix))
			end do

			res_sorted(1) = res_sorted(1) + ndelta
			neg: do ix=1,n-1
				if (res_sorted(ix) .lt. 0) then
					res_sorted(ix+1) = res_sorted(ix+1) + res_sorted(ix)
					res_sorted(ix) = 0
				else
					exit neg
				end if
			end do neg
			res_sorted(n) = max(0, res_sorted(n))

			edelta = dot_product(ens_sorted, res_sorted)
			edelta = capacity - edelta
			iter = 0
			do while ((abs(edelta) .gt. epsilon) .and. (iter < 100))
				best_edelta = 1d0
				do ix=1,n-1
					do jx=ix+1,n
						if (de*edelta .gt. 0) then
							empty = res_sorted(ix) < 1
						else
							empty = res_sorted(jx) < 1
						end if
						if (.not. empty) then
							de = abs(en_diffs(ix, jx) - edelta)
							if (de < best_edelta) then
								best_edelta = de
								best_ix = ix
								best_jx = jx
							end if
						end if
					end do
				end do
				if (de * edelta .gt. 0) then
					res_sorted(best_ix) = res_sorted(best_ix) - 1
					res_sorted(best_jx) = res_sorted(best_jx) + 1
				else
					res_sorted(best_ix) = res_sorted(best_ix) + 1
					res_sorted(best_jx) = res_sorted(best_jx) - 1
				end if
				edelta = capacity - dot_product(ens_sorted, res_sorted)
				iter = iter + 1
			end do

			do ix=1,n
				res(indices(ix)) = res_sorted(ix)
			end do
			sumres = dot_product(res, energies)
		end if
	end subroutine adjust

	subroutine knap_n(capacity, n, energies, occs, epsilon, nmax, res, sumres, with_adjust)
		integer, intent(in)								:: n, nmax
		real(dbl), intent(in) 							:: capacity, epsilon
		real(dbl), dimension(n), intent(in)				:: energies
		integer(smallint), dimension(n), intent(in)		:: occs
		integer(smallint), dimension(n), intent(out)  	:: res
		real(dbl), intent(out)							:: sumres
		logical, intent(in)								:: with_adjust

		integer								 			:: ntot
		integer(smallint), allocatable, dimension(:) 	:: tempres
		real(dbl), allocatable, dimension(:) 			:: totvalues

		call expand(n, energies, occs, totvalues)
		ntot = size(totvalues)
		allocate(tempres(ntot))
		call fptas(capacity, ntot, totvalues, epsilon, tempres, sumres)
		call contract(n, ntot, occs, tempres, res)

		if (with_adjust) call adjust(capacity, n, energies, epsilon, nmax, res, sumres)

		deallocate(tempres)
		deallocate(totvalues)
	end subroutine knap_n

	recursive subroutine iterate(sys, noccs, emax, emin, occ, ix, enlist, occix, checked, maxnocc)
		type(sysdata), intent(inout)							:: sys
		integer, intent(in)										:: ix
		integer(bigint), intent(in)								:: noccs, maxnocc
		integer(bigint), intent(inout)							:: occix, checked
		integer(smallint), dimension(sys%nlevels), intent(inout)	:: occ
		real(dbl), dimension(noccs), intent(inout)				:: enlist
		real(dbl), intent(in)									:: emax, emin

		integer		:: i, n, tmp
		real(dbl) 	:: en

		n = sys%nlevels
		do i=sys%minoccs(ix),sys%maxoccs(ix)
			occ(ix) = i

			if (ix .lt. n) then
				call iterate(sys, noccs, emax, emin, occ, ix+1, enlist, occix, checked, maxnocc)
			else
				en = dot_product(sys%energies, occ)
				if ((en .lt. emax) .and. (en .gt. emin)) then
					sys%mm%current_block(1:n, occix) = occ(1:n)
					enlist(occix) = en
					occix = occix+1
					if (modulo(occix, 100000) .eq. 0) write(*, *) occix
					if (occix .gt. sys%mm%chunk_size) then
						write(*, '(1x,a,1x,i10)') 'Finished block', sys%mm%current_record
						call sys%calculate_kic(sys%mm%chunk_size, sys%mm%current_block, init=.false., stopix=sys%mm%chunk_size)

						if (sys%do_write) then
							call sys%mm%block_swap(sys%energies, sys%hrfactors, sys%mm%chunk_size, tmp)
						else
							sys%mm%current_record = sys%mm%current_record + 1
							sys%mm%current_block = 0
						end if
						occix = 1
					end if
				end if
				checked = checked + 1
				if (mod(checked, print_frequency) .eq. 0) write(*, *) 'Checked ', checked
			end if
		end do
	end subroutine iterate

	subroutine brute_force(sys, emax, emin, enlist, noccs)
		type(sysdata), intent(inout)						:: sys
		real(dbl), intent(in)								:: emax, emin
		real(dbl), allocatable, dimension(:), intent(out)	:: enlist
		integer(bigint), intent(out)						:: noccs

		integer(bigint) :: max_nocc, occsize, checked
		integer(smallint)	:: cocc(sys%nlevels)
		integer :: ix
		cocc = 0
		cocc(1) = max(sys%minoccs(1), 1)
		max_nocc = sys%maxnoccs
		occsize = sys%mm%chunk_size

		allocate(enlist(occsize))
		noccs = 1
		checked = 0
		write(*, '(1x,a,1x,e12.4,1x,a)') 'Estimated', real(max_nocc), 'occupations to check'
		call iterate(sys, occsize, emax, emin, cocc, 1, enlist, noccs, checked, max_nocc)
	end subroutine brute_force

	subroutine screened_brute_force(sys, emax, emin, enlist, noccs, worst_case)
		type(sysdata), intent(inout)						:: sys
		real(dbl), intent(in)								:: emax, emin
		real(dbl), allocatable, dimension(:), intent(out)	:: enlist
		integer(bigint), intent(in)							:: worst_case
		integer(bigint), intent(out)						:: noccs

		integer(bigint)	:: max_nocc, occsize, checked
		integer(smallint)	:: cocc(sys%nlevels)
		integer :: ix
		cocc = 0
		cocc(1) = sys%cutoffs(1, sys%nthresh+1)
		max_nocc = sys%maxnoccs
		occsize = sys%mm%chunk_size

		allocate(enlist(occsize))
		noccs = 1
		checked = 0
		write(*, '(1x,a,1x,e12.4)') 'Worst case no. of occs to check:', real(worst_case)
		call screened_iterate(sys, occsize, emax, emin, cocc, 1, enlist, noccs, checked, max_nocc, sys%maxnfix, fixedn=.false.)
	end subroutine screened_brute_force

	subroutine screened_fixed_n(sys, emax, emin, enlist, noccs, worst_case)
		type(sysdata), intent(inout)						:: sys
		real(dbl), intent(in)								:: emax, emin
		real(dbl), allocatable, dimension(:), intent(out)	:: enlist
		integer(bigint), intent(in)							:: worst_case
		integer(bigint), intent(out)						:: noccs

		integer(bigint)	:: max_nocc, occsize, checked
		integer(smallint)	:: cocc(sys%nlevels)
		integer :: ix, nx
		max_nocc = sys%maxnoccs
		occsize = sys%mm%chunk_size

		allocate(enlist(occsize))
		noccs = 1
		checked = 0
		write(*, '(1x,a,1x,i5,a,1x,i5)') 'Nmin =', sys%minnfix, ', Nmax =', sys%maxnfix
		write(*, '(1x,a,1x,e12.4)') 'Worst case no. of occs to check:', real(worst_case)
		do nx=sys%minnfix, sys%maxnfix
			cocc = 0
			cocc(1) = sys%cutoffs(1, sys%nthresh+1)
			write(*, '(1x,a,1x,i3,1x,a,1x,e12.4,1x,a)') 'Doing N =', nx, 'found', real(noccs-1), 'valid occs so far'
			call screened_iterate(sys, occsize, emax, emin, cocc, 1, enlist, noccs, checked, max_nocc, nfix=nx, fixedn=.true.)
		end do
	end subroutine

	recursive subroutine screened_iterate(sys, noccs, emax, emin, occ, ix, enlist, occix, checked, maxnocc, nfix, fixedn)
		type(sysdata), intent(inout)								:: sys
		integer, intent(in)											:: ix, nfix
		integer(bigint), intent(in)									:: noccs, maxnocc
		integer(bigint), intent(inout)								:: occix, checked
		integer(smallint), dimension(sys%nlevels), intent(inout)	:: occ
		real(dbl), dimension(noccs), intent(inout)					:: enlist
		real(dbl), intent(in)										:: emax, emin
		logical, intent(in)											:: fixedn

		integer		:: i, maxix, minix, bnd, newnfix, tmp
		real(dbl) 	:: en
		maxix = min(occ(ix), nfix)
		if (ix .eq. sys%nlevels) then
			if (fixedn) then
				minix = max(nfix, 0)
				if (maxix .lt. minix) then
					continue
				end if
				maxix = minix
			else
				minix = 0
			end if
			do i=minix,maxix
				occ(ix) = i
				en = dot_product(sys%energies, occ)
				if ((en .lt. emax) .and. (en .gt. emin)) then
					sys%mm%current_block(:, occix) = occ(:)
					enlist(occix) = en
					occix = occix+1
					if (occix .gt. sys%mm%chunk_size) then
						write(*, '(1x,a,1x,i10)') 'Finished block', sys%mm%current_record
						call sys%calculate_kic(sys%mm%chunk_size, sys%mm%current_block, init=.false., stopix=sys%mm%chunk_size)
						if (sys%do_write) then
							call sys%mm%block_swap(sys%energies, sys%hrfactors, sys%mm%chunk_size, tmp)
						else
							sys%mm%current_record = sys%mm%current_record + 1
							sys%mm%current_block = 0
						end if
						occix = 1
					end if
				end if
				checked = checked + 1
				if (mod(checked, print_frequency) .eq. 0) write(*, *) 'Checked ', checked
			end do
		else
			do i=0,maxix
				occ(ix) = i
				newnfix = nfix - i
				call sys%get_next_bound(occ, ix, bnd)
				occ(ix+1) = sys%cutoffs(ix+1, bnd)
				call screened_iterate(sys, noccs, emax, emin, occ, ix+1, enlist, occix, checked, maxnocc, newnfix, fixedn)
			end do
		end if
	end subroutine screened_iterate

	subroutine lambda(sys, emax, emin, klag)
		type(sysdata), intent(inout)	:: sys
		real(dbl), intent(in)			:: emax, emin
		real(dbl), intent(out)			:: klag

		real(dbl)	:: emid, lam, deltae, en, d1, d2, step, de, predde
		real(dbl)	:: rho, tau, trust, p, maxtrust, bestlam, beste
		real(dbl), dimension(3) :: derivs
		real(dbl), dimension(sys%nlevels) :: occs
		integer		:: ix

		emid = 0.5 * (emax + emin)

		bestlam = 0.0
		lam = 16.0
		beste = emid
		do while (beste .gt. 0.5d0)
			lam = lam - 0.5
			call lagrange(sys%nlevels, sys%hrfactors, sys%energies, lam, derivs)
			en = abs(emid - derivs(1))
			if (en .lt. beste) then
				beste = en
				bestlam = lam
			end if
		end do
		write(*, *)
		write(*, *) 'LAGRANGE MULTIPLIER RESULTS'
		write(*, '(1x,a15,1x,f10.4)') 'Starting lambda:', bestlam

		deltae = 1d0
		p = bestlam
		ix = 0
		maxtrust = 1.0d0
		trust = 0.2d0
		do while ((deltae .gt. 0.01d0) .and. (ix .lt. 50))
			lam = p
			call lagrange(sys%nlevels, sys%hrfactors, sys%energies, lam, derivs)
			en = emid - derivs(1)

			! take Newton step
			d1 = -2d0 * en * derivs(2)
			d2 = 2d0 * derivs(3)**2 + d1 * derivs(2)

			tau = 1d0
			if (d2 .gt. 0) then
				tau = min(abs(d1)/ (trust * d2), 1d0)
			end if
			step = -tau * trust * d1 / abs(d1)

			p = lam + step
			call lagrange(sys%nlevels, sys%hrfactors, sys%energies, p, derivs)
			de = (en - emid + derivs(1))
			predde = -(step*d1 + 0.5*step*d2*step)
			rho = abs(de/predde)
			if (rho .lt. 0.25) then
				trust = 0.25*trust
			else if ((rho .gt. 0.75) .and. ((abs(step)-trust) .lt. 1E-3)) then
				trust = min(2.0*trust, maxtrust)
			else
				trust = trust
			end if

			if (rho .lt. 0.1) p = lam

			deltae = abs(en)
			ix = ix + 1
		end do

		d1 = d1 / abs(d1)
		bestlam = lam
		beste = abs(en)
		de = beste
		deltae = -1d0
		do while (deltae .lt. 0)
			lam = lam - d1*0.005
			call lagrange(sys%nlevels, sys%hrfactors, sys%energies, lam, derivs)
			en = abs(emid - derivs(1))
			if (en .lt. beste) then
				beste = en
				bestlam = lam
			end if
			deltae = en - de
			de = en
		end do
		write(*, '(1x,a15,1x,e12.4,1x,a)') 'Best DE:', beste, 'eV'
		write(*, '(1x,a15,1x,f10.4)') 'Best Lambda:', bestlam

		do ix=1,sys%nlevels
			occs(ix) = sys%hrfactors(ix) * exp(-sys%energies(ix) * bestlam)
		end do
		write(*, '(1x,a15,1x,f8.2)') 'Total occ:', sum(occs)
		sys%lambda_n = nint(sum(occs))
		write(*, '(1x,a15,1x,f10.6,1x,a)') 'Total en:', dot_product(occs, sys%energies), 'eV'
		call sys%calculate_from_lagrange(occs, klag)
	end subroutine lambda

	subroutine lagrange(n, hrfactors, levels, lam, results)
		integer, intent(in)						:: n
		real(dbl), dimension(n), intent(in) 	:: levels, hrfactors
		real(dbl), intent(in)					:: lam
		real(dbl), dimension(3), intent(out)	:: results

		integer		:: ix
		real(dbl) 	:: tmp
		results = 0d0
		do ix=1,n
			tmp = hrfactors(ix) * exp(-levels(ix) * lam) * levels(ix)
			results(1) = results(1) + tmp
			results(2) = results(2) - tmp*levels(ix)
			results(3) = results(3) + tmp*(levels(ix)**2)
		end do
	end subroutine lagrange

end module knapsack


      SUBROUTINE DPCHIM(N,X,F,D,INCFD)
*
*   This subroutine comes from the slatec / cmlib and has
*   been slightly modified for inclusion in scilab : all
*   the error treatment has been removed (and so the
*   parameter IERR) because this is done by the scilab
*   interface.
*
C***BEGIN PROLOGUE  DPCHIM
C***DATE WRITTEN   811103   (YYMMDD)
C***REVISION DATE  870707   (YYMMDD)
C***CATEGORY NO.  E1B
C***KEYWORDS  LIBRARY=SLATEC(PCHIP),
C             TYPE=DOUBLE PRECISION(PCHIM-S DPCHIM-D),
C             CUBIC HERMITE INTERPOLATION,MONOTONE INTERPOLATION,
C             PIECEWISE CUBIC INTERPOLATION
C***AUTHOR  FRITSCH, F. N., (LLNL)
C             MATHEMATICS AND STATISTICS DIVISION
C             LAWRENCE LIVERMORE NATIONAL LABORATORY
C             P.O. BOX 808  (L-316)
C             LIVERMORE, CA  94550
C             FTS 532-4275, (415) 422-4275
C***PURPOSE  Set derivatives needed to determine a monotone piecewise
C            cubic Hermite interpolant to given data.  Boundary values
C            are provided which are compatible with monotonicity.  The
C            interpolant will have an extremum at each point where mono-
C            tonicity switches direction.  (See DPCHIC if user control
C            is desired over boundary or switch conditions.)
C***DESCRIPTION
C
C       **** Double Precision version of PCHIM ****
C
C          DPCHIM:  Piecewise Cubic Hermite Interpolation to
C                  Monotone data.
C
C     Sets derivatives needed to determine a monotone piecewise cubic
C     Hermite interpolant to the data given in X and F.
C
C     Default boundary conditions are provided which are compatible
C     with monotonicity.  (See DPCHIC if user control of boundary con-
C     ditions is desired.)
C
C     If the data are only piecewise monotonic, the interpolant will
C     have an extremum at each point where monotonicity switches direc-
C     tion.  (See DPCHIC if user control is desired in such cases.)
C
C     To facilitate two-dimensional applications, includes an increment
C     between successive values of the F- and D-arrays.
C
C     The resulting piecewise cubic Hermite function may be evaluated
C     by DPCHFE or DPCHFD.
C
C ----------------------------------------------------------------------
C
C  Calling sequence:
C
C        PARAMETER  (INCFD = ...)
C        INTEGER  N, IERR
C        DOUBLE PRECISION  X(N), F(INCFD,N), D(INCFD,N)
C
C        CALL  DPCHIM (N, X, F, D, INCFD, IERR)
C
C   Parameters:
C
C     N -- (input) number of data points.  (Error return if N.LT.2 .)
C           If N=2, simply does linear interpolation.
C
C     X -- (input) real*8 array of independent variable values.  The
C           elements of X must be strictly increasing:
C                X(I-1) .LT. X(I),  I = 2(1)N.
C           (Error return if not.)
C
C     F -- (input) real*8 array of dependent variable values to be
C           interpolated.  F(1+(I-1)*INCFD) is value corresponding to
C           X(I).  DPCHIM is designed for monotonic data, but it will
C           work for any F-array.  It will force extrema at points where
C           monotonicity switches direction.  If some other treatment of
C           switch points is desired, DPCHIC should be used instead.
C                                     -----
C     D -- (output) real*8 array of derivative values at the data
C           points.  If the data are monotonic, these values will
C           determine a monotone cubic Hermite function.
C           The value corresponding to X(I) is stored in
C                D(1+(I-1)*INCFD),  I=1(1)N.
C           No other entries in D are changed.
C
C     INCFD -- (input) increment between successive values in F and D.
C           This argument is provided primarily for 2-D applications.
C           (Error return if  INCFD.LT.1 .)
C
C     IERR -- (output) error flag.  --REMOVED IN THIS VERSION--
C
C***REFERENCES  1. F.N.FRITSCH AND R.E.CARLSON, 'MONOTONE PIECEWISE
C                 CUBIC INTERPOLATION,' SIAM J.NUMER.ANAL. 17, 2 (APRIL
C                 1980), 238-246.
C               2. F.N.FRITSCH AND J.BUTLAND, 'A METHOD FOR CONSTRUCTING
C                 LOCAL MONOTONE PIECEWISE CUBIC INTERPOLANTS,' SIAM
C                 J.SCI.STAT.COMPUT.5,2 (JUNE 1984), 300-304.
C***ROUTINES CALLED  DPCHST,XERROR  ( XERROR is not called in this version )
C***END PROLOGUE  DPCHIM
C
C ----------------------------------------------------------------------
C
C  Change record:
C     82-02-01   1. Introduced  DPCHST  to reduce possible over/under-
C                   flow problems.
C                2. Rearranged derivative formula for same reason.
C     82-06-02   1. Modified end conditions to be continuous functions
C                   of data when monotonicity switches in next interval.
C                2. Modified formulas so end conditions are less prone
C                   of over/underflow problems.
C     82-08-03   Minor cosmetic changes for release 1.
C     87-07-07   Corrected XERROR calls for d.p. name(s).
C
C ----------------------------------------------------------------------
C
C  Programming notes:
C
C     1. The function  DPCHST(ARG1,ARG2)  is assumed to return zero if
C        either argument is zero, +1 if they are of the same sign, and
C        -1 if they are of opposite sign.
C     2. To produce a single precision version, simply:
C        a. Change DPCHIM to PCHIM wherever it occurs,
C        b. Change DPCHST to PCHST wherever it occurs,
C        c. Change all references to the Fortran intrinsics to their
C           single precision equivalents,
C        d. Change the double precision declarations to real, and
C        e. Change the constants ZERO and THREE to single precision.
C
C  DECLARE ARGUMENTS.
C
      INTEGER N, INCFD
      DOUBLE PRECISION  X(N), F(INCFD,N), D(INCFD,N)
C
C  DECLARE LOCAL VARIABLES.
C
      INTEGER I, NLESS1
      DOUBLE PRECISION  DEL1, DEL2, DMAX, DMIN, DRAT1, DRAT2, DSAVE,
     *      H1, H2, HSUM, HSUMT3, THREE, W1, W2, ZERO
      external DPCHST
      DOUBLE PRECISION  DPCHST
      DATA  ZERO /0.D0/, THREE/3.D0/

      NLESS1 = N - 1
      H1 = X(2) - X(1)
      DEL1 = (F(1,2) - F(1,1))/H1
      DSAVE = DEL1
C
C  SPECIAL CASE N=2 -- USE LINEAR INTERPOLATION.
C
C    patch Bruno N was NLESS1
      IF (N .EQ. 2) THEN
         D(1,1) = DEL1
         D(1,N) = DEL1
         return
      endif

C
C  NORMAL CASE  (N .GE. 3).
C
      H2 = X(3) - X(2)
      DEL2 = (F(1,3) - F(1,2))/H2
C
C  SET D(1) VIA NON-CENTERED THREE-POINT FORMULA, ADJUSTED TO BE
C     SHAPE-PRESERVING.
C
      HSUM = H1 + H2
      W1 = (H1 + HSUM)/HSUM
      W2 = -H1/HSUM
      D(1,1) = W1*DEL1 + W2*DEL2
      IF ( DPCHST(D(1,1),DEL1) .LE. ZERO)  THEN
         D(1,1) = ZERO
      ELSE IF ( DPCHST(DEL1,DEL2) .LT. ZERO)  THEN
C        NEED DO THIS CHECK ONLY IF MONOTONICITY SWITCHES.
         DMAX = THREE*DEL1
         IF (DABS(D(1,1)) .GT. DABS(DMAX))  D(1,1) = DMAX
      ENDIF
C
C  LOOP THROUGH INTERIOR POINTS.
C
      DO 50  I = 2, NLESS1
         IF (I .EQ. 2)  GO TO 40
C
         H1 = H2
         H2 = X(I+1) - X(I)
         HSUM = H1 + H2
         DEL1 = DEL2
         DEL2 = (F(1,I+1) - F(1,I))/H2
   40    CONTINUE
C
C        SET D(I)=0 UNLESS DATA ARE STRICTLY MONOTONIC.
C
         D(1,I) = ZERO
         IF ( DPCHST(DEL1,DEL2) .LT. 0) GOTO 42
         IF ( DPCHST(DEL1,DEL2) .EQ. 0) GOTO 41
         IF ( DPCHST(DEL1,DEL2) .GT. 0) GOTO 45
C
C        COUNT NUMBER OF CHANGES IN DIRECTION OF MONOTONICITY.
C
   41    CONTINUE
         IF (DEL2 .EQ. ZERO)  GO TO 50
         DSAVE = DEL2
         GO TO 50
C
   42    CONTINUE
         DSAVE = DEL2
         GO TO 50
C
C        USE BRODLIE MODIFICATION OF BUTLAND FORMULA.
C
   45    CONTINUE
         HSUMT3 = HSUM+HSUM+HSUM
         W1 = (HSUM + H1)/HSUMT3
         W2 = (HSUM + H2)/HSUMT3
         DMAX = DMAX1( DABS(DEL1), DABS(DEL2) )
         DMIN = DMIN1( DABS(DEL1), DABS(DEL2) )
         DRAT1 = DEL1/DMAX
         DRAT2 = DEL2/DMAX
         D(1,I) = DMIN/(W1*DRAT1 + W2*DRAT2)
C
   50 CONTINUE
C
C  SET D(N) VIA NON-CENTERED THREE-POINT FORMULA, ADJUSTED TO BE
C     SHAPE-PRESERVING.
C
      W1 = -H2/HSUM
      W2 = (H2 + HSUM)/HSUM
      D(1,N) = W1*DEL1 + W2*DEL2
      IF ( DPCHST(D(1,N),DEL2) .LE. ZERO)  THEN
         D(1,N) = ZERO
      ELSE IF ( DPCHST(DEL1,DEL2) .LT. ZERO)  THEN
C        NEED DO THIS CHECK ONLY IF MONOTONICITY SWITCHES.
         DMAX = THREE*DEL2
         IF (DABS(D(1,N)) .GT. DABS(DMAX))  D(1,N) = DMAX
      ENDIF
C
C  NORMAL RETURN.
C
      end

      DOUBLE PRECISION FUNCTION DPCHST(ARG1,ARG2)
C***BEGIN PROLOGUE  DPCHST
C***REFER TO  DPCHCE,DPCHCI,DPCHCS,DPCHIM
C***ROUTINES CALLED  (NONE)
C***REVISION DATE  870707   (YYMMDD)
C***DESCRIPTION
C
C         DPCHST:  DPCHIP Sign-Testing Routine.
C
C
C     Returns:
C        -1. if ARG1 and ARG2 are of opposite sign.
C         0. if either argument is zero.
C        +1. if ARG1 and ARG2 are of the same sign.
C
C     The object is to do this without multiplying ARG1*ARG2, to avoid
C     possible over/underflow problems.
C
C  Fortran intrinsics used:  SIGN.
C
C***END PROLOGUE  DPCHST
C
C ----------------------------------------------------------------------
C
C  Programmed by:  Fred N. Fritsch,  FTS 532-4275, (415) 422-4275,
C                  Mathematics and Statistics Division,
C                  Lawrence Livermore National Laboratory.
C
C  Change record:
C     82-08-05   Converted to SLATEC library version.
C
C ----------------------------------------------------------------------
C
C  Programming notes:
C
C     To produce a single precision version, simply:
C        a. Change DPCHST to PCHST wherever it occurs,
C        b. Change all references to the Fortran intrinsics to their
C           single presision equivalents,
C        c. Change the double precision declarations to real, and
C        d. Change the constants  ZERO  and  ONE  to single precision.
C
C  DECLARE ARGUMENTS.
C
      DOUBLE PRECISION  ARG1, ARG2
C
C  DECLARE LOCAL VARIABLES.
C
      DOUBLE PRECISION  ONE, ZERO
      DATA  ZERO /0.D0/,  ONE/1.D0/
C
C  PERFORM THE TEST.
C
C***FIRST EXECUTABLE STATEMENT  DPCHST
      DPCHST = DSIGN(ONE,ARG1) * DSIGN(ONE,ARG2)
      IF ((ARG1.EQ.ZERO) .OR. (ARG2.EQ.ZERO))  DPCHST = ZERO
C
      RETURN
C------------- LAST LINE OF DPCHST FOLLOWS -----------------------------
      END


      subroutine sblas()
      i = isamax(n,sx,incx)
      r = sasum(n,sx,incx)
      call saxpy(n,sa,sx,incx,sy,incy)
      call scopy(n,sx,incx,sy,incy)
      r = sdot(n,sx,incx,sy,incy)
      r = smach(job)
      r = snrm2 ( n, sx, incx)
      call srot (n,sx,incx,sy,incy,c,s)
      call srotg(sa,sb,c,s)
      call sscal(n,sa,sx,incx)
      call sswap (n,sx,incx,sy,incy)
      stop
      end


#include "eiscor.h"
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! d_orthhess_qr
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This routine computes the real Schur factorization of a real
! orthogonal upper-Hessenberg matrix H.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! INPUT VARIABLES:
!
!  VEC             LOGICAL
!                    .TRUE.: compute schurvectors
!                    .FALSE.: no schurvectors
!
!  ID              LOGICAL
!                    .TRUE.: initialize to Z to identity
!                    .FALSE.: assume Z is already initialized
!
!  N               INTEGER
!                    dimension of matrix
!
!  H               REAL(8) array of dimension (N,N)
!                    orthogonal hessenberg matrix, assumed that
!                    H(ii,jj) = 0 for |ii-jj| > 0
!                    on exit H is a block diagonal matrix
!
!  WORK            REAL(8) array of dimension (3*N)
!                    work space for eigensolver
!
!  M               INTEGER
!                    leading dimension of Z
!
! OUTPUT VARIABLES:
!
!  Z               REAL(8) array of dimension (M,N)
!                    components of schurvectors
!                    if VEC = .FALSE. unused
!                    if VEC = .TRUE. and ID = .TRUE. initializes Z to I
!                    if VEC = .TRUE. and ID = .FALSE. assumes Z initialized
!
!  ITS             INTEGER array of dimension (N-1)
!                    Contains the number of iterations per deflation
!
!  INFO            INTEGER
!                    INFO = 2 implies d_orthfact_qr failed
!                    INFO = 1 implies d_orthhess_factor failed
!                    INFO = 0 implies successful computation
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine d_orthhess_qr(VEC,ID,N,H,WORK,M,Z,ITS,INFO)

  implicit none

  ! input variables
  logical, intent(in) :: VEC, ID
  integer, intent(in) :: N, M
  real(8), intent(inout) :: WORK(3*N)
  integer, intent(inout) :: ITS(N-1), INFO
  real(8), intent(inout) :: H(N,N), Z(M,N)

  ! compute variables
  integer :: ii

  ! initialize INFO
  INFO = 0

  ! compress H
  call d_orthhess_factor(N,H,WORK(1:(2*N)),WORK((2*N+1):(3*N)),INFO)

  ! check info
  if (INFO.NE.0) then
    ! print error in debug mode
    if (DEBUG) then
      call u_infocode_check(__FILE__,__LINE__,"d_orthhess_factor failed" &
      ,INFO,INFO)
    end if
    INFO = 1
    return
  end if

  ! compute eigenvalues
  call d_orthfact_qr(VEC,ID,N,WORK(1:(2*N)),WORK((2*N+1):(3*N)),M,Z,ITS,INFO)

  ! check info
  if (INFO.NE.0) then
    ! print error in debug mode
    if (DEBUG) then
      call u_infocode_check(__FILE__,__LINE__,"d_orthfact_qr failed",INFO,INFO)
    end if
    INFO = 2
    return
  end if

  ! update H
  H = 0d0
  do ii=1,N
    H(ii,ii) = 1d0
  end do
  do ii=1,N-1
    if (abs(WORK(2*ii)).NE.0d0) then
       H(ii,ii) = WORK(2*ii-1)
       H(ii+1,ii+1) = WORK(2*ii-1)
       H(ii+1,ii) = WORK(2*ii)
       H(ii,ii+1) = -WORK(2*ii)
    end if
  end do
  do ii=1,N
    H(:,ii) = H(:,ii)*WORK(2*N+ii)
  end do

end subroutine d_orthhess_qr


module fgamma

use assert, only: wp

implicit none (type, external)

contains

!***********************************************************************************************************************************
!  PSI
!
!  Returns the digamma function of the argument XX.  XX cannot be 0 or a negative integer.
!
!  From http://www.netlib.org/specfun/gamma
!***********************************************************************************************************************************

      FUNCTION PSI(XX)
!----------------------------------------------------------------------
!
! This function program evaluates the logarithmic derivative of the
!   gamma function,
!
!      psi(x) = d/dx (gamma(x)) / gamma(x) = d/dx (ln gamma(x))
!
!   for real x, where either
!
!          -xmax1 < x < -xmin (x not a negative integer), or
!            xmin < x.
!
!   The calling sequence for this function is
!
!                  Y = PSI(X)
!
!   The main computation uses rational Chebyshev approximations
!   published in Math. Comp. 27, 123-127 (1973) by Cody, Strecok and
!   Thacher.  This transportable program is patterned after the
!   machine-dependent FUNPACK program PSI(X), but cannot match that
!   version for efficiency or accuracy.  This version uses rational
!   approximations that are theoretically accurate to 20 significant
!   decimal digits.  The accuracy achieved depends on the arithmetic
!   system, the compiler, the intrinsic functions, and proper selection
!   of the machine-dependent constants.
!
!*******************************************************************
!*******************************************************************
!
! Explanation of machine-dependent constants
!
!   XINF   = largest positive machine number
!   XMAX1  = beta ** (p-1), where beta is the radix for the
!            floating-point system, and p is the number of base-beta
!            digits in the floating-point significand.  This is an
!            upper bound on non-integral floating-point numbers, and
!            the negative of the lower bound on acceptable negative
!            arguments for PSI.  If rounding is necessary, round this
!            value down.
!   XMIN1  = the smallest in magnitude acceptable argument.  We
!            recommend XMIN1 = MAX(1/XINF,xmin) rounded up, where
!            xmin is the smallest positive floating-point number.
!   XSMALL = absolute argument below which  PI*COTAN(PI*X)  may be
!            represented by 1/X.  We recommend XSMALL < sqrt(3 eps)/pi,
!            where eps is the smallest positive number such that
!            1+eps > 1.
!   XLARGE = argument beyond which PSI(X) may be represented by
!            LOG(X).  The solution to the equation
!               x*ln(x) = beta ** p
!            is a safe value.
!
!     Approximate values for some important machines are
!
!                        beta  p     eps     xmin       XINF
!
!  CDC 7600      (S.P.)    2  48  7.11E-15  3.13E-294  1.26E+322
!  CRAY-1        (S.P.)    2  48  7.11E-15  4.58E-2467 5.45E+2465
!  IEEE (IBM/XT,
!    SUN, etc.)  (S.P.)    2  24  1.19E-07  1.18E-38   3.40E+38
!  IEEE (IBM/XT,
!    SUN, etc.)  (D.P.)    2  53  1.11D-16  2.23E-308  1.79D+308
!  IBM 3033      (D.P.)   16  14  1.11D-16  5.40D-79   7.23D+75
!  SUN 3/160     (D.P.)    2  53  1.11D-16  2.23D-308  1.79D+308
!  VAX 11/780    (S.P.)    2  24  5.96E-08  2.94E-39   1.70E+38
!                (D.P.)    2  56  1.39D-17  2.94D-39   1.70D+38
!   (G Format)   (D.P.)    2  53  1.11D-16  5.57D-309  8.98D+307
!
!                         XMIN1      XMAX1     XSMALL    XLARGE
!
!  CDC 7600      (S.P.)  3.13E-294  1.40E+14  4.64E-08  9.42E+12
!  CRAY-1        (S.P.)  1.84E-2466 1.40E+14  4.64E-08  9.42E+12
!  IEEE (IBM/XT,
!    SUN, etc.)  (S.P.)  1.18E-38   8.38E+06  1.90E-04  1.20E+06
!  IEEE (IBM/XT,
!    SUN, etc.)  (D.P.)  2.23D-308  4.50D+15  5.80D-09  2.71D+14
!  IBM 3033      (D.P.)  1.39D-76   4.50D+15  5.80D-09  2.05D+15
!  SUN 3/160     (D.P.)  2.23D-308  4.50D+15  5.80D-09  2.71D+14
!  VAX 11/780    (S.P.)  5.89E-39   8.38E+06  1.35E-04  1.20E+06
!                (D.P.)  5.89D-39   3.60D+16  2.05D-09  2.05D+15
!   (G Format)   (D.P.)  1.12D-308  4.50D+15  5.80D-09  2.71D+14
!
!*******************************************************************
!*******************************************************************
!
! Error Returns
!
!  The program returns XINF for  X < -XMAX1, for X zero or a negative
!    integer, or when X lies in (-XMIN1, 0), and returns -XINF
!    when X lies in (0, XMIN1).
!
! Intrinsic functions required are:
!
!     ABS, AINT, DBLE, INT, LOG, REAL, TAN
!
!
!  Author: W. J. Cody
!          Mathematics and Computer Science Division
!          Argonne National Laboratory
!          Argonne, IL 60439
!
!  Latest modification: June 8, 1988
!
!----------------------------------------------------------------------
      INTEGER I,N,NQ
!S    REAL
      real(wp)                                                  &
         AUG,DEN,PSI,FOUR,FOURTH,HALF,ONE,P1,P2,PIOV4,Q1,Q2,       &
         SGN,THREE,XLARGE,UPPER,W,X,XINF,XMAX1,XMIN1,XSMALL,X01,        &
         X01D,X02,XX,Z,ZERO
      DIMENSION P1(9),P2(7),Q1(8),Q2(6)
!----------------------------------------------------------------------
!  Mathematical constants.  PIOV4 = pi / 4
!----------------------------------------------------------------------
!S    DATA ZERO,FOURTH,HALF,ONE/0.0E0,0.25E0,0.5E0,1.0E0/
!S    DATA THREE,FOUR/3.0E0,4.0E0/,PIOV4/7.8539816339744830962E-01/
      DATA ZERO,FOURTH,HALF,ONE/0.0D0,0.25D0,0.5D0,1.0D0/
      DATA THREE,FOUR/3.0D0,4.0D0/,PIOV4/7.8539816339744830962D-01/
!----------------------------------------------------------------------
!  Machine-dependent constants
!----------------------------------------------------------------------
!S    DATA XINF/1.70E+38/, XMIN1/5.89E-39/, XMAX1/8.38E+06/,
!S   1     XSMALL/1.35E-04/, XLARGE/1.20E+06/
      DATA XINF/1.70D+38/, XMIN1/5.89D-39/, XMAX1/3.60D+16/,            &
           XSMALL/2.05D-09/, XLARGE/2.04D+15/
!----------------------------------------------------------------------
!  Zero of psi(x)
!----------------------------------------------------------------------
!S    DATA X01/187.0E0/,X01D/128.0E0/,X02/6.9464496836234126266E-04/
      DATA X01/187.0D0/,X01D/128.0D0/,X02/6.9464496836234126266D-04/
!----------------------------------------------------------------------
!  Coefficients for approximation to  psi(x)/(x-x0)  over [0.5, 3.0]
!----------------------------------------------------------------------
!S    DATA P1/4.5104681245762934160E-03,5.4932855833000385356E+00,
!S   1        3.7646693175929276856E+02,7.9525490849151998065E+03,
!S   2        7.1451595818951933210E+04,3.0655976301987365674E+05,
!S   3        6.3606997788964458797E+05,5.8041312783537569993E+05,
!S   4        1.6585695029761022321E+05/
!S    DATA Q1/9.6141654774222358525E+01,2.6287715790581193330E+03,
!S   1        2.9862497022250277920E+04,1.6206566091533671639E+05,
!S   2        4.3487880712768329037E+05,5.4256384537269993733E+05,
!S   3        2.4242185002017985252E+05,6.4155223783576225996E-08/
      DATA P1/4.5104681245762934160D-03,5.4932855833000385356D+00,      &
              3.7646693175929276856D+02,7.9525490849151998065D+03,      &
              7.1451595818951933210D+04,3.0655976301987365674D+05,      &
              6.3606997788964458797D+05,5.8041312783537569993D+05,      &
              1.6585695029761022321D+05/
      DATA Q1/9.6141654774222358525D+01,2.6287715790581193330D+03,      &
              2.9862497022250277920D+04,1.6206566091533671639D+05,      &
              4.3487880712768329037D+05,5.4256384537269993733D+05,      &
              2.4242185002017985252D+05,6.4155223783576225996D-08/
!----------------------------------------------------------------------
!  Coefficients for approximation to  psi(x) - ln(x) + 1/(2x)
!     for  x > 3.0
!----------------------------------------------------------------------
!S    DATA P2/-2.7103228277757834192E+00,-1.5166271776896121383E+01,
!S   1        -1.9784554148719218667E+01,-8.8100958828312219821E+00,
!S   2        -1.4479614616899842986E+00,-7.3689600332394549911E-02,
!S   3        -6.5135387732718171306E-21/
!S    DATA Q2/ 4.4992760373789365846E+01, 2.0240955312679931159E+02,
!S   1         2.4736979003315290057E+02, 1.0742543875702278326E+02,
!S   2         1.7463965060678569906E+01, 8.8427520398873480342E-01/
      DATA P2/-2.7103228277757834192D+00,-1.5166271776896121383D+01,    &
              -1.9784554148719218667D+01,-8.8100958828312219821D+00,    &
              -1.4479614616899842986D+00,-7.3689600332394549911D-02,    &
              -6.5135387732718171306D-21/
      DATA Q2/ 4.4992760373789365846D+01, 2.0240955312679931159D+02,    &
               2.4736979003315290057D+02, 1.0742543875702278326D+02,    &
               1.7463965060678569906D+01, 8.8427520398873480342D-01/
!----------------------------------------------------------------------
!S    CONV(I) = REAL(I)

      X = XX
      W = ABS(X)
      AUG = ZERO
!----------------------------------------------------------------------
!  Check for valid arguments, then branch to appropriate algorithm
!----------------------------------------------------------------------
      IF ((-X .GE. XMAX1) .OR. (W .LT. XMIN1)) THEN
            GO TO 410
         ELSE IF (X .GE. HALF) THEN
            GO TO 200
!----------------------------------------------------------------------
!  X < 0.5, use reflection formula: psi(1-x) = psi(x) + pi * cot(pi*x)
!     Use 1/X for PI*COTAN(PI*X)  when  XMIN1 < |X| <= XSMALL.
!----------------------------------------------------------------------
         ELSE IF (W .LE. XSMALL) THEN
            AUG = -ONE / X
            GO TO 150
      END IF
!----------------------------------------------------------------------
!  Argument reduction for cot
!----------------------------------------------------------------------
  100 IF (X .LT. ZERO) THEN
            SGN = PIOV4
         ELSE
            SGN = -PIOV4
      END IF
      W = W - AINT(W)
      NQ = INT(W * FOUR)
      W = FOUR * (W - real(NQ, wp) * FOURTH)
!----------------------------------------------------------------------
!  W is now related to the fractional part of  4.0 * X.
!     Adjust argument to correspond to values in the first
!     quadrant and determine the sign.
!----------------------------------------------------------------------
      N = NQ / 2
      IF ((N+N) .NE. NQ) W = ONE - W
      Z = PIOV4 * W
      IF (MOD(N,2) .NE. 0) SGN = - SGN
!----------------------------------------------------------------------
!  determine the final value for  -pi * cotan(pi*x)
!----------------------------------------------------------------------
      N = (NQ + 1) / 2
      IF (MOD(N,2) .EQ. 0) THEN
!----------------------------------------------------------------------
!  Check for singularity
!----------------------------------------------------------------------
            IF (Z .EQ. ZERO) GO TO 410
            AUG = SGN * (FOUR / TAN(Z))
         ELSE
            AUG = SGN * (FOUR * TAN(Z))
      END IF
  150 X = ONE - X
  200 IF (X .GT. THREE) GO TO 300
!----------------------------------------------------------------------
!  0.5 <= X <= 3.0
!----------------------------------------------------------------------
      DEN = X
      UPPER = P1(1) * X
      DO 210 I = 1, 7
         DEN = (DEN + Q1(I)) * X
         UPPER = (UPPER + P1(I+1)) * X
  210 CONTINUE
      DEN = (UPPER + P1(9)) / (DEN + Q1(8))
      X = (X-X01/X01D) - X02
      PSI = DEN * X + AUG
      GO TO 500
!----------------------------------------------------------------------
!  3.0 < X
!----------------------------------------------------------------------
  300 IF (X .LT. XLARGE) THEN
         W = ONE / (X * X)
         DEN = W
         UPPER = P2(1) * W
         DO 310 I = 1, 5
            DEN = (DEN + Q2(I)) * W
            UPPER = (UPPER + P2(I+1)) * W
  310    CONTINUE
         AUG = (UPPER + P2(7)) / (DEN + Q2(6)) - HALF / X + AUG
      END IF
      PSI = AUG + LOG(X)
      GO TO 500
!----------------------------------------------------------------------
!  Error return
!----------------------------------------------------------------------
  410 PSI = XINF
      IF (X .GT. ZERO) PSI = -XINF
  500 RETURN
!---------- Last card of PSI ----------
      END function psi

!***********************************************************************************************************************************
!  CGAMMA
!
!  Complex gamma function.
!  Formulae from "An Atlas of Functions" by Spanier and Oldham, Sect. 43:11.
!***********************************************************************************************************************************

      complex(wp) FUNCTION CGAMMA (Z) RESULT (R)
      use hyper, only: csch

      COMPLEX(wp), INTENT(IN) :: Z

      real(wp), PARAMETER :: EPS = 1.0D-14
      real(wp), PARAMETER :: PI = 4._wp * atan(1._wp)
      real(wp), PARAMETER :: EULER = 0.57721566490153286060651209008240243104215933593992359880576723488486772677766467094_wp

      INTEGER :: J
      real(wp) :: X, Y, THETA, SUM, PROD, PSUM, PPROD

      X = DBLE(Z)
      Y = AIMAG(Z)

      IF (Z .EQ. (0.0D0,0.0D0)) THEN
         ! write(stderr, *) '  CGAMMA Error.'
         R = (0.0D0,0.0D0)
         RETURN
      END IF

      IF (Y .EQ. 0.0D0) THEN                                                        ! if real Z
         R = CMPLX(GAMMA(X),0.0D0, wp)
         RETURN
      END IF

      IF (X .EQ. 0.0D0) GO TO 100                                                   ! branch for imaginary Z

!
!     Complex Z
!

      SUM = 0.0D0
      PSUM = HUGE(0._wp)

      J = 0
      DO
         SUM = SUM + Y/(real(j,wp)+X) - ATAN2(Y, real(J,wp)+X)
         IF (ABS((SUM-PSUM)/SUM) .LE. EPS) EXIT
         PSUM = SUM
         J = J + 1
         IF (J .LT. 0) EXIT
      END DO

      THETA = Y*PSI(X) + SUM

      PROD = 1._wp
      PPROD = HUGE(0._wp)

      J = 0
      DO
         PROD = PROD * ABS(real(j,wp)+X)/SQRT(Y**2+(real(J,wp)+X)**2)
         IF (ABS((PROD-PPROD)/PROD) .LE. EPS) EXIT
         PPROD = PROD
         J = J + 1
         IF (J .LT. 0) EXIT
      END DO

      R = CMPLX(COS(THETA),SIN(THETA), wp) * ABS(gamma(X)) * PROD

      RETURN

!
!     Imaginary Z
!

  100 SUM = 0.0D0
      PSUM = HUGE(0._wp)

      J = 1
      DO
         SUM = SUM + Y/real(j,wp) - ATAN2(Y,real(j,wp))
         IF (ABS((SUM-PSUM)/SUM) .LE. EPS) EXIT
         PSUM = SUM
         J = J + 1
         IF (J .LT. 0) EXIT
      END DO

      THETA = -EULER*Y + SUM

      R = SQRT((PI/Y)*CSCH(PI*Y)) * CMPLX(SIN(THETA),-COS(THETA), wp)

      END FUNCTION CGAMMA

!***********************************************************************************************************************************
!  BETA
!
!  Beta function.
!***********************************************************************************************************************************

elemental real(wp) FUNCTION BETA (X,Y) RESULT (R)

real(wp), INTENT(IN) :: X, Y

R = gamma(X)*gamma(Y)/gamma(X+Y)

END FUNCTION BETA

!***********************************************************************************************************************************
!  CBETA
!
!  Complex beta function.
!***********************************************************************************************************************************

complex(wp) FUNCTION CBETA (X,Y) RESULT (R)

COMPLEX(wp), INTENT(IN) :: X, Y

R = CGAMMA(X)*CGAMMA(Y)/CGAMMA(X+Y)

END FUNCTION CBETA

!***********************************************************************************************************************************
!  RBETA
!
!  Rational beta function.
!***********************************************************************************************************************************

elemental SUBROUTINE RBETA (X, Y, N, D)
use rat, only: ratnorm

INTEGER, INTENT(IN) :: X, Y
INTEGER, INTENT(OUT) :: N, D
INTEGER :: I

N = 1

DO I = 2, X-1
   N = N * I
END DO

DO I = 2, Y-1
   N = N * I
END DO

D = 1

DO I = 2, X+Y-1
   D = D * I
END DO

CALL RATNORM (N, D)

END SUBROUTINE RBETA

end module fgamma


subroutine MakeGeoidGrid(geoid, cilm, lmax, r0pot, GM, PotRef, omega, r, &
                         gridtype, order, nlat, nlong, interval, lmax_calc,&
                         a, f, exitstatus)
!------------------------------------------------------------------------------
!
!   This subrouine will calculate the height to an equipotential surface with
!   respect to a spherical reference radius R for a body rotating with angular
!   rotation rate OMEGA. This method uses a first-, second-, or third-order
!   Taylor series expansion of the potential, evaluated at radius R. The output
!   gridype is specificied by GRIDTYPE, which can be either Cartesian 2D,
!   Guass-Legendre quadrature, or Driscoll and Healy. If the optional
!   parameters A and F are specified, the geoid will be referenced to a
!   flattened ellispoid with semimajor axis A and flattening F.
!
!   Note that this routine is only strictly valid when the geoid is above the
!   surface! To calculated the height of the geoid when it is below the
!   surface, one would need to know the density structure of the planet.
!   Furthermore, the calculation of the potential (and its derivatives) is
!   only strictly valid when R lies above the maximum radius of the planet.
!
!   Latitude is geocentric latitude.
!
!   Calling Parameters
!
!       IN
!           cilm        Spherical harmonic potential coefficients.
!           lmax        Maximum spherical harmonic degree. For gridtype=1, this
!                       is simply the maximum degree used in evaluating the
!                       spherical harmonic coefficients. For the other grids,
!                       this sets the spacing of the output grid.
!           r0pot       Reference radius of the potential coefficients.
!           GM          GM associated with the potential coefficients.
!           PotRef      Reference potential of the geoid.
!           omega       Angular rotation rate used when calculating the geoid
!                       on in the rotating coordinate system.
!           r           Reference sperical radius to calculate the geoid
!                       heights.
!           gridtype    1 = Gauss-Legendre quadrature grid corresponding
!                           to LMAX.
!                       2 = N by N Driscoll and Healy grid corresponding
!                           to LMAX.
!                       3 = N by 2N Driscoll and Healy grid corresponding
!                           to LMAX.
!                       4 = 2D Cartesian using MakeGrid2D.
!           order       Order of the Taylor expansion. Either 1, 2, or 3.
!
!       OUT
!           geoid       Gridded values of the geoid, in meters, referenced to
!                       the spherical radius R.
!           nlat        Number of latitude points for the grid.
!           nlong       Number of longitude points for the grid.
!
!       OPTIONAL, IN
!           interval    Grid spacing of the output grid in DEGREES. Used only
!                       when GRIDTYPE = 1.
!           lmax_calc   For GRIDTYPE 2, 3, and 4, this specifies the maximum
!                       spherical harmonic degree to evaluate the function to.
!           a           Semimajor axis of the reference flattened ellipsoid.
!           f           Flattening of the reference ellipsoid.
!
!       OPTIONAL (OUT)
!           exitstatus  If present, instead of executing a STOP when an error
!                       is encountered, the variable exitstatus will be
!                       returned describing the error.
!                       0 = No errors;
!                       1 = Improper dimensions of input array;
!                       2 = Improper bounds for input variable;
!                       3 = Error allocating memory;
!                       4 = File IO error.
!
!   Copyright (c) 2005-2019, SHTOOLS
!   All rights reserved.
!
!------------------------------------------------------------------------------
    use SHTOOLS, only: MakeGrid2D, MakeGridGLQ, MakeGridDH, SHGLQ
    use ftypes

    implicit none

    real(dp), intent(out) :: geoid(:,:)
    real(dp), intent(in) :: cilm(:,:,:), r0pot, GM, r, PotRef, omega
    integer, intent(in) :: lmax, order, gridtype
    integer, intent(in), optional :: lmax_calc
    integer, intent(out) :: nlat, nlong
    real(dp), intent(in), optional :: interval, a, f
    integer, intent(out), optional :: exitstatus
    real(dp) :: pi, r_ex, lat
    integer :: l, nlat1, nlong1, lmax_comp, astat, n, i, astat1, astat2
    real(dp), allocatable :: grida(:,:), gridb(:,:), gridc(:,:), gridd(:,:), &
                           zero(:), w(:), qq(:,:), pp(:,:), uu(:,:), &
                           cilm1(:,:,:), cilm2(:,:,:)

    if (present(exitstatus)) exitstatus = 0

    if ( (present(f) .and. .not. present(a)) .or. (present(a) .and. &
            .not. present(f)) ) then
        print*, "Error --- MakeGeoidGrid"
        print*, "Both F and A must be specified."
        print*, "A, ", present(a)
        print*, "F, ", present(f)
        if (present(exitstatus)) then
            exitstatus = 5
            return
        else
            stop
        end if

    end if

    if (present(lmax_calc)) then
        if (lmax_calc > lmax) then
            print*, "Error --- MakeGeoidGrid"
            print*, "LMAX_CALC must be less than or equal to LMAX."
            print*, "LMAX_CALC = ", lmax_calc
            print*, "LMAX = ", lmax
            if (present(exitstatus)) then
                exitstatus = 2
                return
            else
                stop
            end if

        end if
    end if

    if (gridtype < 1 .or. gridtype > 4) then
        print*, "Error --- MakeGeoidGrid"
        print*, "GRIDTYPE must be either (1) Cartesian 2D, " // &
                "(2) Gauss-Legendre quadrature,"
        print*, "(3) N by N Driscoll and Healy, or " // &
                "(4) N by 2N Driscoll and Healy."
        print*, "Input value = ", gridtype
        if (present(exitstatus)) then
            exitstatus = 2
            return
        else
            stop
        end if

    end if

    if (gridtype == 4 .and. .not. present(interval)) then
        print*, "Error --- MakeGeoidGrid"
        print*, "If GRIDTYPE = 4 (2D Cartesian), the optional " // &
                "parameter INTERVAL must be specified."
        if (present(exitstatus)) then
            exitstatus = 5
            return
        else
            stop
        end if

    end if

    if (order > 3 .or. order < 1) then
        print*, "Error --- MakeGeoidGrid"
        print*, "ORDER must be 1, 2, or 3."
        if (present(exitstatus)) then
            exitstatus = 2
            return
        else
            stop
        end if

    end if

    if (gridtype == 1) then
        nlong = 2 * lmax + 1
        nlat = lmax + 1

    else if (gridtype == 2) then
        nlat = 2 * lmax+2
        nlong = nlat

    else if (gridtype == 3) then
        nlat = 2 * lmax+2
        nlong = 2 * nlat

    else if (gridtype == 4) then
        nlat = int(180.0_dp / interval + 1)
        nlong = int(360.0_dp / interval + 1)

    end if

    if (size(geoid(:,1)) < nlat .or. size(geoid(1,:)) < nlong ) then
        print*, "Error --- MakeGeoidGrid"
        print*, "GEOID must be dimensioned as (180/INTERVAL+1, " // &
                "360/INTERVAL+1) where INTERVAL is ", interval
        print*, "Input array has dimension ", size(geoid(:,1)), size(geoid(1,:))
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    else if (size(cilm(:,1,1)) < 2) then
        print*, "Error --- MakeGeoidGrid"
        print*, "CILM must be dimensioned as (2, *, *)."
        print*, "Input dimension is ", size(cilm(:,1,1)), size(cilm(1,:,1)), &
                size(cilm(1,1,:))
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    end if

    if (present(lmax_calc)) then
        lmax_comp = min(lmax, size(cilm(1,1,:))-1, size(cilm(1,:,1))-1, &
                        lmax_calc)

    else
        lmax_comp = min(lmax, size(cilm(1,1,:))-1, size(cilm(1,:,1))-1)

    end if

    if (gridtype == 1) then
        allocate (zero(lmax+1), stat = astat)

        if (astat /= 0) then
            print*, "Error --- MakeGeoidGrid"
            print*, "Problem allocating array ZERO, ", astat
            if (present(exitstatus)) then
                exitstatus = 3
                return
            else
                stop
            end if

        end if

        allocate (w(lmax+1), stat = astat)

        if (astat /= 0) then
            print*, "Error --- MakeGeoidGrid"
            print*, "Problem allocating array W, ", astat
            if (present(exitstatus)) then
                exitstatus = 3
                return
            else
                stop
            end if

        end if

        if (present(exitstatus)) then
            call SHGLQ(lmax, zero, w, norm = 1, csphase = 1, &
                       exitstatus = exitstatus)
            if (exitstatus /= 0) return
        else
            call SHGLQ(lmax, zero, w, norm = 1, csphase = 1)
        end if

    end if

    allocate (cilm1(2,lmax+1,lmax+1), stat = astat1)
    allocate (cilm2(2,lmax+1,lmax+1), stat = astat2)
    if (astat1 /= 0 .or. astat2 /= 0) then
        print*, "Error --- MakeGeoidGrid"
        print*, "Problem allocating arrays CILM1 and CILM2", astat1, astat2
        if (present(exitstatus)) then
             exitstatus = 3
             return
        else
             stop
        end if

    end if

    pi = acos(-1.0_dp)

    cilm1(1:2,1:lmax_comp+1, 1:lmax_comp+1) = cilm(1:2,1:lmax_comp+1, &
                                                   1:lmax_comp+1)
    cilm1(1,1,1) = 1.0_dp    ! Make sure that the degree-0 term is included

    !--------------------------------------------------------------------------
    !
    !   Solve the equation a + b x + c x**2 + d x**3 = 0
    !   for each grid point.
    !
    !--------------------------------------------------------------------------
    ! Create grid A.
    do l = 0, lmax_comp
        cilm2(1:2,l+1,1:l+1) = (GM / r) * cilm1(1:2,l+1,1:l+1) * (r0pot / r)**l
    end do

    ! add rotation and subtract reference field
    cilm2(1,1,1) = cilm2(1,1,1) + (omega * r)**2 / 3.0_dp - PotRef
    cilm2(1,3,1) = cilm2(1,3,1) - (omega * r)**2 / (3.0_dp * sqrt(5.0_dp))

    allocate (grida(nlat, nlong), stat = astat)
    if (astat /= 0) then
        print*, "Error --- MakeGeoidGrid"
        print*, "Problem allocating array GRIDA", astat
        if (present(exitstatus)) then
            exitstatus = 3
            return
        else
            stop
         end if

    end if

    select case (gridtype)
        case (1)
            if (present(exitstatus)) then
                call MakeGridGLQ(grida, cilm2, lmax, zero=zero, norm=1, &
                                 csphase=1, lmax_calc=lmax_comp, &
                                 exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridGLQ(grida, cilm2, lmax, zero=zero, norm=1, &
                                 csphase=1, lmax_calc=lmax_comp)
            end if

        case (2)
            if (present(exitstatus)) then
                call MakeGridDH(grida, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=1, lmax_calc = lmax_comp, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridDH(grida, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=1, lmax_calc = lmax_comp)
            end if

        case (3)
            if (present(exitstatus)) then
                call MakeGridDH(grida, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=2, lmax_calc = lmax_comp, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridDH(grida, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=2, lmax_calc = lmax_comp)
            end if

        case (4)
            if (present(exitstatus)) then
                call MakeGrid2D(grida, cilm2, lmax_comp, interval, nlat1, &
                                nlong1, norm=1, csphase=1, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGrid2D(grida, cilm2, lmax_comp, interval, nlat1, &
                                nlong1, norm=1, csphase=1)
            end if

    end select

    ! Create Grid B
    do l = 0, lmax_comp
        cilm2(1:2,l+1,1:l+1) = -(GM / r**2) * dble(l+1) &
                               * cilm1(1:2,l+1,1:l+1) * (r0pot / r)**l
    end do

    ! add rotational terms
    cilm2(1,1,1) = cilm2(1,1,1) + 2.0_dp * r * omega**2 / 3.0_dp
    cilm2(1,3,1) = cilm2(1,3,1) - 2.0_dp * r * omega**2 / (3.0_dp * sqrt(5.0_dp))

    allocate (gridb(nlat, nlong), stat = astat)
    if (astat /= 0) then
        print*, "Error --- MakeGeoidGrid"
        print*, "Problem allocating array GRIDB", astat
        if (present(exitstatus)) then
            exitstatus = 3
            return
        else
            stop
        end if

    end if

    select case (gridtype)
        case (1)
            if (present(exitstatus)) then
                call MakeGridGLQ(gridb, cilm2, lmax, zero=zero, norm=1, &
                                 csphase=1, lmax_calc=lmax_comp, &
                                 exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridGLQ(gridb, cilm2, lmax, zero=zero, norm=1, &
                                 csphase=1, lmax_calc=lmax_comp)
            end if

        case (2)
            if (present(exitstatus)) then
                call MakeGridDH(gridb, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=1, lmax_calc = lmax_comp, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridDH(gridb, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=1, lmax_calc = lmax_comp)
            end if

        case (3)
            if (present(exitstatus)) then
                call MakeGridDH(gridb, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=2, lmax_calc = lmax_comp, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridDH(gridb, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=2, lmax_calc = lmax_comp)
            end if

        case (4)
            if (present(exitstatus)) then
                call MakeGrid2D(gridb, cilm2, lmax_comp, interval, nlat1, &
                                nlong1, norm=1, csphase=1, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGrid2D(gridb, cilm2, lmax_comp, interval, nlat1, &
                                nlong1, norm=1, csphase=1)
            end if

    end select

    ! Create Grid C
    if (order == 2 .or. order == 3) then

        do l = 0, lmax_comp
            cilm2(1:2,l+1,1:l+1) = GM / (2.0_dp * r**3) * dble(l+1) &
                                   * dble(l+2) * cilm1(1:2,l+1,1:l+1) &
                                   * (r0pot / r)**l
        end do

        ! add rotational terms
        cilm2(1,1,1) = cilm2(1,1,1) + omega**2 / 3.0_dp
        cilm2(1,3,1) = cilm2(1,3,1) - omega**2 / (3.0_dp * sqrt(5.0_dp))

        allocate (gridc(nlat, nlong), stat = astat)
        if (astat /= 0) then
            print*, "Error --- MakeGeoidGrid"
            print*, "Problem allocating array GRIDC", astat
            if (present(exitstatus)) then
                exitstatus = 3
                return
            else
                stop
            end if

        end if

    select case (gridtype)
        case (1)
            if (present(exitstatus)) then
                call MakeGridGLQ(gridc, cilm2, lmax, zero=zero, norm=1, &
                                 csphase=1, lmax_calc=lmax_comp, &
                                 exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridGLQ(gridc, cilm2, lmax, zero=zero, norm=1, &
                                 csphase=1, lmax_calc=lmax_comp)
            end if

        case (2)
            if (present(exitstatus)) then
                call MakeGridDH(gridc, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=1, lmax_calc = lmax_comp, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridDH(gridc, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=1, lmax_calc = lmax_comp)
            end if

        case (3)
            if (present(exitstatus)) then
                call MakeGridDH(gridc, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=2, lmax_calc = lmax_comp, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridDH(gridc, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=2, lmax_calc = lmax_comp)
            end if

        case (4)
            if (present(exitstatus)) then
                call MakeGrid2D(gridc, cilm2, lmax_comp, interval, nlat1, &
                                nlong1, norm=1, csphase=1, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGrid2D(gridc, cilm2, lmax_comp, interval, nlat1, &
                                nlong1, norm=1, csphase=1)
            end if

    end select

    end if

    ! Create Grid D
    if (order == 3) then

        do l = 0, lmax_comp
            cilm2(1:2,l+1,1:l+1) = -GM / (6.0_dp * r**4) * dble(l+1) &
                                   * dble(l+2) * dble(l+3) * &
                                    cilm1(1:2,l+1,1:l+1) * (r0pot / r)**l
        end do

        allocate (gridd(nlat, nlong), stat = astat)
        if (astat /= 0) then
            print*, "Error --- MakeGeoidGrid"
            print*, "Problem allocating array GRIDD", astat
            if (present(exitstatus)) then
                exitstatus = 3
                return
            else
                stop
            end if

        end if

    select case (gridtype)
        case (1)
            if (present(exitstatus)) then
                call MakeGridGLQ(gridd, cilm2, lmax, zero=zero, norm=1, &
                                 csphase=1, lmax_calc=lmax_comp, &
                                 exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridGLQ(gridd, cilm2, lmax, zero=zero, norm=1, &
                                 csphase=1, lmax_calc=lmax_comp)
            end if

        case (2)
            if (present(exitstatus)) then
                call MakeGridDH(gridd, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=1, lmax_calc = lmax_comp, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridDH(gridd, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=1, lmax_calc = lmax_comp)
            end if

        case (3)
            if (present(exitstatus)) then
                call MakeGridDH(gridd, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=2, lmax_calc = lmax_comp, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGridDH(gridd, n, cilm2, lmax, norm=1, csphase=1, &
                                sampling=2, lmax_calc = lmax_comp)
            end if

        case (4)
            if (present(exitstatus)) then
                call MakeGrid2D(gridd, cilm2, lmax_comp, interval, nlat1, &
                                nlong1, norm=1, csphase=1, &
                                exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call MakeGrid2D(gridd, cilm2, lmax_comp, interval, nlat1, &
                                nlong1, norm=1, csphase=1)
            end if

    end select

    end if

    ! solve equation
    if (order == 1) then
        geoid(1:nlat,1:nlong) = -grida(1:nlat,1:nlong) / gridb(1:nlat,1:nlong)

    else if (order == 2) then
        geoid(1:nlat, 1:nlong) = (-gridb(1:nlat, 1:nlong) - &
                                  sqrt(gridb(1:nlat, 1:nlong)**2 &
                                  - 4.0_dp * gridc(1:nlat,1:nlong) * &
                                  grida(1:nlat, 1:nlong)) ) &
                                  / (2.0_dp * gridc(1:nlat,1:nlong))

    else if (order == 3) then
        allocate (qq(nlat, nlong), stat = astat)
        if (astat /= 0) then
            print*, "Error --- MakeGeoidGrid"
            print*, "Problem allocating array QQ", astat
            if (present(exitstatus)) then
                exitstatus = 3
                return
            else
                stop
            end if

        end if

        allocate (pp(nlat, nlong), stat = astat)
        if (astat /= 0) then
            print*, "Error --- MakeGeoidGrid"
            print*, "Problem allocating array PP", astat
            if (present(exitstatus)) then
                exitstatus = 3
                return
            else
                stop
            end if

        end if

        allocate (uu(nlat, nlong), stat = astat)
        if (astat /= 0) then
            print*, "Error --- MakeGeoidGrid"
            print*, "Problem allocating array UU", astat
            if (present(exitstatus)) then
                exitstatus = 3
                return
            else
                stop
            end if

        end if

        pp(1:nlat,1:nlong) = gridb(1:nlat,1:nlong) / gridd(1:nlat,1:nlong) &
                             - ((gridc(1:nlat,1:nlong) &
                             / gridd(1:nlat,1:nlong))**2) / 3.0_dp
        qq(1:nlat,1:nlong) = grida(1:nlat,1:nlong) / gridd(1:nlat,1:nlong) &
             + 2.0_dp * ((gridc(1:nlat,1:nlong) / gridd(1:nlat,1:nlong))**3) &
             / 27.0_dp - 9.0_dp * gridc(1:nlat,1:nlong) * gridb(1:nlat,1:nlong) &
             / (gridd(1:nlat,1:nlong)**2) / 27.0_dp
        uu(1:nlat,1:nlong) = (qq(1:nlat,1:nlong) / 2.0_dp &
                             + sqrt((qq(1:nlat,1:nlong)**2) / 4.0_dp &
                             + (pp(1:nlat,1:nlong)**3) / 27.0_dp) )**(1.0_dp / 3.0_dp)
        geoid(1:nlat,1:nlong) = pp(1:nlat,1:nlong) / 3.0_dp &
                                / uu(1:nlat,1:nlong) - uu(1:nlat,1:nlong) &
                                - gridc(1:nlat,1:nlong) &
                                / gridd(1:nlat,1:nlong) / 3.0_dp

        deallocate (qq)
        deallocate (pp)
        deallocate (uu)

    end if

    !--------------------------------------------------------------------------
    !
    !   Reference geoid to a flattened ellipsoid
    !
    !--------------------------------------------------------------------------
    if (present(a) .and. present(f)) then

        do i = 1, nlat
            if (gridtype == 4) then
                lat = 90.0_dp - dble(i-1) * interval

            else if (gridtype == 1) then
                lat = asin(zero(i)) * 180.0_dp / pi

            else if (gridtype == 2 .or. gridtype == 3) then
                lat = 90.0_dp - 180.0_dp * dble(i-1) / dble(nlat)

            end if

            r_ex = a**2 * (1.0_dp + tan(lat * pi / 180.0_dp)**2) &
                   / (1.0_dp + tan(lat * pi / 180.0_dp)**2 / (1.0_dp - f)**2)
            r_ex = sqrt(r_ex)

            geoid(i,1:nlong) = geoid(i,1:nlong) + r - r_ex

        end do

    end if

    !--------------------------------------------------------------------------
    !
    !   Clean up
    !
    !--------------------------------------------------------------------------
    if (gridtype == 2) then
        deallocate(zero)
        deallocate(w)
    end if

    deallocate (grida)
    deallocate (gridb)
    if (order == 2) deallocate (gridc)

    if (order == 3) then
        deallocate (gridc)
        deallocate (gridd)
    end if

    deallocate (cilm1)
    deallocate (cilm2)

end subroutine MakeGeoidGrid


real :: x,dx,a,b
integer :: i
x = 1.0
dx = 1.0
do i = 1, 30
     a = (x + dx)**2 + 5*(x+dx) + 1
     b = x**2 + 5 * x + 1
     print*, (a-b)/dx
     dx = dx/2
end do
stop
end

4670 Numerical Analysis/Class Examples/der.f90


! =============================================================================
!                       Test ellipsoid merge
!
!         This unit test checks the merging of 9 ellipsoids. The biggest
!         ellipsoid is located at the origin. The smaller ellipsoids are located
!         on all sides. The final ellipsoid is a circle located at the
!         origin.
! =============================================================================
program test_ellipsoid_merge_1
    use unit_test
    use constants, only : pi, three, four, five, eight, ten, f23
    use parcel_container
    use parcel_merge, only : merge_parcels, merge_timer
    use options, only : parcel
    use parameters, only : update_parameters, lower, extent, nx, ny, nz
    use parcel_ellipsoid
    use parcel_nearest
    use timer
    implicit none

    double precision :: a1b1c1, a2b2c2, error

    nx = 1
    ny = 1
    nz = 1
    lower  = (/-pi / two, -pi /two, -pi /two/)
    extent = (/pi, pi, pi/)

    call register_timer('parcel merge', merge_timer)
    call register_timer('merge nearest', merge_nearest_timer)
    call register_timer('merge tree resolve', merge_tree_resolve_timer)

    parcel%lambda_max = five
    parcel%min_vratio = ten

    call update_parameters

    call parcel_alloc(9)

    a1b1c1 = 0.49d0
    a2b2c2 = 0.25d0

    call parcel_setup

    call merge_parcels(parcels)

    ! check result
    error = eval_max_error()

    call print_result_dp('Test ellipsoid merge 1', error, atol=dble(2.0e-15))

    call parcel_dealloc

    contains

        subroutine parcel_setup
            integer :: i

            n_parcels = 9
            parcels%position(:, 1) = zero
            parcels%volume(1) = a1b1c1 * four * pi / three
            parcels%B(:, 1) = zero
            parcels%B(1, 1) = a1b1c1 ** f23
            parcels%B(4, 1) = a1b1c1 ** f23
            parcels%buoyancy(1) = 1.5d0
#ifndef ENABLE_DRY_MODE
            parcels%humidity(1) = 1.3d0
#endif

            do i = 0, 1
                ! small parcel below front/back left
                parcels%position(1, 2+4*i) = -0.6d0
                parcels%position(2, 2+4*i) = -0.6d0 + 1.2d0 * dble(i)
                parcels%position(3, 2+4*i) = -0.6d0
                parcels%volume(2+4*i) = a2b2c2 * four * pi / three
                parcels%B(:, 2+4*i) = zero
                parcels%B(1, 2+4*i) = a2b2c2 ** f23
                parcels%B(4, 2+4*i) = a2b2c2 ** f23
                parcels%buoyancy(2+4*i) = 1.8d0
#ifndef ENABLE_DRY_MODE
                parcels%humidity(2+4*i) = 1.2d0
#endif
                ! small parcel below front/back right
                parcels%position(1, 3+4*i) =  0.6d0
                parcels%position(2, 3+4*i) = -0.6d0 + 1.2d0 * dble(i)
                parcels%position(3, 3+4*i) = -0.6d0
                parcels%volume(3+4*i) = a2b2c2 * four * pi / three
                parcels%B(:, 3+4*i) = zero
                parcels%B(1, 3+4*i) = a2b2c2 ** f23
                parcels%B(4, 3+4*i) = a2b2c2 ** f23
                parcels%buoyancy(3+4*i) = 1.4d0
#ifndef ENABLE_DRY_MODE
                parcels%humidity(3+4*i) = 1.1d0
#endif
                ! small parcel above front/back left
                parcels%position(1, 4+4*i) = -0.6d0
                parcels%position(2, 4+4*i) = -0.6d0 + 1.2d0 * dble(i)
                parcels%position(3, 4+4*i) =  0.6d0
                parcels%volume(4+4*i) = a2b2c2 * four * pi / three
                parcels%B(:, 4+4*i) = zero
                parcels%B(1, 4+4*i) = a2b2c2 ** f23
                parcels%B(4, 4+4*i) = a2b2c2 ** f23
                parcels%buoyancy(4+4*i) = 1.7d0
#ifndef ENABLE_DRY_MODE
                parcels%humidity(4+4*i) = 1.0d0
#endif
                ! small parcel above front/back right
                parcels%position(1, 5+4*i) =  0.6d0
                parcels%position(2, 5+4*i) = -0.6d0 + 1.2d0 * dble(i)
                parcels%position(3, 5+4*i) =  0.6d0
                parcels%volume(5+4*i) = a2b2c2 * four * pi / three
                parcels%B(:, 5+4*i) = zero
                parcels%B(1, 5+4*i) = a2b2c2 ** f23
                parcels%B(4, 5+4*i) = a2b2c2 ** f23
                parcels%buoyancy(5+4*i) = 1.5d0
#ifndef ENABLE_DRY_MODE
                parcels%humidity(5+4*i) = 1.4d0
#endif
            enddo
        end subroutine parcel_setup

        function eval_max_error() result(max_err)
            double precision :: abc, B11, B12, B13, B22, B23, B33, vol, buoy
#ifndef ENABLE_DRY_MODE
            double precision :: hum
#endif
            double precision :: max_err

            ! reference solution
            abc = a1b1c1 + eight * a2b2c2  ! a == b == c since it is a sphere
            B11 = abc ** f23
            B12 = zero
            B13 = zero
            B22 = abc ** f23
            B23 = zero
            B33 = abc ** f23
            vol = abc * four * pi / three

            buoy = (1.5d0 * a1b1c1 + two * (1.8d0 + 1.4d0 + 1.7d0 + 1.5d0) * a2b2c2) / abc
#ifndef ENABLE_DRY_MODE
            hum  = (1.3d0 * a1b1c1 + two * (1.2d0 + 1.1d0 + 1.0d0 + 1.4d0) * a2b2c2) / abc
#endif
            max_err = zero

            max_err = max(max_err, abs(dble(n_parcels - 1)))
            max_err = max(max_err, abs(parcels%B(1, 1) - B11))
            max_err = max(max_err, abs(parcels%B(2, 1) - B12))
            max_err = max(max_err, abs(parcels%B(3, 1) - B13))
            max_err = max(max_err, abs(parcels%B(4, 1) - B22))
            max_err = max(max_err, abs(parcels%B(5, 1) - B23))
            max_err = max(max_err, abs(get_B33(parcels%B(:, 1), &
                                               parcels%volume(1)) - B33))
            max_err = max(max_err, sum(abs(parcels%position(:, 1))))
            max_err = max(max_err, abs(parcels%volume(1) - vol))
            max_err = max(max_err, abs(parcels%buoyancy(1) - buoy))
#ifndef ENABLE_DRY_MODE
            max_err = max(max_err, abs(parcels%humidity(1) - hum))
#endif
        end function eval_max_error

end program test_ellipsoid_merge_1


module diag_tests
  use test_params
  implicit none
contains

  ! Diag tests
  Subroutine test_diag_real()

!!$    Use mpi, Only : mpi_bcast, mpi_comm_world, mpi_double_precision
    Use mpi, Only : mpi_comm_world, mpi_double_precision

    Use numbers_module           , Only : wp
    Use distributed_matrix_module, Only : distributed_matrix, real_distributed_matrix, &
      distributed_matrix_init, distributed_matrix_comm_to_base, distributed_matrix_finalise, &
      distributed_matrix_set_default_blocking

    Implicit None

    Type( real_distributed_matrix ) :: base
    Class( distributed_matrix ), Allocatable :: Am, Qm, QTm, Cm
    Real( wp ), Dimension( : ) , Allocatable :: E

    Real( wp ), Dimension( :, : ), Allocatable :: A
    Real( wp ), Dimension( :, : ), Allocatable :: tmp

    Integer :: n
    Integer :: i

    n = m

    Allocate( A( 1:n, 1:n ) )
    If( me == 0 ) Then
      Call Random_number( A )
      A = A + Transpose( A )
    End If
    Call mpi_bcast( A, Size( A ), mpi_double_precision, 0, mpi_comm_world, error )

    Call distributed_matrix_init
    Call distributed_matrix_set_default_blocking( n_block )
    Call distributed_matrix_comm_to_base( mpi_comm_world, base )
    Allocate( real_distributed_matrix :: Am )
    Allocate( real_distributed_matrix :: Qm )
    Call Am%create( n, n, base )
    Call Am%set_by_global( 1, n, 1, n, A )

    Call Am%diag( Qm, E )

    QTm = .Dagger. Qm
    Cm = QTm * Am * Qm
    Allocate( tmp( 1:n, 1:n ) )
    Call Cm%get_by_global( 1, n, 1, n, tmp )
    Do i = 1, n
      tmp( i, i ) = tmp( i, i ) - E( i )
    End Do

    If( me == 0 ) Then
      Write( *, error_format ) 'Error in real diag ', Maxval( Abs( tmp ) ), &
        Merge( passed, FAILED, Maxval( Abs( tmp ) ) < tol )
    End If

  End Subroutine test_diag_real

  Subroutine test_diag_complex()

!!$    Use mpi, Only : mpi_bcast, mpi_comm_world, mpi_double_complex
    Use mpi, Only : mpi_comm_world, mpi_double_complex

    Use numbers_module           , Only : wp
    Use distributed_matrix_module, Only : distributed_matrix, real_distributed_matrix, complex_distributed_matrix, &
      distributed_matrix_init, distributed_matrix_comm_to_base, distributed_matrix_finalise, &
      distributed_matrix_set_default_blocking

    Implicit None

    Type( real_distributed_matrix ) :: base
    Class( distributed_matrix ), Allocatable :: Am, Qm, QTm, Cm
    Real( wp ), Dimension( : ) , Allocatable :: E

    Complex( wp ), Dimension( :, : ), Allocatable :: A
    Complex( wp ), Dimension( :, : ), Allocatable :: tmp

    Real( wp ), Dimension( :, : ), Allocatable :: rand1, rand2

    Integer :: n
    Integer :: i

    n = m

    Allocate( A( 1:n, 1:n ) )
    If( me == 0 ) Then
      Allocate( rand1( 1:n, 1:n ), rand2( 1:n, 1:n ) )
      Call Random_number( rand1 ); Call Random_number( rand2 )
      A = Cmplx( rand1, rand2, wp )
      A = A + Conjg( Transpose( A ) )
    End If
    Call mpi_bcast( A, Size( A ), mpi_double_complex, 0, mpi_comm_world, error )

    Call distributed_matrix_init
    Call distributed_matrix_set_default_blocking( n_block )
    Call distributed_matrix_comm_to_base( mpi_comm_world, base )
    Allocate( complex_distributed_matrix :: Am )
    Allocate( complex_distributed_matrix :: Qm )
    Call Am%create( n, n, base )
    Call Am%set_by_global( 1, n, 1, n, A )

    Call Am%diag( Qm, E )

    QTm = .Dagger. Qm
    Cm = QTm * Am * Qm
    Allocate( tmp( 1:n, 1:n ) )
    Call Cm%get_by_global( 1, n, 1, n, tmp )
    Do i = 1, n
      tmp( i, i ) = tmp( i, i ) - E( i )
    End Do

    If( me == 0 ) Then
      Write( *, error_format ) 'Error in complex diag ', Maxval( Abs( tmp ) ), &
        Merge( passed, FAILED, Maxval( Abs( tmp ) ) < tol )
    End If

  End Subroutine test_diag_complex
end module diag_tests


*
*     -------------------------------------------------------------
*      S I X J 4
*     -------------------------------------------------------------
*
*                                                                  *
*     THIS PACKAGE DETERMINES THE VALUES OF 6j COEFFICIENT         *
*                                                                  *
*     | JC/2  JE/2  JD/2 |                                         *
*     | JB/2  JF/2    4  |                                         *
*                                                                  *
*                                                                  *
*     Written by G. Gaigalas,                                      *
*     Vanderbilt University,  Nashville             October 1996   *
*
*
      SUBROUTINE SIXJ4(JC,JE,JD,JB,JF,ITIK,SI)
      IMPLICIT DOUBLEPRECISION (A-H,O-Z)
      COMMON/CONSTS/ZERO,TENTH,HALF,ONE,TWO,THREE,FOUR,SEVEN,ELEVEN,EPS
      SI=ZERO
      IF(ITIK.NE.0) THEN
C
C     CHESKED TRIANGULAR CONDITIONS
C
        IF(IXJTIK(JC,JE,JD,JB,JF,8).EQ.0)RETURN
      ENDIF
      IF(IXJTIK(JC,JE,JD,JB,JF,6).EQ.0) THEN
	CALL GRACAH1(JC,JE,JF,JB,JD,8,SI)
	IF(MOD(JC+JE+JF+JB,4).NE.0) SI=-SI
      ELSE
        A=THREE
        C=DBLE(JC)*HALF
        E=DBLE(JE)*HALF
        D=DBLE(JD)*HALF
        B=DBLE(JB)*HALF
        F=DBLE(JF)*HALF
        X1=A*DSQRT((A+B+E+TWO)*(A-B+E+ONE)*(A+B-E+ONE)*(-A+B+E)*
     :  (A+C+F+TWO)*(A-C+F+ONE)*(A+C-F+ONE)*(-A+C+F))
        X2=(A+ONE)*DSQRT((A+B+E+ONE)*(A-B+E)*(A+B-E)*(-A+B+E+ONE)*
     :  (A+C+F+ONE)*(A-C+F)*(A+C-F)*(-A+C+F+ONE))
        X3=(TWO*A+ONE)*(TWO*(A*(A+ONE)*D*(D+ONE)-B*(B+ONE)*C*(C+ONE)-
     :  E*(E+ONE)*F*(F+ONE))+
     : (A*(A+ONE)-B*(B+ONE)-E*(E+ONE))*(A*(A+ONE)-C*(C+ONE)-F*(F+ONE)))
        IF(DABS(X2).LT.EPS) THEN
	  S2=ZERO
        ELSE
          CALL SIXJ2(JC,JE,JD,JB,JF,0,S2)
        ENDIF
        IF(DABS(X3).LT.EPS) THEN
	  S3=ZERO
        ELSE
          CALL SIXJ3(JC,JE,JD,JB,JF,0,S3)
        ENDIF
        SI=(X3*S3-X2*S2)/X1
      ENDIF
      RETURN
      END