C This file contains all the subroutines needed by the nonlinear solver broydn
C code modified based on on-line version in Numerical Recipes Website, Dec 28, 2004

      SUBROUTINE cpbroydn(x0min,x,x0max,STPMX,n,
     &  fveccopy,funcv,TOLF,ierr)
      implicit none
      INTEGER n,NP,MAXITS,ierr
      double precision x(n),EPS,TOLF,TOLMIN,TOLX,STPMX
      double precision x0min(n),x0max(n),fveccopy(n)
      LOGICAL check
!      PARAMETER (NP=1000,MAXITS=250,EPS=1.0d-7,TOLF=1.0d-4,
!     &     TOLMIN=1.d-6,TOLX=EPS)
      PARAMETER(NP=1000,MAXITS=250,EPS=1.0d-7,TOLX=EPS)
CU    USES fdjac,funcv,lnsrch,qrdcmp,qrupdt,rsolv
      INTEGER i,its,j,k
      double precision den,f,fold,stpmax,sum,temp,test,c(NP),
     &     d(NP),fvcold(NP),g(NP),p(NP),qt(NP,NP),r(NP,NP),
     &     s(NP),t(NP),w(NP),xold(NP),fvec(NP)
      LOGICAL restrt,sing,skip
      EXTERNAL funcv
      TOLMIN=TOLF*0.01d0
      call funcv(n,x,fvec,f)
      test=0.0d0
      do 11 i=1,n
        if(dabs(fvec(i)).gt.test)test=dabs(fvec(i))
11    continue
      if(test.lt..01d0*TOLF)then
        ierr=0
!        check=.false.
        return
      endif
      sum=0.0d0
      do 12 i=1,n
        sum=sum+x(i)*x(i)
12    continue
      stpmax=STPMX*dmax1(dsqrt(sum),dble(n))
      restrt=.true.
      do 42 its=1,MAXITS
        if(restrt)then
          do i=1,n
            if(x(i).lt.x0min(i).or.x(i).gt.x0max(i))then
              ierr=1
              return
            endif
          enddo            
          call cpfdjac(n,x,fvec,NP,r,funcv)
          call cpqrdcmp(r,n,NP,c,d,sing)
!          if(sing) pause      'singular Jacobian in broydn'
          if(sing)then
            ierr=2
            return
          end if
          do 14 i=1,n
            do 13 j=1,n
              qt(i,j)=0.0d0
13          continue
            qt(i,i)=1.0d0
14        continue
          do 18 k=1,n-1
            if(c(k).ne.0.0d0)then
              do 17 j=1,n
                sum=0.0d0
                do 15 i=k,n
                  sum=sum+r(i,k)*qt(i,j)
15              continue
                sum=sum/c(k)
                do 16 i=k,n
                  qt(i,j)=qt(i,j)-sum*r(i,k)
16              continue
17            continue
            endif
18        continue
          do 21 i=1,n
            r(i,i)=d(i)
            do 19 j=1,i-1
              r(i,j)=0.0d0
19          continue
21        continue
        else
          do 22 i=1,n
            s(i)=x(i)-xold(i)
22        continue
          do 24 i=1,n
            sum=0.0d0
            do 23 j=i,n
              sum=sum+r(i,j)*s(j)
23          continue
            t(i)=sum
24        continue
          skip=.true.
          do 26 i=1,n
            sum=0.0d0
            do 25 j=1,n
              sum=sum+qt(j,i)*t(j)
25          continue
            w(i)=fvec(i)-fvcold(i)-sum
            if(dabs(w(i)).ge.EPS*(dabs(fvec(i))+
     &       dabs(fvcold(i))))then
              skip=.false.
            else
              w(i)=0.0d0
            endif
26        continue
          if(.not.skip)then
            do 28 i=1,n
              sum=0.0d0
              do 27 j=1,n
                sum=sum+qt(i,j)*w(j)
27            continue
              t(i)=sum
28          continue
            den=0.0d0
            do 29 i=1,n
              den=den+s(i)*s(i)
29          continue
            do 31 i=1,n
              s(i)=s(i)/den
31          continue
            call cpqrupdt(r,qt,n,NP,t,s)
            do 32 i=1,n
              if(r(i,i).eq.0.0d0) then
                write(*,*) 'r singular in broydn'
              end if
              d(i)=r(i,i)
32          continue
          endif
        endif
        do 34 i=1,n
          sum=0.0d0
          do 33 j=1,n
            sum=sum+qt(i,j)*fvec(j)
