Module RootFMM

!******************************************************************************
! Procedury Zeroin, BrentZero z knihy
! "Computer Methods for Mathematical  Computations",
! by George E. Forsythe, Michael A. Malcolm, and Cleve B. Moler
! Prentice-Hall, 1977.
!
! Prepis pro Fortran 90:
! Last updated: 28 February 2005 by Ralph Carmichael ralph@pdas.com 
! Public Domain Aeronautical Software 
! P.O. Box 1438 Santa Cruz CA 95061 USA 
!
! Drobne upravy pro g95
! Jan Cely, UFKL,Prirodovedecka fakulta MU, leden 2007,unor 2008
!******************************************************************************
!
!!=============================================================================
!!	FUNCTION Zeroin(a,b,F,tol) result(z)
!!		Vraci koren z rovnice F(x)=0 v intervalu <a,b> s presnosti tol
!!	VSTUP
!!	a, b	okraje intervalu v nemz se hleda koren F(x)=0
!!	F		funkce F(x) deklarovana takto
!!				FUNCTION F(x) RESULT(g)
!!					IMPLICIT NONE
!!					INTEGER,PARAMETER :: FP = selected_real_kind(X)
!!					REAL(FP),INTENT(IN):: x
!!					REAL(FP):: g
!!				END Function F
!!			kde FP = SP|DP je-li X = 4|8
!!	tol		velikost intervalu v nemz se pozaduje lokalizace korene
!!-----------------------------------------------------------------------------
!!	SUBROUTINE BrentZero(ax,bx,F,tol,maxIter,neval,errCode,xZero,fZero)
!!		Brentova modifikace Zeroin (proceduru vola i Zeroin)
!!	VSTUP
!!	ax, bx		viz a, b v Zeroin
!!	F			viz F v Zeroin
!!	tol			viz tol v Zeroin
!!	maxIter		maximalni povoleny pocet iteraci, dobra hodnota je 25
!!	VYSTUP
!!	neval		INTEGER, zde se nevyuziva, musi byt ale deklarovana a dosazena
!!	errCode		INTEGER, 0 = OK, 1 = prilis mnoho iteraci, 2 = F(ax)*F(bx)>0
!!	xZero		REAL(FP), nalezena hodnota korene, FP = SP|DP
!!	fZero		REAL(FP), F(xZero)
!!=============================================================================

implicit none

integer,parameter :: SP = selected_real_kind(4)
integer,parameter :: DP = selected_real_kind(8)

character(len=20),parameter,public :: RootFMMVer = "1.0"


private

PUBLIC :: Zeroin,BrentZero
private :: ZeroinSingle,BrentZeroSingle,ZeroinDouble,BrentZeroDouble

INTERFACE Zeroin
	MODULE PROCEDURE ZeroinSingle,ZeroinDouble
END INTERFACE
  
INTERFACE BrentZero
	MODULE PROCEDURE BrentZeroSingle,BrentZeroDouble
END INTERFACE

contains

FUNCTION ZeroinSingle(a,b,F,tol) result(z)
! ---------------------------------------------------------------------------
! PURPOSE - Compute a zero of f in the interval (a,b)
	
	REAL(SP),INTENT(IN):: a,b   ! left and right endpoints of interval
	REAL(SP),INTENT(IN):: tol   ! desired interval of uncertainity 
    REAL(SP):: z
	INTERFACE
		FUNCTION F(x) RESULT(g)
			IMPLICIT NONE
			integer,parameter :: SP = selected_real_kind(4)
			REAL(SP),INTENT(IN):: x
			REAL(SP):: g
		END Function F
	END INTERFACE    
  
  	INTEGER:: errCode
  	INTEGER,PARAMETER:: MAXITER=25
  	INTEGER:: neval   ! not used
  	REAL(SP):: xZero,fZero
	!------------------------------------------------------------------------
  	CALL BrentZeroSingle(a,b,F,tol,MAXITER,neval,errCode,xZero,fZero)
  	z=xZero
	RETURN
END Function ZeroinSingle   
! -----------------------------------------------