33        continue
          p(i)=-sum
34      continue
        do 36 i=n,1,-1
          sum=0.0d0
          do 35 j=1,i
            sum=sum-r(j,i)*p(j)
35        continue
          g(i)=sum
36      continue
        do 37 i=1,n
          xold(i)=x(i)
          fvcold(i)=fvec(i)
37      continue
        fold=f
        call cprsolv(r,n,NP,d,p)

! Gu modification starts
        do 100 i=1,n
          if(xold(i).lt.x0min(i).or.xold(i).gt.x0max(i))then
            ierr=1
            return
          endif
100     continue
! Gu modification ends
        call cplnsrch(n,xold,fold,g,p,x,f,
     &    stpmax,check,funcv,fvec)
        test=0.0d0
        do 38 i=1,n
          if(dabs(fvec(i)).gt.test)test=dabs(fvec(i))
          fveccopy(i)=fvec(i)
38      continue
        if(test.lt.TOLF)then
          ierr=0
!          check=.false.
          return
        endif
        if(check)then
          if(restrt)then
            ierr=3
            return
          else
            test=0.0d0
            den=dmax1(f,.5d0*dble(n))
            do 39 i=1,n
              temp=dabs(g(i))*dmax1(dabs(x(i)),1.0d0)/den
              if(temp.gt.test)test=temp
39          continue
            if(test.lt.TOLMIN)then
              ierr=4
              return
            else
              restrt=.true.
            endif
          endif
        else
          restrt=.false.
          test=0.0d0
          do 41 i=1,n
            temp=(dabs(x(i)-xold(i)))/dmax1(dabs(x(i)),1.0d0)
            if(temp.gt.test)test=temp
41        continue
          if(test.lt.TOLX)then
            ierr=4
!            check=.true.
            return
          endif
        endif
42    continue
      ierr=5      
      return
      END

      SUBROUTINE cpfdjac(n,x,fvec,np,df,funcv)
      implicit none
      INTEGER n,np
      double precision df(np,np),fvec(n),x(n),EPS
      PARAMETER (EPS=1.0d-4)
CU    USES funcv
      INTEGER i,j,k
      double precision h,temp,f(n),fsqsum
      external funcv
      do 12 j=1,n
        temp=x(j)
        h=EPS*dabs(temp)
        if(h.eq.0.0d0)h=EPS
        x(j)=temp+h
        h=x(j)-temp
        call funcv(n,x,f,fsqsum)
        x(j)=temp
        do 11 i=1,n
          df(i,j)=(f(i)-fvec(i))/h
11      continue
12    continue
      return
      END
c           
      SUBROUTINE cplnsrch(n,xold,fold,g,p,x,f,
     &   stpmax,check,funcv,fvec)
      implicit none
      INTEGER n
      LOGICAL check
      DOUBLE PRECISION f,fold,stpmax,g(n),p(n),x(n),
     *xold(n),ALF,TOLX,fvec(n)
      PARAMETER (ALF=1.d-4,TOLX=1.d-7)
      EXTERNAL funcv
CU    USES funcv
      INTEGER i
      DOUBLE PRECISION a,alam,alam2,alamin,b,disc,
     *f2,rhs1,rhs2,slope,sum,temp,test,tmplam
      check=.false.
      sum=0.0d0
      do 11 i=1,n
        sum=sum+p(i)*p(i)
11    continue
      sum=dsqrt(sum)
      if(sum.gt.stpmax)then
        do 12 i=1,n
          p(i)=p(i)*stpmax/sum
12      continue
      endif
      slope=0.0d0
      do 13 i=1,n
        slope=slope+g(i)*p(i)
13    continue
!      if(slope.ge.0.0d0)pause 'roundoff problem in lnsrch'
      test=0.0d0
      do 14 i=1,n
        temp=dabs(p(i))/dmax1(dabs(xold(i)),1.0d0)
        if(temp.gt.test)test=temp
14    continue
      alamin=TOLX/test
      alam=1.0d0
1     continue
        do 15 i=1,n
          x(i)=xold(i)+alam*p(i)
15      continue
        call funcv(n,x,fvec,f)
        if(alam.lt.alamin)then
          do 16 i=1,n
            x(i)=xold(i)