SUBROUTINE BrentZeroSingle(ax,bx,F,tol,maxIter,neval,errCode,xZero,fZero)
! ---------------------------------------------------------------------------
! PURPOSE - Compute a zero of F in the interval (ax,bx)

  REAL(SP),INTENT(IN):: ax,bx   ! left and right endpoints of interval
  REAL(SP),INTENT(IN):: tol     ! desired interval of uncertainity 
  INTEGER,INTENT(IN):: maxIter  ! max number of iterations allowed. 25 is good
  INTEGER,INTENT(OUT):: neval
  INTEGER,INTENT(OUT):: errCode   ! =0 is OK; =1 too many iterations
                                  ! =2 if F(ax) and F(bx) have the same sign
  REAL(SP),INTENT(OUT):: xZero,fZero! the last and best value of the zero                                   
      

	INTERFACE
	  FUNCTION F(x) RESULT(g)
		IMPLICIT NONE
		integer,parameter :: SP = selected_real_kind(4)
		REAL(SP),INTENT(IN):: x
		REAL(SP):: g
	  END Function F
	END INTERFACE    

	REAL(SP):: a,b,c,d,e,eps
	REAL(SP):: fa,fb,fc,tol1
	INTEGER:: kIter
	!  INTEGER:: method   ! =0 bisection; =1 linear; =2 inverse quadratic
	REAL(SP):: xm,p,q,r,s
	REAL(SP),PARAMETER:: ZERO=0.0, ONE=1.0, TWO=2.0, THREE=3.0, HALF=0.5
!----------------------------------------------------------------------------
  eps=EPSILON(ax)
  tol1=ONE+eps

  a=ax   ! initialization
  b=bx
  fa=f(a)
  fb=f(b)
  neval=2
  IF (fa==ZERO) THEN
    xZero=a
    fZero=ZERO
    errCode=0
    RETURN
  END IF  
  IF (fb==ZERO) THEN
    xZero=b
    fZero=ZERO
    errCode=0
    RETURN
  END IF  
  IF (fa*fb > ZERO) THEN
    xZero=HUGE(a)
    fZero=HUGE(a)
    errCode=2
    RETURN
  END IF
! The trivial cases have now been dealt with. On to the real work...

  c=a
  fc=fa
  d=b-a
  e=d
    
  DO kIter=1,maxIter
    IF ( (fb>0 .AND. fc>0) .OR. (fb<0 .AND. fc<0) ) THEN
      c=a  ! we insist that b and c straddle the zero
      fc=fa
      d=b-a
      e=d
    END IF

    IF (ABS(fc) < ABS(fb)) THEN
      a=b    ! we insist that b be the better guess of b and c
      b=c
      c=a
      fa=fb
      fb=fc
      fc=fa
    END IF

    tol1=TWO*eps*ABS(b)+HALF*tol   ! convergence test
    xm=HALF*(c-b)
    IF (ABS(xm) <= tol1 .OR. fb==ZERO) THEN
      xZero=b
      fZero=fb
      errCode=0   ! SUCCESS! The proper way to leave
      RETURN
    END IF

!    WRITE(DBG,*) "BrentZero, start of kIter=",kIter
!    WRITE(DBG,'(A,2ES25.16)') "a,fa=", a,fa
!    WRITE(DBG,'(A,2ES25.16)') "b,fb=", b,fb
!    WRITE(DBG,'(A,2ES25.16)') "c,fc=", c,fc
!    WRITE(DBG,'(A,2ES25.16)') "xm,tol1=", xm,tol1

    IF (ABS(e) < tol1 .OR. ABS(fa) <= ABS(fb) ) THEN
      d=xm   ! bisection
      e=d
!      method=0
    ELSE
      IF (a==c) THEN
        s=fb/fa   ! linear interpolation
        p=TWO*xm*s
        q=ONE-s
!        method=1
      ELSE
        q=fa/fc   ! inverse quadratic interpolation
        r=fb/fc
        s=fb/fa
        p=s*(TWO*xm*q*(q-r)-(b-a)*(r-ONE))
        q=(q-ONE)*(r-ONE)*(s-ONE)
!        method=2
      END IF
      IF (p > ZERO) q=-q   ! adjust signs
      p=ABS(p)
      IF (p+p >= (THREE*xm*q-ABS(tol1*q)) .OR. p+p >= ABS(e*q) ) THEN
        d=xm   ! don't interpolate. Use bisection
        e=d
 !       method=-1
      ELSE
        e=d   ! OK, use interpolation
        d=p/q
      END IF
    END IF  
   
    a=b   ! complete step. a becomes the previous iteration
    fa=fb
   IF (ABS(d) > tol1) THEN
     b=b+d
   ELSE  
     b=b+SIGN(tol1,xm)
   END IF
     
   fb=F(b)   ! the newest and best value (we hope)
   neval=neval+1   ! keep count of the function evaluations
!!   IF ((fb*(fc/ABS(fc))) > ZERO) GO TO 20
!    WRITE(DBG,*) "BrentZero, end of kIter=",kIter, "   method=",method
!    WRITE(DBG,'(A,2ES25.16)') " new b,fb=", b,fb
!    WRITE(DBG,'(A,2ES25.16)') "d,e=", d,e
!    WRITE(DBG,'(A,2ES25.16)') "p,q=", p,q
  END DO
  
! The loop should never terminate. If it does, return the last iteration
!  and set errCode to 1
  xZero=b
  fZero=fb
  errCode=1
  RETURN  

END Subroutine BrentZeroSingle
!-----------------------------------------------------------------------------

!=============================== DP ==========================================

FUNCTION ZeroinDouble (a,b,F,tol) RESULT(z)
! ---------------------------------------------------------------------------
! PURPOSE - Compute a zero of f in the interval (a,b)

  REAL(DP),INTENT(IN):: a,b   	! left and right endpoints of interval
  REAL(DP),INTENT(IN):: tol     ! desired interval of uncertainity 
      
  REAL(DP):: z
INTERFACE
  FUNCTION F(x) RESULT(g)
	IMPLICIT NONE
	integer,parameter :: DP = selected_real_kind(8)
    REAL(DP),INTENT(IN):: x
    REAL(DP):: g
  END Function F
END INTERFACE    
  
  INTEGER:: errCode
  INTEGER,PARAMETER:: MAXITER=25
  INTEGER:: neval   ! not used
  REAL(DP):: xZero,fZero
!----------------------------------------------------------------------------
  CALL BrentZeroDouble(a,b,F,tol,MAXITER,neval,errCode,xZero,fZero)
  z=xZero
  RETURN
END Function ZeroinDouble   
!----------------------------------------------------------------------------

!+
SUBROUTINE BrentZeroDouble(ax,bx,F,tol,maxIter,neval,errCode,xZero,fZero)
! ---------------------------------------------------------------------------
! PURPOSE - Compute a zero of F in the interval (ax,bx)

  REAL(DP),INTENT(IN):: ax,bx   ! left and right endpoints of interval
  REAL(DP),INTENT(IN):: tol     ! desired interval of uncertainity 
  INTEGER,INTENT(IN):: maxIter   ! max number of iterations allowed. 25 is good
  INTEGER,INTENT(OUT):: neval
  INTEGER,INTENT(OUT):: errCode   ! =0 is OK; =1 too many iterations
                                  ! =2 if F(ax) and F(bx) have the same sign
  REAL(DP),INTENT(OUT):: xZero,fZero ! the last and best value of the zero                                   
      
INTERFACE
  FUNCTION F(x) RESULT(g)
	IMPLICIT NONE
	integer,parameter :: DP = selected_real_kind(8)
    REAL(DP),INTENT(IN):: x
    REAL(DP):: g
  END Function F
END INTERFACE    

  REAL(DP):: a,b,c,d,e,eps
  REAL(DP):: fa,fb,fc,tol1
  INTEGER:: kIter
!  INTEGER:: method   ! =0 bisection; =1 linear; =2 inverse quadratic
  REAL(DP):: xm,p,q,r,s
  REAL(DP),PARAMETER:: ZERO=0.0, ONE=1.0, TWO=2.0, THREE=3.0, HALF=0.5