16        continue
          check=.true.
          return
        else if(f.le.fold+ALF*alam*slope)then
          return
        else
          if(alam.eq.1.0d0)then
            tmplam=-slope/(2.0d0*(f-fold-slope))
          else
            rhs1=f-fold-alam*slope
            rhs2=f2-fold-alam2*slope
            a=(rhs1/alam**2-rhs2/alam2**2)/(alam-alam2)
            b=(-alam2*rhs1/alam**2+alam*rhs2/alam2**2)/
     &           (alam-alam2)
            if(a.eq.0.0d0)then
              tmplam=-slope/(2.0d0*b)
            else
              disc=b*b-3.0d0*a*slope
              if(disc.lt.0.0d0) then
                tmplam=0.5d0*alam
              else if(b.le.0.0d0)then
                tmplam=(-b+dsqrt(disc))/(3.0d0*a)
              else
                tmplam=-slope/(b+dsqrt(disc))
              endif
            endif
            if(tmplam.gt..5d0*alam)tmplam=.5d0*alam
          endif
        endif
        alam2=alam
        f2=f
        alam=dmax1(tmplam,.1d0*alam)
      goto 1
      END
c
      SUBROUTINE cpqrdcmp(a,n,np,c,d,sing)
      implicit none
      INTEGER n,np
      DOUBLE PRECISION a(np,np),c(n),d(n)
      LOGICAL sing
      INTEGER i,j,k
      DOUBLE PRECISION scale,sigma,sum,tau
      sing=.false.
      do 17 k=1,n-1
        scale=0.0d0
        do 11 i=k,n
          scale=dmax1(scale,dabs(a(i,k)))
11      continue
        if(scale.eq.0.0d0)then
          sing=.true.
          c(k)=0.0d0
          d(k)=0.0d0
        else
          do 12 i=k,n
            a(i,k)=a(i,k)/scale
12        continue
          sum=0.0d0
          do 13 i=k,n
            sum=sum+a(i,k)**2
13        continue
          sigma=dsign(dsqrt(sum),a(k,k))
          a(k,k)=a(k,k)+sigma
          c(k)=sigma*a(k,k)
          d(k)=-scale*sigma
          do 16 j=k+1,n
            sum=0.0d0
            do 14 i=k,n
              sum=sum+a(i,k)*a(i,j)
14          continue
            tau=sum/c(k)
            do 15 i=k,n
              a(i,j)=a(i,j)-tau*a(i,k)
15          continue
16        continue
        endif
17    continue
      d(n)=a(n,n)
      if(d(n).eq.0.0d0)sing=.true.
      return
      END
c
      SUBROUTINE cpqrupdt(r,qt,n,np,u,v)
      implicit none
      INTEGER n,np
      DOUBLE PRECISION r(np,np),qt(np,np),u(np),v(np)
CU    USES rotate
      INTEGER i,j,k
      do 11 k=n,1,-1
        if(u(k).ne.0.0d0)goto 1
11    continue
      k=1
1     do 12 i=k-1,1,-1
        call cprotate(r,qt,n,np,i,u(i),-u(i+1))
        if(u(i).eq.0.0d0)then
          u(i)=dabs(u(i+1))
        else if(dabs(u(i)).gt.dabs(u(i+1)))then
          u(i)=dabs(u(i))*dsqrt(1.0d0+(u(i+1)/u(i))**2)
        else
          u(i)=dabs(u(i+1))*dsqrt(1.0d0+(u(i)/u(i+1))**2)
        endif
12    continue
      do 13 j=1,n
        r(1,j)=r(1,j)+u(1)*v(j)
13    continue
      do 14 i=1,k-1
        call cprotate(r,qt,n,np,i,r(i,i),-r(i+1,i))
14    continue
      return
      END
c
      SUBROUTINE cprsolv(a,n,np,d,b)
      implicit none
      INTEGER n,np
      DOUBLE PRECISION a(np,np),b(n),d(n)
      INTEGER i,j
      DOUBLE PRECISION sum
      b(n)=b(n)/d(n)
      do 12 i=n-1,1,-1
        sum=0.0d0
        do 11 j=i+1,n
          sum=sum+a(i,j)*b(j)
11      continue
        b(i)=(b(i)-sum)/d(i)
12    continue
      return
      END
c
      SUBROUTINE cprotate(r,qt,n,np,i,a,b)
      implicit none
      INTEGER n,np,i
      DOUBLE PRECISION a,b,r(np,np),qt(np,np)
      INTEGER j
      DOUBLE PRECISION c,fact,s,w,y
      if(a.eq.0.0d0)then
        c=0.0d0
        s=dsign(1.0d0,b)
      else if(dabs(a).gt.dabs(b))then
        fact=b/a
        c=dsign(1.0d0/dsqrt(1.0d0+fact**2),a)
        s=fact*c
      else
        fact=a/b
        s=dsign(1.0d0/dsqrt(1.0d0+fact**2),b)
        c=fact*s
      endif
      do 11 j=i,n
        y=r(i,j)
        w=r(i+1,j)
        r(i,j)=c*y-s*w
        r(i+1,j)=s*y+c*w