!----------------------------------------------------------------------------
  eps=EPSILON(ax)
  tol1=ONE+eps

  a=ax   ! initialization
  b=bx
  fa=f(a)
  fb=f(b)
  neval=2
  IF (fa==ZERO) THEN
    xZero=a
    fZero=ZERO
    errCode=0
    RETURN
  END IF  
  IF (fb==ZERO) THEN
    xZero=b
    fZero=ZERO
    errCode=0
    RETURN
  END IF  
  IF (fa*fb > ZERO) THEN
    xZero=HUGE(a)
    fZero=HUGE(a)
    errCode=2
    RETURN
  END IF
! The trivial cases have now been dealt with. On to the real work...

  c=a
  fc=fa
  d=b-a
  e=d
    
  DO kIter=1,maxIter
    IF ( (fb>0 .AND. fc>0) .OR. (fb<0 .AND. fc<0) ) THEN
      c=a  ! we insist that b and c straddle the zero
      fc=fa
      d=b-a
      e=d
    END IF

    IF (ABS(fc) < ABS(fb)) THEN
      a=b    ! we insist that b be the better guess of b and c
      b=c
      c=a
      fa=fb
      fb=fc
      fc=fa
    END IF

    tol1=TWO*eps*ABS(b)+HALF*tol   ! convergence test
    xm=HALF*(c-b)
    IF (ABS(xm) <= tol1 .OR. fb==ZERO) THEN
      xZero=b
      fZero=fb
      errCode=0   ! SUCCESS! The proper way to leave
      RETURN
    END IF

!    WRITE(DBG,*) "BrentZero, start of kIter=",kIter
!    WRITE(DBG,'(A,2ES25.16)') "a,fa=", a,fa
!    WRITE(DBG,'(A,2ES25.16)') "b,fb=", b,fb
!    WRITE(DBG,'(A,2ES25.16)') "c,fc=", c,fc
!    WRITE(DBG,'(A,2ES25.16)') "xm,tol1=", xm,tol1

    IF (ABS(e) < tol1 .OR. ABS(fa) <= ABS(fb) ) THEN
      d=xm   ! bisection
      e=d
!      method=0
    ELSE
      IF (a==c) THEN
        s=fb/fa   ! linear interpolation
        p=TWO*xm*s
        q=ONE-s
!        method=1
      ELSE
        q=fa/fc   ! inverse quadratic interpolation
        r=fb/fc
        s=fb/fa
        p=s*(TWO*xm*q*(q-r)-(b-a)*(r-ONE))
        q=(q-ONE)*(r-ONE)*(s-ONE)
!        method=2
      END IF
      IF (p > ZERO) q=-q   ! adjust signs
      p=ABS(p)
      IF (p+p >= (THREE*xm*q-ABS(tol1*q)) .OR. p+p >= ABS(e*q) ) THEN
        d=xm   ! don't interpolate. Use bisection
        e=d
 !       method=-1
      ELSE
        e=d   ! OK, use interpolation
        d=p/q
      END IF
    END IF  
   
    a=b   ! complete step. a becomes the previous iteration
    fa=fb
   IF (ABS(d) > tol1) THEN
     b=b+d
   ELSE  
     b=b+SIGN(tol1,xm)
   END IF
     
   fb=F(b)   ! the newest and best value (we hope)
   neval=neval+1   ! keep count of the function evaluations
!!   IF ((fb*(fc/ABS(fc))) > ZERO) GO TO 20
!    WRITE(DBG,*) "BrentZero, end of kIter=",kIter, "   method=",method
!    WRITE(DBG,'(A,2ES25.16)') " new b,fb=", b,fb
!    WRITE(DBG,'(A,2ES25.16)') "d,e=", d,e
!    WRITE(DBG,'(A,2ES25.16)') "p,q=", p,q
  END DO
  
! The loop should never terminate. If it does, return the last iteration
!  and set errCode to 1
  xZero=b
  fZero=fb
  errCode=1
  RETURN  

END Subroutine BrentZeroDouble
!-----------------------------------------------------------------------------


end module RootFMM