11    continue
      do 12 j=1,n
        y=qt(i,j)
        w=qt(i+1,j)
        qt(i,j)=c*y-s*w
        qt(i+1,j)=s*y+c*w
12    continue
      return
      END

      subroutine cpxmprove(N,NP,a,b,x,mark)
      implicit none
      INTEGER i,j,idum,N,NP,indx(N),mark
      double precision d,a(NP,NP),b(N),x(N),aa(NP,NP)

      do 12 i=1,N
        x(i)=b(i)
        do 11 j=1,N
          aa(i,j)=a(i,j)
11      continue
12    continue
      call cpludcmp(aa,N,NP,indx,d,mark)
      if (mark .eq. 0) goto 20
      call cplubksb(aa,N,NP,indx,x)
      call cpmprove(a,aa,N,NP,indx,b,x)
20    continue
      return
      END


      SUBROUTINE cpmprove(a,alud,n,np,indx,b,x)
      implicit none
      INTEGER n,np,indx(n),NMAX
      double precision a(np,np),alud(np,np),b(n),x(n)
      PARAMETER (NMAX=500)
CU    USES lubksb
      INTEGER i,j
      double precision r(NMAX)
      DOUBLE PRECISION sdp
      do 12 i=1,n
        sdp=-b(i)
        do 11 j=1,n
          sdp=sdp+(a(i,j))*(x(j))
11      continue
        r(i)=sdp
12    continue
      call cplubksb(alud,n,np,indx,r)
      do 13 i=1,n
        x(i)=x(i)-r(i)
13    continue
      return
      END

      SUBROUTINE cpludcmp(a,n,np,indx,d,mark)
      implicit none
      INTEGER n,np,indx(n),NMAX
      double precision d,a(np,np),TINY
      PARAMETER (NMAX=500,TINY=1.0d-20)
      INTEGER i,imax,j,k,mark
      double precision aamax,dum,sum,vv(NMAX)
      mark=1
      d=1.0d0
      do 12 i=1,n
        aamax=0.0d0
        do 11 j=1,n
          if (dabs(a(i,j)).gt.aamax) aamax=dabs(a(i,j))
11      continue
        if (aamax.eq.0.0d0) then
! singular matrix          
          mark=0
          return
        end if
        vv(i)=1.0d0/aamax
12    continue
      do 19 j=1,n
        do 14 i=1,j-1
          sum=a(i,j)
          do 13 k=1,i-1
            sum=sum-a(i,k)*a(k,j)
13        continue
          a(i,j)=sum
14      continue
        aamax=0.0d0
        do 16 i=j,n
          sum=a(i,j)
          do 15 k=1,j-1
            sum=sum-a(i,k)*a(k,j)
15        continue
          a(i,j)=sum
          dum=vv(i)*dabs(sum)
          if (dum.ge.aamax) then
            imax=i
            aamax=dum
          endif
16      continue
        if (j.ne.imax)then
          do 17 k=1,n
            dum=a(imax,k)
            a(imax,k)=a(j,k)
            a(j,k)=dum
17        continue
          d=-d
          vv(imax)=vv(j)
        endif
        indx(j)=imax
        if(a(j,j).eq.0.0d0)a(j,j)=TINY
        if(j.ne.n)then
          dum=1.0d0/a(j,j)
          do 18 i=j+1,n
            a(i,j)=a(i,j)*dum
18        continue
        endif
19    continue
      return
      END

      SUBROUTINE cplubksb(a,n,np,indx,b)
      implicit none
      INTEGER n,np,indx(n)
      double precision a(np,np),b(n)
      INTEGER i,ii,j,ll
      double precision sum
      ii=0
      do 12 i=1,n
        ll=indx(i)
        sum=b(ll)
        b(ll)=b(i)
        if (ii.ne.0)then
          do 11 j=ii,i-1
            sum=sum-a(i,j)*b(j)
11        continue
        else if (sum.ne.0.0d0) then
          ii=i
        endif
        b(i)=sum
12    continue
      do 14 i=n,1,-1
        sum=b(i)
        do 13 j=i+1,n
          sum=sum-a(i,j)*b(j)
13      continue
        b(i)=sum/a(i,i)
14    continue
      return
      END