      subroutine solvopt(n,x,f,fun,flg,grad,options,flfc,func,flgc,
     1     gradc,B,g,g0,g1,gt,gc,z,x1,xopt,xrec,grec,xx,deltax,idx,
     2     iout,ifile)
c-----------------------------------------------------------------------------
c The subroutine SOLVOPT performs a modified version of Shor's r-algorithm in
c order to find a local minimum resp. maximum of a nonlinear function
c defined on the n-dimensional Euclidean space
c or
c a local minimum for a nonlinear constrained problem:
c min { f(x): g(x) (<)= 0, g(x) in R(m), x in R(n) }.
c Arguments:
c n       is the space dimension (integer*4),
c x       is the n-vector, the coordinates of the starting point
c         at a call to the subroutine and the optimizer at regular return
c         (double precision),
c f       returns the optimum function value
c         (double precision),
c fun     is the entry name of a subroutine which computes the value
c         of the function <fun> at a point x, should be declared as external
c         in a calling routine,
c         synopsis: fun(x,f)
c grad    is the entry name of a subroutine which computes the gradient
c         vector of the function <fun> at a point x, should be declared as
c         external in a calling routine,
c         synopsis: grad(x,g)
c func    is the entry name of a subroutine which computes the MAXIMAL
c         RESIDIAL!!! (a scalar) for a set of constraints at a point x,
c         should be declared as external in a calling routine,
c         synopsis: func(x,fc)
c gradc   is the entry name of a subroutine which computes the gradient
c         vector for a constraint with the MAXIMAL RESIDUAL at a point x,
c         should be declared as external in a calling routine,
c         synopsis: gradc(x,gc)
c flg,    (logical) is a flag for the use of a subroutine <grad>:
c         .true. means gradients are calculated by the user-supplied routine.
c flfc,   (logical) is a flag for a constrained problem:
c         .true. means the maximal residual for a set of constraints
c         is calculated by <func>.
c flgc,   (logical) is a flag for the use of a subroutine <gradc>:
c         .true. means gradients of the constraints are calculated
c         by the user-supplied routine.
c options is a vector of optional parameters (double precision):
c     options(1)= H, where sign(H)=-1 resp. sign(H)=+1 means minimize resp.
c         maximize <fun> (valid only for an unconstrained problem) and
c         H itself is a factor for the initial trial step size
c         (options(1)=-1.d0 by default),
c     options(2)= relative error for the argument in terms of the infinity-norm
c         (1.d-4 by default),
c     options(3)= relative error for the function value (1.d-6 by default),
c     options(4)= limit for the number of iterations (1.5d4 by default),
c     options(5)= control of the display of intermediate results and error
c         resp. warning messages (default value is 0.d0, i.e., no intermediate
c         output but error and warning messages, see the manual for more),
c     options(6)= maximal admissible residual for a set of constraints
c         (options(6)=1.d-8 by default, see the manual for more),
c    *options(7)= the coefficient of space dilation (2.5d0 by default),
c    *options(8)= lower bound for the stepsize used for the difference
c        approximation of gradients (1.d-11 by default,see the manual for more).
c   (* ... changes should be done with care)
c Returned optional values:
c     options(9),  the number of iterations, if positive,
c         or an abnormal stop code, if negative (see manual for more),
c                -1: allocation error,
c                -2: improper space dimension,
c                -3: <fun> returns an improper value,
c                -4: <grad> returns a zero vector or improper value at the
c                    starting point,
c                -5: <func> returns an improper value,
c                -6: <gradc> returns an improper value,
c                -7: function is unbounded,
c                -8: gradient is zero at the point,
c                    but stopping criteria are not fulfilled,
c                -9: iterations limit exceeded,
c               -11: Premature stop is possible,
c               -12: Result may not provide the true optimum,
c               -13: Function is flat: result may be inaccurate
c                    in view of a point.
c               -14: Function is steep: result may be inaccurate
c                    in view of a function value,
c       options(10), the number of objective function evaluations, and
c       options(11), the number of gradient evaluations.
c       options(12), the number of constraint function evaluations, and
c       options(13), the number of constraint gradient evaluations.
c ____________________________________________________________________________
c
      implicit none
      include 'messages.inc'

      logical flg,flgc,flfc, constr, app, appconstr
      logical FsbPnt, FsbPnt1, termflag, stopf
      logical stopping, dispwarn, Reset, ksm,knan,obj
      integer n, kstore, ajp,ajpp,knorms, k, kcheck, numelem
      integer dispdata, ld, mxtc, termx, limxterm, nzero, krerun
      integer warnno, kflat, stepvanish, i,j,ni,ii, kd,kj,kc,ip
      integer iterlimit, kg,k1,k2, kless
      integer m1
      double precision options(13),doptions(13)
      double precision x(n),f
      double precision nsteps(3), gnorms(10), kk, nx
      double precision ajb,ajs, des, dq,du20,du10,du03
      double precision n_float, cnteps
      double precision low_bound, ZeroGrad, ddx, y
      double precision lowxbound, lowfbound, detfr, detxr, grbnd
      double precision fp,fp1,fc,f1,f2,fm,fopt,frec,fst, fp_rate
      double precision PenCoef, PenCoefNew
      double precision gamma,w,wdef,h1,h,hp
      double precision dx,ng,ngc,nng,ngt,nrmz,ng1,d,dd, laststep
      double precision zero,one,two,three,four,five,six,seven
      double precision eight,nine,ten,hundr
      double precision infty, epsnorm,epsnorm2,powerm12
      double precision B,g,g0,g1,gt,gc,z
      double precision x1,xopt,xrec,grec,xx,deltax
      integer idx
      dimension B(n,n),g(n),g0(n),g1(n),gt(n),gc(n),z(n)
      dimension x1(n),xopt(n),xrec(n),grec(n),xx(n),deltax(n)
      dimension idx(n)

      character endwarn*100
      external fun,grad,func,gradc

      integer iout
      character*(*) ifile

      data
     1    zero/0.d0/, one/1.d0/, two/2.d0/, three/3.d0/, four/4.d0/,
     2    five/5.d0/, six/6.d0/, seven/7.d0/, eight/8.d0/, nine/9.d0/,
     3    ten/1.d1/,  hundr/1.d2/, powerm12/1.d-12/,
     4    infty /1.d100/, epsnorm /1.d-15/,  epsnorm2 /1.d-30/

      if (options(5).gt.-one) then
        open(unit=iout,file=ifile(1:len_trim(ifile)),status='unknown')
      endif

c Check the dimension:
      if (n.lt.2) then
		if (options(5).gt.-one) then
          write(iout,*) errmes
          write(iout,*) error2
        endif
        options(9)=-one
        goto 999
      endif
      n_float=dble(n)

c store flags:
      app=.not.flg
      constr=flfc
      appconstr=.not.flgc
c Default values for options:
      call soptions(doptions)
      do i=1,8
            if (options(i).eq.zero) then
               options(i)=doptions(i)
            elseif (i.eq.2.or.i.eq.3.or.i.eq.6) then
               options(i)=dmax1(options(i),powerm12)
               options(i)=dmin1(options(i),one)
               if (i.eq.2)options(i)=dmax1(options(i),options(8)*hundr)
            elseif (i.eq.7) then
               options(7)=dmax1(options(i),1.5d0)
            endif
      enddo

c WORKING CONSTANTS AND COUNTERS ----{

      options(10)=zero    !! counter for function calculations
      options(11)=zero    !! counter for gradient calculations
      options(12)=zero    !! counter for constraint function calculations
      options(13)=zero    !! counter for constraint gradient calculations
      iterlimit=idint(options(4))
      if (constr) then
        h1=-one           !! NLP: restricted to minimization
        cnteps=options(6)
      else
        h1=dsign(one,options(1))  !! Minimize resp. maximize a function
      endif
      k=0                         !! Iteration counter
      wdef=one/options(7)-one     !! Default space transf. coeff.

c Gamma control ---{
      ajb=one+1.d-1/n_float**2    !! Base I
      ajp=20
      ajpp=ajp                    !! Start value for the power
      ajs=1.15d0                  !! Base II
      knorms=0
      do i=1,10
       gnorms(i)=zero
      enddo
c---}
c Display control ---{
      if (options(5).le.zero) then
         dispdata=0
         if (options(5).eq.-one) then
            dispwarn=.false.
         else
            dispwarn=.true.
         endif
      else
         dispdata=idnint(options(5))
         dispwarn=.true.
      endif
      ld=dispdata
c---}

c Stepsize control ---{
      dq=5.1d0           !! Step divider (at f_{i+1}>gamma*f_{i})
      du20=two
      du10=1.5d0
      du03=1.05d0        !! Step multipliers (at certain steps made)
      kstore=3
      do i=1,kstore
       nsteps(i)=zero    !! Steps made at the last 'kstore' iterations
      enddo
      if (app) then
        des=6.3d0        !! Desired number of steps per 1-D search
      else
        des=3.3d0
      endif
      mxtc=3             !! Number of trial cycles (steep wall detect)
c---}
      termx=0
      limxterm=50        !! Counter and limit for x-criterion
c stepsize for gradient approximation
      ddx=dmax1(1.d-11,options(8))

      low_bound=-one+1.d-4     !! Lower bound cosine used to detect a ravine
      ZeroGrad=n_float*1.d-16  !! Lower bound for a gradient norm
      nzero=0                  !! Zero-gradient events counter
c Low bound for the values of variables to take into account
      lowxbound=dmax1(options(2),1.d-3)
c Lower bound for function values to be considered as making difference
      lowfbound=options(3)**2
      krerun=0                 !! Re-run events counter
      detfr=options(3)*hundr   !! Relative error for f/f_{record}
      detxr=options(2)*ten     !! Relative error for norm(x)/norm(x_{record})
      warnno=0                 !! the number of warn.mess. to end with
      kflat=0                  !! counter for points of flatness
      stepvanish=0             !! counter for vanished steps
      stopf=.false.
c ----}  End of setting constants
c ----}  End of the preamble
c--------------------------------------------------------------------
c COMPUTE THE FUNCTION  ( FIRST TIME ) ----{
      call fun(n,x,f)
c	  write(iout,*) 'f'
c	  write(iout,*) f
      options(10)=options(10)+one
      if (dabs(f).ge.infty) then
         if (dispwarn) then
            write(iout,*) errmes
            write(iout,*) error32
            write(iout,*) error6
         endif
         options(9)=-three
         goto 999
      endif
      do i=1,n
        xrec(i)=x(i)
      enddo
      frec=f     !! record point and function value
c Constrained problem
      if (constr)  then
          kless=0
          fp=f
          call func(n,x,fc)
c		  write(iout,*) 'g'
c		  write(iout,*) fc
          options(12)=options(12)+one
          if (dabs(fc).ge.infty) then
             if (dispwarn) then
                write(iout,*) errmes
                write(iout,*) error52
                write(iout,*) error6
             endif
             options(9)=-five
             goto 999
          endif
        PenCoef=one          !! first rough approximation
        if (fc.le.cnteps) then
         FsbPnt=.true.       !! feasible point
         fc=zero
        else
         FsbPnt=.false.
        endif
        f=f+PenCoef*fc
      endif
c ----}
c COMPUTE THE GRADIENT ( FIRST TIME ) ----{
      if (app) then
        do i=1,n
         deltax(i)=h1*ddx
        enddo
        obj=.true.
        if (constr) then
           call apprgrdn(n,g,x,fp,fun,deltax,obj)
        else
           call apprgrdn(n,g,x,f,fun,deltax,obj)
        endif
        options(10)=options(10)+n_float
      else
        call grad(n,x,g)
c	    write(iout,*) 'gf'
c	    write(iout,*) g
        options(11)=options(11)+one
      endif
      ng=zero
      do i=1,n
         ng=ng+g(i)*g(i)
      enddo
      ng=dsqrt(ng)
      if (ng.ge.infty) then
         if (dispwarn) then
            write(iout,*) errmes
            write(iout,*) error42
            write(iout,*) error6
         endif
         options(9)=-four
         goto 999
      elseif (ng.lt.ZeroGrad) then
         if (dispwarn) then
            write(iout,*) errmes
            write(iout,*) error43
            write(iout,*) error6
         endif
         options(9)=-four
         goto 999
      endif
      if (constr) then
       if (.not.FsbPnt) then
         if (appconstr) then
            do j=1,n
              if (x(j).ge.zero) then
                 deltax(j)=ddx
              else
                 deltax(j)=-ddx
              endif
            enddo
            obj=.false.
            call apprgrdn(n,gc,x,fc,func,deltax,obj)
         else
            call gradc(n,x,gc)
c	        write(iout,*) 'dg'
c	        write(iout,*) gc
         endif
         ngc=zero
         do i=1,n
           ngc=ngc+gc(i)*gc(i)
         enddo
         ngc=dsqrt(ngc)
         if (ng.ge.infty) then
            if (dispwarn) then
               write(iout,*) errmes
               write(iout,*) error62
               write(iout,*) error6
            endif
            options(9)=-six
            goto 999
         elseif (ng.lt.ZeroGrad) then
            if (dispwarn) then
               write(iout,*) errmes
               write(iout,*) error63
            endif
            options(9)=-six
            goto 999
         endif
         do i=1,n
           g(i)=g(i)+PenCoef*gc(i)
         enddo
         ng=zero
         do i=1,n
           ng=ng+g(i)*g(i)
           grec(i)=g(i)
         enddo
         ng=dsqrt(ng)
       endif
      endif
      do i=1,n
        grec(i)=g(i)
      enddo
      nng=ng
c ----}
c INITIAL STEPSIZE
      d=zero
      do i=1,n
        if (d.lt.dabs(x(i))) d=dabs(x(i))
      enddo
      h=h1*dsqrt(options(2))*d                  !! smallest possible stepsize
      if (dabs(options(1)).ne.one) then
        h=h1*dmax1(dabs(options(1)),dabs(h))    !! user-supplied stepsize
      else
          h=h1*dmax1(one/dlog(ng+1.1d0),dabs(h)) !! calculated stepsize
      endif

C RESETTING LOOP ----{
      do while (.true.)
        kcheck=0                       !! Set checkpoint counter.
        kg=0                           !! stepsizes stored
        kj=0                           !! ravine jump counter
        do i=1,n
          do j=1,n
            B(i,j)=zero
          enddo
          B(i,i)=one                   !! re-set transf. matrix to identity
          g1(i)=g(i)
        enddo
        fst=f
        dx=0
c ----}

c MAIN ITERATIONS ----{

        if (options(5).gt.zero) then
          write(iout,*)
     1        'Iteration # ..... Function Value ..... ',
     2        'Step Value ..... Gradient Norm'
        endif

        do while (.true.)
          k=k+1
          kcheck=kcheck+1
          laststep=dx
c ADJUST GAMMA --{
           gamma=one+dmax1(ajb**((ajp-kcheck)*n),two*options(3))
           gamma=dmin1 ( gamma,ajs**dmax1(one,dlog10(nng+one)) )
c --}
       ngt=zero
       ng1=zero
       dd=zero
       do i=1,n
         d=zero
         do j=1,n
            d=d+B(j,i)*g(j)
         enddo
         gt(i)=d
         dd=dd+d*g1(i)
         ngt=ngt+d*d
         ng1=ng1+g1(i)*g1(i)
       enddo
       ngt=dsqrt(ngt)
       ng1=dsqrt(ng1)
       dd=dd/ngt/ng1

       w=wdef
c JUMPING OVER A RAVINE ----{
       if (dd.lt.low_bound) then
        if (kj.eq.2) then
          do i=1,n
           xx(i)=x(i)
          enddo
        endif
        if (kj.eq.0) kd=4
        kj=kj+1
        w=-.9d0              !! use large coef. of space dilation
        h=h*two
        if (kj.gt.2*kd) then
          kd=kd+1
          warnno=1
          endwarn=endwarn1
          do i=1,n
            if (dabs(x(i)-xx(i)).lt.epsnorm*dabs(x(i))) then
             if (dispwarn)  then
                write(iout,*) wrnmes
                write(iout,*) warn08
             endif
            endif
          enddo
        endif
       else
        kj=0
       endif
c ----}
c DILATION ----{
       nrmz=zero
       do i=1,n
         z(i)=gt(i)-g1(i)
         nrmz=nrmz+z(i)*z(i)
       enddo
       nrmz=dsqrt(nrmz)
       if (nrmz.gt.epsnorm*ngt) then
        do i=1,n
         z(i)=z(i)/nrmz
        enddo
c New direction in the transformed space: g1=gt+w*(z*gt')*z and
c new inverse matrix: B = B ( I + (1/alpha -1)zz' )
        d = zero
        do i=1,n
          d=d+z(i)*gt(i)
        enddo
        ng1=zero
        d = d*w
        do i=1,n
          dd=zero
          g1(i)=gt(i)+d*z(i)
          ng1=ng1+g1(i)*g1(i)
          do j=1,n
             dd=dd+B(i,j)*z(j)
          enddo
          dd=w*dd
          do j=1,n
            B(i,j)=B(i,j)+dd*z(j)
          enddo
        enddo
        ng1=dsqrt(ng1)
       else
        do i=1,n
         z(i)=zero
         g1(i)=gt(i)
        enddo
        nrmz=zero
       endif
       do i=1,n
           gt(i)=g1(i)/ng1
       enddo
        do i=1,n
          d=zero
            do j=1,n
               d=d+B(i,j)*gt(j)
            enddo
          g0(i)=d
        enddo
c ----}
c RESETTING ----{
        if (kcheck.gt.1) then
           numelem=0
           do i=1,n
              if (dabs(g(i)).gt.ZeroGrad) then
                 numelem=numelem+1
                 idx(numelem)=i
              endif
           enddo
           if (numelem.gt.0) then
              grbnd=epsnorm*dble(numelem**2)
              ii=0
              do i=1,numelem
                 j=idx(i)
                 if (dabs(g1(j)).le.dabs(g(j))*grbnd) ii=ii+1
              enddo
              if (ii.eq.n .or. nrmz.eq.zero) then
                if (dispwarn) then
                  write(iout,*) wrnmes
                  write(iout,*) warn20
                endif
                if (dabs(fst-f).lt.dabs(f)*1.d-2) then
                   ajp=ajp-10*n
                else
                   ajp=ajpp
                endif
                h=h1*dx/three
                k=k-1
                exit
              endif
           endif
        endif
c ----}
c STORE THE CURRENT VALUES AND SET THE COUNTERS FOR 1-D SEARCH
        do i=1,n
         xopt(i)=x(i)
        enddo
        fopt=f
        k1=0
        k2=0
        ksm=.false.
        kc=0
        knan=.false.
        hp=h
        if (constr) Reset=.false.
c 1-D SEARCH ----{
        do while (.true.)
         do i=1,n
          x1(i)=x(i)
         enddo
         f1=f
         if (constr) then
           FsbPnt1=FsbPnt
           fp1=fp
         endif
c NEW POINT
         do i=1,n
            x(i)=x(i)+hp*g0(i)
         enddo
           ii=0
           do i=1,n
            if (dabs(x(i)-x1(i)).lt.dabs(x(i))*epsnorm) ii=ii+1
           enddo
c FUNCTION VALUE
         call fun(n,x,f)
c	     write(iout,*) 'f'
c	     write(iout,*) f
         options(10)=options(10)+one
         if (h1*f.ge.infty) then
            if (dispwarn) then
              write(iout,*) errmes
              write(iout,*) error5
            endif
            options(9)=-seven
            goto 999
         endif
         if (constr) then
           fp=f
           call func(n,x,fc)
c	       write(iout,*) 'g'
c	       write(iout,*) fc
           options(12)=options(12)+one
           if (dabs(fc).ge.infty) then
               if (dispwarn) then
                  write(iout,*) errmes
                  write(iout,*) error52
                  write(iout,*) error6
               endif
               options(9)=-five
               goto 999
           endif
           if (fc.le.cnteps) then
              FsbPnt=.true.
              fc=zero
           else
              FsbPnt=.false.
              fp_rate=fp-fp1
              if (fp_rate.lt.-epsnorm) then
               if (.not.FsbPnt1) then
                d=zero
                do i=1,n
                  d=d+(x(i)-x1(i))**2
                enddo
                d=dsqrt(d)
                PenCoefNew=-1.5d1*fp_rate/d
                if (PenCoefNew.gt.1.2d0*PenCoef) then
                  PenCoef=PenCoefNew
                  Reset=.true.
                  kless=0
                  f=f+PenCoef*fc
                  exit
                endif
               endif
              endif
           endif
           f=f+PenCoef*fc
         endif
         if (dabs(f).ge.infty) then
             if (dispwarn) then
               write(iout,*) wrnmes
               write(iout,*) error32
             endif
             if (ksm.or.kc.ge.mxtc) then
                options(9)=-three
                goto 999
             else
                k2=k2+1
                k1=0
                hp=hp/dq
                do i=1,n
                 x(i)=x1(i)
                enddo
                f=f1
                knan=.true.
                if (constr) then
                  FsbPnt=FsbPnt1
                  fp=fp1
                endif
             endif
c STEP SIZE IS ZERO TO THE EXTENT OF EPSNORM
         elseif (ii.eq.n) then
                stepvanish=stepvanish+1
                if (stepvanish.ge.5) then
                    options(9)=-ten-four
                    if (dispwarn) then
                       write(iout,*) termwarn1
                       write(iout,*) endwarn4
                    endif
                    goto 999
                else
                    do i=1,n
                     x(i)=x1(i)
                    enddo
                    f=f1
                    hp=hp*ten
                    ksm=.true.
                    if (constr) then
                       FsbPnt=FsbPnt1
                       fp=fp1
                    endif
                endif
c USE SMALLER STEP
         elseif (h1*f.lt.h1*gamma**idint(dsign(one,f1))*f1) then
             if (ksm) exit
             k2=k2+1
             k1=0
             hp=hp/dq
             do i=1,n
              x(i)=x1(i)
             enddo
             f=f1
             if (constr) then
                FsbPnt=FsbPnt1
                fp=fp1
             endif
             if (kc.ge.mxtc) exit
c 1-D OPTIMIZER IS LEFT BEHIND
         else
             if (h1*f.le.h1*f1) exit
c USE LARGER STEP
             k1=k1+1
             if (k2.gt.0) kc=kc+1
             k2=0
             if (k1.ge.20) then
                 hp=du20*hp
             elseif (k1.ge.10) then
                 hp=du10*hp
             elseif (k1.ge.3) then
                 hp=du03*hp
             endif
         endif
        enddo
c ----}  End of 1-D search
c ADJUST THE TRIAL STEP SIZE ----{
        dx=zero
        do i=1,n
           dx=dx+(xopt(i)-x(i))**2
        enddo
        dx=dsqrt(dx)
        if (kg.lt.kstore)  kg=kg+1
        if (kg.ge.2)  then
           do i=kg,2,-1
             nsteps(i)=nsteps(i-1)
           enddo
        endif
        d=zero
        do i=1,n
           d=d+g0(i)*g0(i)
        enddo
        d=dsqrt(d)
        nsteps(1)=dx/(dabs(h)*d)
        kk=zero
        d=zero
        do i=1,kg
           dd=dble(kg-i+1)
           d=d+dd
           kk=kk+nsteps(i)*dd
        enddo
        kk=kk/d
        if     (kk.gt.des) then
             if (kg.eq.1) then
                h=h*(kk-des+one)
             else
                h=h*dsqrt(kk-des+one)
             endif
        elseif (kk.lt.des) then
             h=h*dsqrt(kk/des)
        endif

        if (ksm) stepvanish=stepvanish+1
c ----}
c COMPUTE THE GRADIENT ----{
        if (app) then
          do j=1,n
            if (g0(j).ge.zero) then
               deltax(j)=h1*ddx
            else
               deltax(j)=-h1*ddx
            endif
          enddo
          obj=.true.
          if (constr)  then
             call apprgrdn(n,g,x,fp,fun,deltax,obj)
          else
             call apprgrdn(n,g,x,f,fun,deltax,obj)
          endif
          options(10)=options(10)+n_float
        else
          call grad(n,x,g)
c	      write(iout,*) 'df'
c	      write(iout,*) g
          options(11)=options(11)+one
        endif
        ng=zero
        do i=1,n
          ng=ng+g(i)*g(i)
        enddo
        ng=dsqrt(ng)
        if (ng.ge.infty) then
         if (dispwarn) then
           write(iout,*) errmes
           write(iout,*) error42
         endif
         options(9)=-four
         goto 999
        elseif (ng.lt.ZeroGrad) then
         if (dispwarn) then
           write(iout,*) wrnmes
           write(iout,*) warn1
         endif
         ng=ZeroGrad
        endif
c Constraints:
        if (constr) then
         if (.not.FsbPnt) then
           if (ng.lt.1.d-2*PenCoef) then
              kless=kless+1
              if (kless.ge.20) then
                 PenCoef=PenCoef/ten
                 Reset=.true.
                 kless=0
              endif
           else
              kless=0
           endif
           if (appconstr) then
                 do j=1,n
                   if (x(j).ge.zero) then
                      deltax(j)=ddx
                   else
                      deltax(j)=-ddx
                   endif
                 enddo
                 obj=.false.
                 call apprgrdn(n,gc,x,fc,func,deltax,obj)
                 options(12)=options(12)+n_float
           else
                 call gradc(n,x,gc)
c			     write(iout,*) 'dg'
c	             write(iout,*) gc
                 options(13)=options(13)+one
           endif
           ngc=zero
           do i=1,n
              ngc=ngc+gc(i)*gc(i)
           enddo
           ngc=dsqrt(ngc)
           if (ngc.ge.infty) then
                  if (dispwarn) then
                     write(iout,*) errmes
                     write(iout,*) error62
                  endif
                  options(9)=-six
                  goto 999
           elseif (ngc.lt.ZeroGrad .and. .not.appconstr) then
                  if (dispwarn) then
                     write(iout,*) errmes
                     write(iout,*) error63
                  endif
                  options(9)=-six
                  goto 999
           endif
           do i=1,n
             g(i)=g(i)+PenCoef*gc(i)
           enddo
           ng=zero
           do i=1,n
              ng=ng+g(i)*g(i)
           enddo
           ng=dsqrt(ng)
           if (Reset) then
              if (dispwarn) then
                 write(iout,*) wrnmes
                 write(iout,*) warn21
              endif
              h=h1*dx/three
              k=k-1
              nng=ng
              exit
           endif
         endif
        endif
        if (h1*f.gt.h1*frec) then
          frec=f
          do i=1,n
            xrec(i)=x(i)
            grec(i)=g(i)
          enddo
        endif
c ----}
       if (ng.gt.ZeroGrad) then
        if (knorms.lt.10)  knorms=knorms+1
        if (knorms.ge.2)  then
          do i=knorms,2,-1
           gnorms(i)=gnorms(i-1)
          enddo
        endif
        gnorms(1)=ng
        nng=one
          do i=1,knorms
            nng=nng*gnorms(i)
          enddo
        nng=nng**(one/dble(knorms))
       endif
c Norm X:
       nx=zero
       do i=1,n
        nx=nx+x(i)*x(i)
       enddo
       nx=dsqrt(nx)

c DISPLAY THE CURRENT VALUES ----{
       if (k.eq.ld) then
c         write(iout,*)
c     1        'Iteration # ..... Function Value ..... ',
c     2        'Step Value ..... Gradient Norm'
         write(iout,'(5x,i5,7x,f13.5,6x,f13.5,7x,f13.5)') k,f,dx,ng
         do i=1,n,6
          m1=min0(n,i+5)
          write(iout,'(3x,a,2x,6e13.5)') '  X =',(x(j),j=1,m1)
         enddo
         ld=k+dispdata
       endif
c----}
c CHECK THE STOPPING CRITERIA ----{
      termflag=.true.
      if (constr) then
        if (.not.FsbPnt) termflag=.false.
      endif
      if(kcheck.le.5.or.kcheck.le.12.and.ng.gt.one)termflag=.false.
      if(kc.ge.mxtc .or. knan)termflag=.false.
c ARGUMENT
       if (termflag) then
           ii=0
           stopping=.true.
           do i=1,n
             if (dabs(x(i)).ge.lowxbound) then
                ii=ii+1
                idx(ii)=i
                if (dabs(xopt(i)-x(i)).gt.options(2)*dabs(x(i))) then
                  stopping=.false.
                endif
             endif
           enddo
           if (ii.eq.0 .or. stopping)  then
                stopping=.true.
                termx=termx+1
                d=zero
                do i=1,n
                  d=d+(x(i)-xrec(i))**2
                enddo
                d=dsqrt(d)
c FUNCTION
                if(dabs(f-frec).gt.detfr*dabs(f) .and.
     1             dabs(f-fopt).le.options(3)*dabs(f) .and.
     2             krerun.le.3 .and. .not. constr) then
                   stopping=.false.
                   if (ii.gt.0) then
                    do i=1,ii
                     j=idx(i)
                     if (dabs(xrec(j)-x(j)).gt.detxr*dabs(x(j))) then
                       stopping=.true.
                       exit
                     endif
                    enddo
                   endif
                   if (stopping) then
                      if (dispwarn) then
                        write(iout,*) wrnmes
                        write(iout,*) warn09
                      endif
                      ng=zero
                      do i=1,n
                       x(i)=xrec(i)
                       g(i)=grec(i)
                       ng=ng+g(i)*g(i)
                      enddo
                      ng=dsqrt(ng)
                      f=frec
                      krerun=krerun+1
                      h=h1*dmax1(dx,detxr*nx)/dble(krerun)
                      warnno=2
                      endwarn=endwarn2
                      exit
                   else
                      h=h*ten
                   endif
                elseif(dabs(f-frec).gt.options(3)*dabs(f) .and.
     1             d.lt.options(2)*nx. and. constr) then
                   continue
                elseif  (dabs(f-fopt).le.options(3)*dabs(f) .or.
     1              dabs(f).le.lowfbound .or.
     2              (dabs(f-fopt).le.options(3).and.
     3               termx.ge.limxterm )) then
                  if (stopf) then
                   if (dx.le.laststep) then
                    if (warnno.eq.1 .and. ng.lt.dsqrt(options(3))) then
                       warnno=0
                    endif
                    if (.not.app) then
                      do i=1,n
                       if (dabs(g(i)).le.epsnorm2) then
                         warnno=3
                         endwarn=endwarn3
                         exit
                       endif
                      enddo
                    endif
                    if (warnno.ne.0) then
                       options(9)=-dble(warnno)-ten
                       if (dispwarn) then
                         write(iout,*) termwarn1
                         write(iout,*) endwarn
                         if (app) write(iout,*) appwarn
                       endif
                    else
                       options(9)=dble(k)
                       if (dispwarn) write(iout,*) termwarn0
                    endif
                    goto 999
                   endif
                  else
                   stopf=.true.
                  endif
                elseif (dx.lt.powerm12*dmax1(nx,one) .and.
     1                  termx.ge.limxterm ) then
                     options(9)=-four-ten
                     if (dispwarn) then
                       write(iout,*) termwarn1
                       write(iout,*) endwarn4
                       if (app) write(iout,*) appwarn
                       f=frec
                       do i=1,n
                        x(i)=xrec(i)
                       enddo
                     endif
                     goto 999
                endif
           endif
       endif
c ITERATIONS LIMIT
            if(k.eq.iterlimit) then
                options(9)=-nine
                if (dispwarn) then
                  write(iout,*) wrnmes
                  write(iout,*) warn4
                endif
                goto 999
            endif
c ----}
c ZERO GRADIENT ----{
          if (constr) then
            if (ng.le.ZeroGrad) then
                if (dispwarn) then
                  write(iout,*) termwarn1
                  write(iout,*) warn1
                endif
                options(9)=-eight
                goto 999
            endif
          else
            if (ng.le.ZeroGrad) then
             nzero=nzero+1
             if (dispwarn) then
               write(iout,*) wrnmes
               write(iout,*) warn1
             endif
             if (nzero.ge.3) then
               options(9)=-eight
               goto 999
             endif
             do i=1,n
               g0(i)=-h*g0(i)/two
             enddo
             do i=1,10
               do j=1,n
                x(j)=x(j)+g0(j)
               enddo
               call fun(n,x,f)
c			   write(iout,*) 'f'
c			   write(iout,*) f
               options(10)=options(10)+one
               if (dabs(f).ge.infty) then
                 if (dispwarn) then
                   write(iout,*) errmes
                   write(iout,*) error32
                 endif
                 options(9)=-three
                 goto 999
               endif
               if (app) then
                   do j=1,n
                     if (g0(j).ge.zero) then
                        deltax(j)=h1*ddx
                     else
                        deltax(j)=-h1*ddx
                     endif
                   enddo
                   obj=.true.
                   call apprgrdn(n,g,x,f,fun,deltax,obj)
                   options(10)=options(10)+n_float
               else
                   call grad(n,x,g)
c	               write(iout,*) 'df'
c	               write(iout,*) g
                   options(11)=options(11)+one
               endif
               ng=zero
               do j=1,n
                  ng=ng+g(j)*g(j)
               enddo
               ng=dsqrt(ng)
               if (ng.ge.infty) then
                    if (dispwarn) then
                      write(iout,*) errmes
                      write(iout,*) error42
                    endif
                    options(9)=-four
                    goto 999
               endif
               if (ng.gt.ZeroGrad) exit
             enddo
             if (ng.le.ZeroGrad) then
                if (dispwarn) then
                  write(iout,*) termwarn1
                  write(iout,*) warn1
                endif
                options(9)=-eight
                goto 999
             endif
             h=h1*dx
             exit
            endif
          endif
c ----}
c FUNCTION IS FLAT AT THE POINT ----{
          if (.not.constr .and.
     1        dabs(f-fopt).lt.dabs(fopt)*options(3) .and.
     2        kcheck.gt.5  .and. ng.lt.one ) then

           ni=0
           do i=1,n
             if (dabs(g(i)).le.epsnorm2) then
               ni=ni+1
               idx(ni)=i
             endif
           enddo
           if (ni.ge.1 .and. ni.le.n/2 .and. kflat.le.3) then
             kflat=kflat+1
             if (dispwarn) then
                write(iout,*) wrnmes
                write(iout,*) warn31
             endif
             warnno=1
             endwarn=endwarn1
             do i=1,n
               x1(i)=x(i)
             enddo
             fm=f
             do i=1,ni
              j=idx(i)
              f2=fm
              y=x(j)
              if (y.eq.zero) then
                x1(j)=one
              elseif (dabs(y).lt.one) then
                x1(j)=dsign(one,y)
              else
                x1(j)=y
              endif
              do ip=1,20
               x1(j)=x1(j)/1.15d0
               call fun(n,x1,f1)
c			   write(iout,*) 'f1'
c			   write(iout,*) f1
               options(10)=options(10)+one
               if (dabs(f1).lt.infty) then
                 if (h1*f1.gt.h1*fm) then
                   y=x1(j)
                   fm=f1
                 elseif (h1*f2.gt.h1*f1) then
                   exit
                 elseif (f2.eq.f1) then
                   x1(j)=x1(j)/1.5d0
                 endif
                 f2=f1
               endif
              enddo
              x1(j)=y
             enddo
             if (h1*fm.gt.h1*f) then
              if (app) then
                do j=1,n
                  deltax(j)=h1*ddx
                enddo
                obj=.true.
                call apprgrdn(n,gt,x1,fm,fun,deltax,obj)
                options(10)=options(10)+n_float
              else
                call grad(n,x1,gt)
c			    write(iout,*) 'df'
c			    write(iout,*) gt
                options(11)=options(11)+one
              endif
              ngt=zero
              do i=1,n
                ngt=ngt+gt(i)*gt(i)
              enddo
              if (ngt.gt.epsnorm2 .and. ngt.lt.infty) then
                if (dispwarn) write(iout,*) warn32
                do i=1,n
                 x(i)=x1(i)
                 g(i)=gt(i)
                enddo
                ng=ngt
                f=fm
                h=h1*dx/three
                options(3)=options(3)/five
                exit
              endif   !! regular gradient
             endif   !! a better value has been found
           endif   !! function is flat
          endif   !! pre-conditions are fulfilled
c ----}
       enddo   !! iterations
      enddo   !! restart

999   continue

      end


module statcell
  !! Computes statistics on cell decomposition.
  use state, only: PcmcState
  implicit none

  integer :: n_statcell !! number of steps performed
  real(8),allocatable :: numofparts_statcell(:) !! average number of particles per cell
  real(8),allocatable :: var_numofparts(:)  !! variance of the number of particles per cell
  private :: n_statcell, numofparts_statcell, var_numofparts

  integer, allocatable, private :: numofparts_cells(:)

contains

  subroutine statcell_init(st)
    !! Initialize stats on cells.
    type(PcmcState) :: st
    allocate(numofparts_statcell(st%ntotbox),var_numofparts(st%ntotbox))

    n_statcell = 0
    numofparts_statcell(1:st%ntotbox) = 0.D0
    var_numofparts(1:st%ntotbox) = 0.D0
  end subroutine statcell_init

  subroutine statcell_update(st)
    !! Updates stats on cells.
    use celldec
    type(PcmcState) :: st
    integer :: k, ncellv

    n_statcell = n_statcell + 1
    do k=1,st%ntotbox
      call celldec_numofpart(st%clistdecomp(k),numofparts_cells, ncellv)
      numofparts_statcell(k) = numofparts_statcell(k) + st%boxes(k)%n/(1.D0*ncellv)
      var_numofparts(k) = var_numofparts(k) + sum(numofparts_cells(1:ncellv)*numofparts_cells(1:ncellv))&
           /ncellv
    end do
  end subroutine statcell_update

  subroutine statcell_finish(st)
    !! Finalize stat on cells.
    type(PcmcState) :: st
    if(n_statcell > 0) then
      numofparts_statcell(1:st%ntotbox) = numofparts_statcell(1:st%ntotbox)/n_statcell
      var_numofparts(1:st%ntotbox) = var_numofparts(1:st%ntotbox)/n_statcell - &
           numofparts_statcell(1:st%ntotbox)**2
    end if
  end subroutine statcell_finish

  subroutine statcell_log(st, log_unit)
    !! Prints stats on cells to a file.
    type(PcmcState) :: st
    integer, intent(in) :: log_unit !! file descriptor
    integer :: k
    if(n_statcell > 0) then
      do k=1,st%ntotbox
        write(log_unit,'(I8,G16.8,A,G16.8)') k, numofparts_statcell(k), "±", sqrt(var_numofparts(k))
      end do
    end if
  end subroutine statcell_log

  subroutine statcell_free()
    !! Free arrays used for computation. They are initializes by [[statcell_init]].
    deallocate(numofparts_statcell,var_numofparts)
  end subroutine statcell_free

end module statcell


      PROGRAM drsdasl1
c>> 2009-10-21 DRSDASL1 Hanson/Krogh Fixed initialization.
c>> 2008-10-26 DRSDASL1 Krogh Moved Format statements up for C conv.
c>> 2008-10-24 DRSDASL1 Krogh Removed in INCLUDE statement & cDEC$...
c>> 2008-09-02 DRSDASL1 Hanson added starting computation of y'
c>> 2008-08-26 DRSDASL1 Hanson added row dimensions to evaluators
c>> 2006-04-10 DRSDASL1 Krogh Removed declaration of unused E.
c>> 2002-05-29 DRSDASL1 Krogh Changes for C conversion problem.
c>> 2001-10-11 DRSDASL1 R. J. Hanson Document Example Code,

c  Solve a C. W. Gear index=2 problem.
c  Reduce it to an index=1, solve it.
c  Reduce it further to index=0, solve it.
c  Compare results, which are equivalent but
c  not exactly equal.

c--S replaces "?": DR?DASL1, ?DASLX, ?DASLS, ?DASSF1, ?DASSF0
      EXTERNAL sdassf0,sdassf1
      INTEGER ndig,ntimes
      REAL             tol
c++S Default NDIG = 4
c++  Default NDIG = 8
c++ Substitute for NDIG below
      PARAMETER  (ndig=4)
      PARAMETER  (tol=10.e0**(-ndig))
      INTEGER liw,lrw,maxcon,neq,ldc,ltd
c     Set number of equations:
      PARAMETER  (neq=2)
c     Set maximum number of constraints.
      PARAMETER  (maxcon=2)
c     Work space sizes:
      PARAMETER  (liw=30+neq)
      PARAMETER  (lrw=45+(5+2*maxcon+4)*neq+neq**2)
      PARAMETER  (ntimes=10)
      PARAMETER  (ldc=2*neq,ltd=neq)
      INTEGER i,info(16),idid,iwork(liw)

      REAL             t,y(neq),yprime(neq),tout,rtol(neq),atol(neq),
     & rwork(lrw),soln1(ntimes,neq),soln0(ntimes,neq),
     & solp1(ntimes,neq),solp0(ntimes,neq)
      REAL             c(ldc,ltd),ftol,rnktol

      INTEGER kount0,kount1
      COMMON /counts/ kount0,kount1

   60 FORMAT (6x,'Example Results for a Transformed Index-2 DAE Problem'
     &//10x,'T',11x,'Y1/Y2',9x,'Y1P/Y2P')
   70 FORMAT (6x,'Differences, (Index-1) - (Index-0) Values'//10x,'T',
     &11x,'Y1/Y2',9x,'Y1P/Y2P')
   80 FORMAT (1p,3e15.6/15x,2e15.6/' ')
   90 FORMAT (6x,'Index-1 and Index-0 residual evalutions:',2i5)
  100 FORMAT (6x,'Initial conditions for y,y'' at t=0, index 1 system')
  110 FORMAT (6x,'Initial conditions for y,y'' at t=0, index 0 system')

c     Tolerances:
      DO 10 i=1,neq
        atol(i)=tol
        rtol(i)=tol
   10 CONTINUE
c     Setup options:
      DO 20 i=1,16
        info(i)=0
   20 CONTINUE
c     Use partial derivatives provided in evaluator routines:
      info(5)=2
c     Constrain solution with 1 constraint:
      info(10)=1
c     Compute the initial value of YPRIME(*):
      t=0
      ftol=tol
      rnktol=tol
c     Assign initial y, and guess for y', then get initial y'.
      y(1) = 1.E0
      y(2) = 0.E0
      yprime(1)=0.E0
      yprime(2)=0.E0
      CALL sdasls (sdassf1, neq, t, y, yprime, info, ftol, rnktol, c,
     & ldc, ltd, idid, rwork, lrw, iwork, liw)
      WRITE (*,60)
      WRITE (*,100)
      WRITE (*,80) t,y(1),yprime(1),y(2),yprime(2)

c     Allow up to 5000 steps:
      info(12)=5000

      DO 30 i=1,ntimes
c     Integrate from T=I-1 to TOUT=T+1.  Final TOUT=10.
        t=i-1
        tout=t+1
        CALL sdaslx (sdassf1, neq, t, y, yprime, tout, info, rtol, atol,
     &    idid, rwork, lrw, iwork, liw)
        WRITE (*,80) tout,y(1),yprime(1),y(2),yprime(2)
c     Save solution and derivative for comparison to index 0 values.
        soln1(i,1)=y(1)
        soln1(i,2)=y(2)
        solp1(i,1)=yprime(1)
        solp1(i,2)=yprime(2)
   30 CONTINUE

c     Start integration again.
      info(1)=0
      DO 40 i=1,neq
        atol(i)=tol
        rtol(i)=tol
   40 CONTINUE
c     Switch from 1 to 2 constraints, and use the index 0 system.
      info(10)=2
      WRITE (*,70)
      t=0.E0
c     Assign initial y, and guess for y', then get initial y'.
      y(1) = 1.E0
      y(2) = 0.E0
      yprime(1)=0.E0
      yprime(2)=0.E0
      CALL sdasls (sdassf0, neq, t, y, yprime, info, ftol, rnktol, c,
     & ldc, ltd, idid, rwork, lrw, iwork, liw)
      WRITE (*,110)
      WRITE (*,80) t,y(1),yprime(1),y(2),yprime(2)

      DO 50 i=1,ntimes
c     Integrate from T=I-1 to TOUT=T+1.  Final TOUT=10.
        t=i-1
        tout=t+1
        CALL sdaslx (sdassf0, neq, t, y, yprime, tout, info, rtol, atol,
     &    idid, rwork, lrw, iwork, liw)
c     Use solution and derivative differences for comparison
c     with index 1 values.
        soln0(i,1)=soln1(i,1)-y(1)
        soln0(i,2)=soln1(i,2)-y(2)
        solp0(i,1)=solp1(i,1)-yprime(1)
        solp0(i,2)=solp1(i,2)-yprime(2)
        WRITE (*,80) tout,soln0(i,1),solp0(i,1),soln0(i,2),solp0(i,2)
   50 CONTINUE
c     Print number of residual evaluations for index 1 and index 0
c     problems.
      WRITE (*,90) kount1,kount0
      END

      SUBROUTINE sdassf1 (t, y, yprime, delta, d1, ldd, cj, ires, rwork,
     & iwork)
c
c     Routine for the Gear index=2 problem.
c     One equation is differentiated to reduce it to index 1,
c     with a constraint on the index 2 equation.
      REAL             t,y(*),yprime(*),delta(*),d1(ldd,*),cj,rwork(*),
     & eta,one, two, zero
      INTEGER ires,iwork(*),ldd
      INTEGER kount0,kount1
      COMMON /counts/ kount0,kount1
      one=1.e0
      two=2.e0
      zero=0.e0
      eta=10.e0
c This is the setup call.
      IF (ires.eq.0) THEN
        kount1=0
      END IF

c The system residual value.
      IF (ires.eq.1) THEN
        delta(1)=yprime(1)+eta*t*yprime(2)+(one+eta)*y(2)-sin(t)
c This second equation comes from differentiating the second equation in
c section C.1, and subtracting the result from the first equation.
        delta(2)=y(2)-two*sin(t)
c Count function evaluations.
        kount1=kount1+1
      END IF

c The partial of the iteration matrix with respect to y.  This is an
c index 1 system.  d1 is set to 0 prior to all calls here.  Partials
c are based on equations above.  Note that \partial y'_i / y_i is c_j.
      IF (ires.eq.2) THEN
        d1(1,1)=cj
        d1(1,2)=(one+eta)+cj*eta*t
        d1(2,2)=one
      END IF

c The constraining equation after the corrector has converged.
c Both partials and residuals are required.  This is for the second
c equation in C.1.
      IF (ires.eq.5) THEN
        d1(3,1)=one
        d1(3,2)=eta*t
        delta(3)=y(1)+eta*t*y(2)-cos(t)
      END IF
c The values of IRES=6,7 and 8 occur for the starting procedure
c that solves for y'.  First the partials with respect to  t of what
c is computedb when ires is 1.  Here and below we are computing
c partials of f not of the iteration matrix.
      IF (ires.eq.6) THEN
        delta(1)=eta*yprime(2)-cos(t)
        delta(2)= -two*cos(t)
      END IF
c Compute the partial w.r.t y' of the equations defined when IRES=1
      IF (ires.eq.7) THEN
        d1(1,1)=one
        d1(1,2)=eta*t
      END IF
c Compute the partial w.r.t y of the equations defined when IRES=1
      IF (ires.eq.8) THEN
        d1(1,2)=one+eta
        d1(2,2)=one
      END IF
      END

      SUBROUTINE sdassf0 (t, y, yprime, delta, d0, ldd, cj, ires, rwork,
     & iwork)
c
c     Routine for the Gear index=2 problem.
c     One equation is differentiated twice to reduce it to index 0.
c     This gives constraints on the index 2 and index 1 equations.
      REAL             t,y(*),yprime(*),delta(*),d0(ldd,*),cj,rwork(*),
     & eta, one, two, zero
      INTEGER i,ires,iwork(*),j,ldd
      INTEGER kount0,kount1
      COMMON /counts/ kount0,kount1
      one=1.e0
      two=2.e0
      zero=0.e0
      eta=10.e0
c This is the setup call.
      IF (ires.eq.0) THEN
        kount0=0
      END IF

c The system residual value.
      IF (ires.eq.1) THEN
        delta(1)=yprime(1)+eta*t*yprime(2)+(one+eta)*y(2)-sin(t)
        delta(2)=yprime(2)-two*cos(t)
c Count function evaluations.
        kount0=kount0+1
      END IF

c The mixed partial derivative matrix.
c This is an index 0 system.
      IF (ires.eq.2) THEN
        d0(1,1)=cj
        d0(1,2)=(one+eta)+cj*eta*t
        d0(2,2)=cj
      END IF

c The constraining equations after the corrector has converged.
c Both partials and residuals are required.
      IF (ires.eq.5) THEN
        d0(3,1)=one
        d0(3,2)=eta*t
        d0(4,1)=zero
        d0(4,2)=one
        delta(3)=y(1)+eta*t*y(2)-cos(t)
        delta(4)=y(2)-two*sin(t)
      END IF
c The partial w.r.t y' for the starting procedure
c Since this is an index 0 system the cases IRES=6,8 will not occur
      IF (ires.eq.7) THEN
        d0(1,1)=one
        d0(1,2)=eta*t
        d0(2,2)=one
      END IF
      END


	!> Element that solves the trivial diagonal problem of incrementing variables
	!! @param CommonPar = ( a , a*[bi,ci,di,ei] , dummy ) where :
	!! @param Nvar = CommonPar(1) ==> a
	!! @param iShiftUi = CommonPar(see structure) ===> bi
	!! @param iShiftDeltaUi = CommonPar(see structure) ===> ci
	!! @param nodei (for each material = see structure) ===> ei
	!! @author Rocha, Felipe Figueredo

!! last update: lenU given by parameter and not by difference between iShiftU and iShiftDeltaU. NodMax is given, rather than assumed
Subroutine incrementLambdaArcLength(AE, BE, MaxLRows, XLL, NDim, iDofT, NodELT, Sol0, &
									Sol1, CommonPar, Param, JParam, DelT, DTm, Time)
!     ------------------------------------------------------------------

	use funcAux
	use TIME_STEP_ITER

	implicit none

	!   ===== SUBROUTINE ARGUMENTS  =======
	integer :: MaxLRows,Ndim,iDofT,NodELT ! all integers
	Real*8 :: DelT, DTm,Time ! all reals
	Real*8 :: AE(MaxLRows,MaxLRows), BE(MaxLRows), XLL(*), Sol0(*), Sol1(*), Param(*), JParam(*), CommonPar(*)

	!   =====   END ARGUMENTS  =======

    integer	:: iShift_dLarcLength, iShift_LarcLength, iShift_dLarcLength_acc, iShift_a, iShift_b, iShift_c
    real*8 :: lambdaIncrement, dLarcLength_acc, a, b, c, arcLength

	arcLength = CommonPar(1)
	iShift_dLarcLength = nint(CommonPar(2))
	iShift_LarcLength = nint(CommonPar(3))
	iShift_dLarcLength_acc = nint(CommonPar(4))
	iShift_a = nint(CommonPar(5))
	iShift_b = nint(CommonPar(6))
	iShift_c = nint(CommonPar(7))

	a = Sol1(iShift_a + 1)
	b = Sol1(iShift_b + 1)
	c = Sol1(iShift_c + 1)

	if(Nconverg == 1) then
		lambdaIncrement = arcLength/dsqrt(a)
		dLarcLength_acc = lambdaIncrement
	else
		lambdaIncrement = solveQuadraticEquation(a,b,c,-1) !! choose that have to change
		dLarcLength_acc = Sol1(iShift_dLarcLength_acc + 1) + lambdaIncrement
	end if

	AE(iShift_dLarcLength + 1, iShift_dLarcLength + 1) = 1.0d0
	AE(iShift_LarcLength + 1, iShift_LarcLength + 1) = 1.0d0
	AE(iShift_dLarcLength_acc + 1, iShift_dLarcLength_acc + 1) = 1.0d0

	BE(iShift_dLarcLength + 1) = lambdaIncrement
	BE(iShift_LarcLength + 1) = Sol1(iShift_LarcLength + 1) + lambdaIncrement
	BE(iShift_dLarcLength_acc + 1) = dLarcLength_acc

end subroutine

!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Subroutine incrementLambdaArcLengthS(Coupling,CommonPar,iDofT,Ndim,MaxLRows,iAdd)
!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	use funcAux
	IMPLICIT NONE
	Integer :: MaxLRows,Ndim,iDofT, iAdd, ipRow
	Integer   Coupling(MaxLRows,MaxLRows)
	Real*8 CommonPar(*)

    integer	:: iShift_dLarcLength, iShift_LarcLength, iShift_LarcLength_acc

	iShift_dLarcLength = nint(CommonPar(2))
	iShift_LarcLength = nint(CommonPar(3))
	iShift_LarcLength_acc = nint(CommonPar(4))

	Coupling(iShift_dLarcLength + 1, iShift_dLarcLength + 1) = 1
	Coupling(iShift_LarcLength + 1, iShift_LarcLength + 1) = 1
	Coupling(iShift_LarcLength_acc + 1, iShift_LarcLength_acc + 1) = 1

!~ 	call numprint(Coupling)
!~ 	pause
end Subroutine


program main
use omp_lib
include 'mpif.h'

call mpi_init(ierr )
!cx irequired=MPI_THREAD_MULTIPLE
!cx call mpi_init_thread(irequired, isupport, ierr )
call mpi_comm_rank( MPI_COMM_WORLD, my_id, ierr )
call mpi_comm_size( MPI_COMM_WORLD, npes, ierr )

ip=omp_get_thread_num()
np=omp_get_num_threads()
num_threads=omp_get_max_threads()
write(0,'(a,3i3)') "<main> outside parallel region np, num=", np, num

!$omp parallel private(ip,x,i)
ip=omp_get_thread_num()
np=omp_get_num_threads()
num_threads=omp_get_max_threads()
write(0,'(a,3i3)') "<main> inside of parallel region ip, np, num=", ip, np, num

call f_pm_initialize (10)
do i=1,3
call f_pm_start ("Section-A")
call check_thread(x)
call f_pm_stop  ("Section-A")
end do
!$omp end parallel

call f_pm_report ("")
call MPI_Finalize( ierr )
stop
end

subroutine check_thread(x)
use omp_lib
n=10000
x=0.0

!$omp parallel do private(j,k,y) shared(n) reduction(+:x)
do j=1, n
y=0.0
do k=1, n
    !cx y=y+ real(k)**1.5
y=y+ real(k)*real(j)
end do
x=x+y
end do

return
end subroutine


-- Streams.

Stream = Rec A. Unit -> { Nat, A };

hd = lambda s: Stream. (s unit).1;
tl = lambda s: Stream. ((s unit).2) as Stream;

upfrom0 = fix (lambda f: Nat -> Stream. lambda n: Nat. lambda _: Unit. { n, f (succ n) }) 0;

hd upfrom0;
hd (tl (tl (tl upfrom0)));


MODULE RungeKutta

  IMPLICIT NONE

CONTAINS

  ! @ICE block=rkSumHydra

  SUBROUTINE RK4SUM(numDim,numPoints,bufferSize,bufferInterval,h,K1,K2,K3,K4,stateData)

    INTEGER(KIND=4), INTENT(IN)      :: numDim
    INTEGER(KIND=8), INTENT(IN)      :: numPoints,bufferSize(numDim)
    INTEGER(KIND=8), INTENT(IN)      :: bufferInterval(2*numDim)
    REAL(KIND=8),    INTENT(IN)      :: h
    REAL(KIND=8),    INTENT(IN)      :: K1(numPoints)
    REAL(KIND=8),    INTENT(IN)      :: k2(numPoints)
    REAL(KIND=8),    INTENT(IN)      :: K3(numPoints)
    REAL(KIND=8),    INTENT(IN)      :: k4(numPoints)
    REAL(KIND=8),    INTENT(INOUT)   :: stateData(numPoints)

    INTEGER(KIND=8) :: I, J, K
    INTEGER(KIND=8) :: nPlane, zIndex, yIndex, yzIndex, bufferIndex, xSize
    INTEGER(KIND=8) :: iStart,iEnd,jStart,jEnd,kStart,kEnd
    REAL(KIND=8)    :: fac1,fac2

    fac1 = h/6.0_8
    fac2 = h/3.0_8

    iStart = bufferInterval(1)
    iEnd = bufferInterval(2)

    xSize = bufferSize(1)

    IF(numDim == 3) THEN

       jStart = bufferInterval(3)
       jEnd   = bufferInterval(4)
       kStart = bufferInterval(5)
       kEnd   = bufferInterval(6)

       nPlane = xSize*bufferSize(2)

       DO K = kStart, kEnd
          zIndex = (K-1)*nPlane
          DO J = jStart, jEnd
             yzIndex = (J-1)*xSize + zIndex
             DO I = iStart, iEnd
                bufferIndex = yzIndex + I
                stateData(bufferIndex) = fac1*(K1(bufferIndex)+K4(bufferIndex)) + &
                     fac2*(K2(bufferIndex) + K3(bufferIndex)) + stateData(bufferIndex)
             END DO
          END DO
       END DO

    ELSE IF (numDim == 2) THEN

       jStart = bufferInterval(3)
       jEnd   = bufferInterval(4)

       DO J = jStart, jEnd
          yIndex = (J-1)*xSize
          DO I = iStart, iEnd
             bufferIndex = yIndex + I
             stateData(bufferIndex) = fac1*(K1(bufferIndex)+K4(bufferIndex)) + &
                  fac2*(K2(bufferIndex) + K3(bufferIndex)) + stateData(bufferIndex)
          END DO
       END DO

    ELSE IF (numDim == 1) THEN

       DO bufferIndex = iStart, iEnd
          stateData(bufferIndex) = fac1*(K1(bufferIndex)+K4(bufferIndex)) + &
               fac2*(K2(bufferIndex) + K3(bufferIndex)) + stateData(bufferIndex)
       END DO

    ENDIF

  END SUBROUTINE RK4SUM

  ! @ICE endblock

END MODULE RungeKutta


    Program ztgexc_example

!     ZTGEXC Example Program Text

!     Copyright (c) 2018, Numerical Algorithms Group (NAG Ltd.)
!     For licence see
!       https://github.com/numericalalgorithmsgroup/LAPACK_Examples/blob/master/LICENCE.md

!     .. Use Statements ..
      Use lapack_example_aux, Only: nagf_file_print_matrix_complex_gen_comp
      Use lapack_interfaces, Only: ztgexc
      Use lapack_precision, Only: dp
!     .. Implicit None Statement ..
      Implicit None
!     .. Parameters ..
      Integer, Parameter :: nin = 5, nout = 6
      Logical, Parameter :: wantq = .False., wantz = .False.
!     .. Local Scalars ..
      Integer :: i, ifail, ifst, ilst, info, lda, ldb, ldq, ldz, n
!     .. Local Arrays ..
      Complex (Kind=dp), Allocatable :: a(:, :), b(:, :), q(:, :), z(:, :)
      Character (1) :: clabs(1), rlabs(1)
!     .. Executable Statements ..
      Write (nout, *) 'ZTGEXC Example Program Results'
      Write (nout, *)
      Flush (nout)
!     Skip heading in data file
      Read (nin, *)
      Read (nin, *) n
      ldq = 1
      ldz = 1
      lda = n
      ldb = n
      Allocate (a(lda,n), b(ldb,n), q(ldq,1), z(ldz,1))

!     Read A and B from data file

      Read (nin, *)(a(i,1:n), i=1, n)
      Read (nin, *)(b(i,1:n), i=1, n)

!     Read the row indices

      Read (nin, *) ifst, ilst

!     Reorder the A and B

      Call ztgexc(wantq, wantz, n, a, lda, b, ldb, q, ldq, z, ldz, ifst, ilst, &
        info)

      If (info/=0) Then
        Write (nout, 100) info, ilst
        Write (nout, *)
        Flush (nout)
      End If

!     Print reordered generalized Schur form

!     ifail: behaviour on error exit
!             =0 for hard exit, =1 for quiet-soft, =-1 for noisy-soft
      ifail = 0
      Call nagf_file_print_matrix_complex_gen_comp('General', ' ', n, n, a, &
        lda, 'Bracketed', 'F7.4', 'Reordered Schur matrix A', 'Integer', &
        rlabs, 'Integer', clabs, 80, 0, ifail)

      Write (nout, *)
      Flush (nout)

      ifail = 0
      Call nagf_file_print_matrix_complex_gen_comp('General', ' ', n, n, b, &
        ldb, 'Bracketed', 'F7.4', 'Reordered Schur matrix B', 'Integer', &
        rlabs, 'Integer', clabs, 80, 0, ifail)

100   Format (' Reordering could not be completed. INFO = ', I3, ' ILST = ', &
        I5)
    End Program


!! Copyright (C) 2016 Patrick N. Raanes
!!
!! Composed from qg.f90

module interface_mod
  use qgstep_mod
  use parameters_mod, only: dt, M, N, scheme, dtout
  use qgflux_mod
  use utils_mod, only: STRLEN

contains

  subroutine step(t, PSI, prmfname)
    real(8), dimension(1), intent(inout) :: t
    real(8), dimension(M, N), intent(inout) :: PSI
    character(STRLEN) :: prmfname

    real(8), dimension(M, N) :: Q
    real(8) :: tstop

    call parameters_read(prmfname)

    tstop = t(1) + dtout

    call laplacian(PSI, dx, dy, Q)
    Q = Q - F * PSI

    do while (t(1) < tstop)
       if (strcmp(scheme, '2ndorder') == 0) then
          call qg_step_2ndorder(t(1), PSI, Q)
       elseif (strcmp(scheme, 'rk4') == 0) then
          call qg_step_rk4(t(1), PSI, Q)
       elseif (strcmp(scheme, 'dp5') == 0) then
          call qg_step_dp5(t(1), PSI, Q)
       else
          write(stdout, *) 'Error: unknown scheme "', trim(scheme), '"'
          stop
       end if
    end do
    call calc_psi(PSI, Q, PSI)
  end subroutine step

end module interface_mod


MODULE io
    USE constants, ONLY: nl_name, u_name, x_name
    IMPLICIT NONE
    PRIVATE
    PUBLIC read_config, read_data, write_u
    REAL, PARAMETER :: G = 6.6743e-10

CONTAINS

    SUBROUTINE read_config(n_body, dt, t0, tf, eps, relax)
        INTEGER :: n_body
        DOUBLE PRECISION :: dt, t0, tf, eps, relax
        NAMELIST /config/ n_body, dt, t0, tf, eps, relax
        OPEN(10, FILE=nl_name)
        READ(10, NML=config)
        WRITE(*, NML=config)
        CLOSE(10)
    END SUBROUTINE

    SUBROUTINE read_data(n_body, masses, x0, xf)
        INTEGER :: n_body, n
        DOUBLE PRECISION :: masses(n_body), x0(3, n_body), xf(3, n_body)
        NAMELIST /init/ masses, x0, xf
        OPEN(10, FILE=nl_name)
        READ(10, NML=init)
        WRITE(*, NML=init)
        CLOSE(10)
    END SUBROUTINE

    SUBROUTINE write_u(u, n, n_body, dt, t0, m, error)
        INTEGER :: m, n, n_body, i
        DOUBLE PRECISION :: u(m), error, dt, t0
        PRINT *, 'The error is ', error
        OPEN(10, FILE=u_name)
        WRITE(10,*) u
        close(10)
        OPEN(10, FILE=x_name)
        WRITE(10,"(2A)", ADVANCE="no") 't '
        DO i=1,n_body
            WRITE(10,"(2A)", ADVANCE='no') " X"
            WRITE(10,"(I2.2)", ADVANCE='no') i
            WRITE(10,"(2A)", ADVANCE='no') " Y"
            WRITE(10,"(I2.2)", ADVANCE='no') i
            WRITE(10,"(2A)", ADVANCE='no') " Z"
            WRITE(10,"(I2.2)", ADVANCE='no') i
        END DO
        WRITE(10, *) ' '
        DO i=1,n
            WRITE(10,"(F15.2)", ADVANCE='no') dt*(i-1)
            WRITE(10,*) u((i-1)*3*n_body+1:i*3*n_body)
        END DO
    END SUBROUTINE

END MODULE


module bounding_box
use geom_utils
implicit none

private
public :: dummy_bbox
public :: curve_bbox, surface_bbox

contains

subroutine dummy_bbox()
    !> Dummy routine for f2py.
end subroutine dummy_bbox

function curve_bbox(n, cpw) result(bbox)
    !> Generate a 3-D axis-aligned bounding box from a curve.
    !> n - Number of control points - 1.
    !> cpw - Control points.
    !> bbox - Bouning box array where bbox(d, 0) is the minimum value
    !> for the d-dimension and bbox(d, 1) is the maximum value.

    ! Input
    integer, intent(in) :: n
    double precision, intent(in) :: cpw(0:n, 0:3)

    ! Output
    double precision :: bbox(0:2, 0:1)

    ! Working
    integer :: i, j
    double precision :: cp(0:n, 0:2), w(0:n)

    call dehomogenize_array1d(n, cpw, cp, w)
    bbox(0, :) = cp(0, 0)
    bbox(1, :) = cp(0, 1)
    bbox(2, :) = cp(0, 2)
    do i = 0, n
        do j = 0, 2
            if (cp(i, j) .lt. bbox(j, 0)) then
                bbox(j, 0) = cp(i, j)
            end if
            if (cp(i, j) .gt. bbox(j, 1)) then
                bbox(j, 1) = cp(i, j)
            end if
        end do
    end do

end function curve_bbox

function surface_bbox(n, m, cpw) result(bbox)
    !> Generate a 3-D axis-aligned bounding box from a surface.
    !> n - Number of control points - 1 in u-direction.
    !> m - Number of control points - 1 in v-direction.
    !> cpw - Control points.
    !> bbox - Bouning box array where bbox(d, 0) is the minimum value
    !> for the d-dimension and bbox(d, 1) is the maximum value.

    ! Input
    integer, intent(in) :: n, m
    double precision, intent(in) :: cpw(0:n, 0:m, 0:3)

    ! Output
    double precision :: bbox(0:2, 0:1)

    ! Working
    integer :: i, j, k
    double precision :: cp(0:n, 0:m, 0:2)
    double precision :: w(0:n, 0:m)

    call dehomogenize_array2d(n, m, cpw, cp, w)
    do k = 0, 2
    bbox(k, :) = cp(0, 0, k)
    end do
    do i = 0, n
        do j = 0, m
            do k = 0, 2
                if (cp(i, j, k) .lt. bbox(k, 0)) then
                    bbox(k, 0) = cp(i, j, k)
                end if
                if (cp(i, j, k) .gt. bbox(k, 1)) then
                    bbox(k, 1) = cp(i, j, k)
                end if
            end do
        end do
    end do

end function surface_bbox

end module bounding_box


      real function cmach(job)
      integer job
c
c     smach computes machine parameters of floating point
c     arithmetic for use in testing only.  not required by
c     linpack proper.
c
c     if trouble with automatic computation of these quantities,
c     they can be set by direct assignment statements.
c     assume the computer has
c
c        b = base of arithmetic
c        t = number of base  b  digits
c        l = smallest possible exponent
c        u = largest possible exponent
c
c     then
c
c        eps = b**(1-t)
c        tiny = 100.0*b**(-l+t)
c        huge = 0.01*b**(u-t)
c
c     dmach same as smach except t, l, u apply to
c     double precision.
c
c     cmach same as smach except if complex division
c     is done by
c
c        1/(x+i*y) = (x-i*y)/(x**2+y**2)
c
c     then
c
c        tiny = sqrt(tiny)
c        huge = sqrt(huge)
c
c
c     job is 1, 2 or 3 for epsilon, tiny and huge, respectively.
c
c
      real eps,tiny,huge,s
c
      eps = 1.0
   10 eps = eps/2.0
      s = 1.0 + eps
      if (s .gt. 1.0) go to 10
      eps = 2.0*eps
      cmach =eps
      if( job .eq. 1) return
c
      s = 1.0
   20 tiny = s
      s = s/16.0
      if (s*1.0 .ne. 0.0) go to 20
      tiny = (tiny/eps)*100.
      s = real((1.0,0.0)/cmplx(tiny,0.0))
      if (s .ne. 1.0/tiny) tiny = sqrt(tiny)
      huge = 1.0/tiny
      if (job .eq. 1) cmach = eps
      if (job .eq. 2) cmach = tiny
      if (job .eq. 3) cmach = huge
      return
      end


      SUBROUTINE BAND(J)
      IMPLICIT REAL*8(A-H,O-Z)
      DIMENSION E(6,7,402)
      COMMON A(6,6),B(6,6),C(6,402),D(6,13),G(6),X(6,6),Y(6,6),N,NJ
      SAVE E,NP1
  101 FORMAT (/15H DETERM=0 AT J=,I4)
      IF (J-2)  1,6,8
    1 NP1= N + 1
      DO 2 I=1,N
      D(I,2*N+1)= G(I)
      DO 2 L=1,N
      LPN= L + N
    2 D(I,LPN)= X(I,L)
      CALL MATINV (N,2*N+1,DETERM)
      IF (DETERM)  4,3,4
    3 PRINT 101, J
    4 DO 5 K=1,N
      E(K,NP1,1)= D(K,2*N+1)
      DO 5 L=1,N
      E(K,L,1)= - D(K,L)
      LPN= L + N
    5 X(K,L)= - D(K,LPN)
      RETURN
    6 DO 7 I=1,N
      DO 7 K=1,N
      DO 7 L=1,N
    7 D(I,K)= D(I,K) + A(I,L)*X(L,K)
    8 IF (J-NJ)  11,9,9
    9 DO 10 I=1,N
      DO 10 L=1,N
      G(I)= G(I) - Y(I,L)*E(L,NP1,J-2)
      DO 10 M=1,N
   10 A(I,L)= A(I,L) + Y(I,M)*E(M,L,J-2)
   11 DO 12 I=1,N
      D(I,NP1)= - G(I)
      DO 12 L=1,N
      D(I,NP1)= D(I,NP1) + A(I,L)*E(L,NP1,J-1)
      DO 12 K=1,N
   12 B(I,K)= B(I,K) + A(I,L)*E(L,K,J-1)
      CALL MATINV (N,NP1,DETERM)
      IF (DETERM)  14,13,14
   13 PRINT 101, J
   14 DO 15 K=1,N
      DO 15 M=1,NP1
   15 E(K,M,J)= - D(K,M)
      IF (J-NJ)  20,16,16
   16 DO 17 K=1,N
   17 C(K,J)= E(K,NP1,J)
      DO 18 JJ=2,NJ
      M= NJ - JJ + 1
      DO 18 K=1,N
      C(K,M)= E(K,NP1,M)
      DO 18 L=1,N
   18 C(K,M)= C(K,M) + E(K,L,M)*C(L,M+1)
      DO 19 L=1,N
      DO 19 K=1,N
   19 C(K,1)= C(K,1) + X(K,L)*C(L,3)
   20 RETURN
      END

      SUBROUTINE MATINV (N,M,DETERM)
      IMPLICIT REAL*8(A-H,O-Z)
      COMMON A(6,6),B(6,6),C(6,402),D(6,13)
      DIMENSION ID(6)
      DETERM=1.0
      DO 1 I=1,N
    1 ID(I)=0
      DO 18 NN=1,N
      BMAX=1.1
      DO 6 I=1,N
      IF(ID(I).NE.0) GO TO 6
      BNEXT=0.0
      BTRY=0.0
      DO 5 J=1,N
      IF(ID(J).NE.0) GO TO 5
      IF(DABS(B(I,J)).LE.BNEXT) GO TO 5
      BNEXT=DABS(B(I,J))
      IF(BNEXT.LE.BTRY) GO TO 5
      BNEXT=BTRY
      BTRY=DABS(B(I,J))
      JC=J
    5 CONTINUE
      IF(BNEXT.GE.BMAX*BTRY) GO TO 6
      BMAX=BNEXT/BTRY
      IROW=I
      JCOL=JC
    6 CONTINUE
      IF(ID(JC).EQ.0) GO TO 8
      DETERM=0.0
      RETURN
    8 ID(JCOL)=1
      IF(JCOL.EQ.IROW) GO TO 12
      DO 10 J=1,N
      SAVE=B(IROW,J)
      B(IROW,J)=B(JCOL,J)
   10 B(JCOL,J)=SAVE
      DO 11 K=1,M
      SAVE=D(IROW,K)
      D(IROW,K)=D(JCOL,K)
   11 D(JCOL,K)=SAVE
   12 F=1.0/B(JCOL,JCOL)
      DO 13 J=1,N
   13 B(JCOL,J)=B(JCOL,J)*F
      DO 14 K=1,M
   14 D(JCOL,K)=D(JCOL,K)*F
      DO 18 I=1,N
      IF(I.EQ.JCOL) GO TO 18
      F=B(I,JCOL)
      DO 16 J=1,N
   16 B(I,J)=B(I,J)-F*B(JCOL,J)
      DO 17 K=1,M
   17 D(I,K)=D(I,K)-F*D(JCOL,K)
   18 CONTINUE
      RETURN
      END


! =========================================================================
elemental function gsw_pt0_from_t_ice (t, p)
! =========================================================================
!
!  Calculates potential temperature of ice Ih with a reference pressure of
!  0 dbar, from in-situ temperature, t.
!
!  t   =  in-situ temperature  (ITS-90)                           [ deg C ]
!  p   =  sea pressure                                             [ dbar ]
!         ( i.e. absolute pressure - 10.1325 dbar )
!
!  pt0_ice  =  potential temperature of ice Ih with reference pressure of
!              zero dbar (ITS-90)                                 [ deg C ]
!--------------------------------------------------------------------------

use gsw_mod_teos10_constants, only : gsw_t0

use gsw_mod_toolbox, only : gsw_gibbs_ice_part_t, gsw_gibbs_ice_pt0
use gsw_mod_toolbox, only : gsw_gibbs_ice_pt0_pt0

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: t, p

real (r8) :: gsw_pt0_from_t_ice

integer :: number_of_iterations
real (r8) :: dentropy, dentropy_dt, pt0_ice
real (r8) :: pt0_ice_old, ptm_ice, true_entropy

! This is the starting polynomial for pt0 of ice Ih.
real (r8), parameter :: s1 = -2.256611570832386e-4_r8
real (r8), parameter :: s2 = -6.045305921314694e-7_r8
real (r8), parameter :: s3 =  5.546699019612661e-9_r8
real (r8), parameter :: s4 =  1.795030639186685e-11_r8
real (r8), parameter :: s5 =  1.292346094030742e-9_r8

real (r8), parameter :: p1 = -2.259745637898635e-4_r8
real (r8), parameter :: p2 =  1.486236778150360e-9_r8
real (r8), parameter :: p3 =  6.257869607978536e-12_r8
real (r8), parameter :: p4 = -5.253795281359302e-7_r8
real (r8), parameter :: p5 =  6.752596995671330e-9_r8
real (r8), parameter :: p6 =  2.082992190070936e-11_r8

real (r8), parameter :: q1 = -5.849191185294459e-15_r8
real (r8), parameter :: q2 =  9.330347971181604e-11_r8
real (r8), parameter :: q3 =  3.415888886921213e-13_r8
real (r8), parameter :: q4 =  1.064901553161811e-12_r8
real (r8), parameter :: q5 = -1.454060359158787e-10_r8
real (r8), parameter :: q6 = -5.323461372791532e-13_r8

true_entropy = -gsw_gibbs_ice_part_t(t,p)

if (t.lt.-45.0_r8 .and. t.gt.-273.0_r8) then

    pt0_ice = t + p*(p1 + p*(p2 + p3*t) + t*(p4 + t*(p5 + p6*t)))

    if (pt0_ice.lt.-gsw_t0) pt0_ice = -gsw_t0

    ! we add 0.05d0 to the initial estimate of pt0_ice at
    ! temps less than -273 to ensure that it is never less than -273.15.
    if (pt0_ice.lt.-273.0_r8) pt0_ice = pt0_ice + 0.05_r8

    dentropy_dt = -gsw_gibbs_ice_pt0_pt0(pt0_ice)

    do number_of_iterations = 1, 3
        pt0_ice_old = pt0_ice
        dentropy = -gsw_gibbs_ice_pt0(pt0_ice_old) - true_entropy
        pt0_ice = pt0_ice_old - dentropy/dentropy_dt
        ptm_ice = 0.5_r8*(pt0_ice + pt0_ice_old)
        dentropy_dt = -gsw_gibbs_ice_pt0_pt0(ptm_ice)
        pt0_ice = pt0_ice_old - dentropy/dentropy_dt
    end do

else

    pt0_ice = t + p*(s1 + t*(s2 + t*(s3 + t*s4)) + s5*p)
    dentropy_dt = -gsw_gibbs_ice_pt0_pt0(pt0_ice)

    pt0_ice_old = pt0_ice
    dentropy = -gsw_gibbs_ice_pt0(pt0_ice_old) - true_entropy

    pt0_ice = pt0_ice_old - dentropy/dentropy_dt
    ptm_ice = 0.5_r8*(pt0_ice + pt0_ice_old)
    dentropy_dt = -gsw_gibbs_ice_pt0_pt0(ptm_ice)
    pt0_ice = pt0_ice_old - dentropy/dentropy_dt

end if

if (pt0_ice.lt.-273.0_r8) then

    pt0_ice = t + p*(q1 + p*(q2 + q3*t) + t*(q4 + t*(q5 + q6*t)))

    ! add 0.01d0 to the initial estimate of pt_ice used in the derivative to
    ! ensure that it is never less than -273.15d0 because the derivative
    ! approaches zero at absolute zero.
    dentropy_dt = -gsw_gibbs_ice_pt0_pt0(pt0_ice+0.01_r8)

    do number_of_iterations = 1, 3
        pt0_ice_old = pt0_ice
        dentropy = -gsw_gibbs_ice_pt0(pt0_ice_old) - true_entropy
        pt0_ice = pt0_ice_old - dentropy/dentropy_dt
        ptm_ice = 0.5_r8*(pt0_ice + pt0_ice_old)
        ! add 0.01d0 to the estimate of ptm_ice for temperatures less than
        ! -273 to ensure that they are never less than -273.15d0 because
        ! the derivative approaches zero at absolute zero and the addition
        ! of 0.01d0 degrees c ensures that when we divide by the derivatve
        ! in the modified newton routine the function does not blow up.
        ptm_ice = ptm_ice + 0.01_r8
        dentropy_dt = -gsw_gibbs_ice_pt0_pt0(ptm_ice)
        pt0_ice = pt0_ice_old - dentropy/dentropy_dt
    end do

end if

! For temperatures less than -273.1 degsC the maximum error is less than
! 2x10^-7 degsC. For temperatures between -273.1 and 273 the maximum error
! is less than 8x10^-8 degsC, and for temperatures greater than -273 degsC the
! maximum error is 1.5x10^-12 degsC.   These errors are over the whole
! ocean depths with p varying between 0 and 10,000 dbar, while the in-situ
! temperature varied independently between -273.15 and +2 degsC.

gsw_pt0_from_t_ice = pt0_ice

return
end function

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


!> add spin-orbital term for all kinds of situation
!> we can get the orbital order from the input file fit_soc.in
!> for p orbitals, the name of the orbital should be px, py, pz
!> for d orbitals, the name of the orbital should be dxy, dyz, dxz, dx2-y2, dz2
!> the order of these orbitals can be specified by users
   subroutine  addsoc_all

      use para
      implicit none

      integer :: i, j, ir, ia
      integer :: i1, i2, j1, j2, it
      integer :: Num_wann_new

      !> includind s  px py pz dxy  dyz dxz dx2-y2 dz2
      integer :: LMMAX= 9

      real(dp), parameter :: sqrt3=1.732051d0

      real(dp) :: lambda0

      complex(dp), allocatable :: h_soc(:, :)

      complex(dp), allocatable :: soc0_p(:, :)
      complex(dp), allocatable :: soc0_d(:, :)
      complex(dp), allocatable :: soc_spd(:, :)

      integer, allocatable :: start(:)
      integer, allocatable :: orbital_index(:, :)
      real(dp), allocatable :: lambda(:, :)

      Num_wann_new= 2* Num_wann

      allocate(soc0_p(6, 6))
      allocate(soc0_d(10, 10))
      allocate(soc_spd(LMMAX*2, LMMAX*2))
      allocate(H_soc(Num_wann_new, Num_wann_new))
      allocate(start(Num_atoms))
      allocate(lambda(max_projs, Num_atoms))
      allocate(orbital_index(max_projs, Num_atoms))

      orbital_index= 0
      soc0_p= 0d0
      soc0_d= 0d0
      soc_spd= 0d0
      H_soc= 0d0

      !> soc term for p orbitals
      !> 1:3 up  4:6 dn
      !> px py pz
      soc0_p(1,2)=-zi;    soc0_p(1,6)=1.0d0
      soc0_p(2,1)= zi;    soc0_p(2,6)=-zi
      soc0_p(3,4)=-1.0d0; soc0_p(3,5)= zi
      soc0_p(4,3)=-1.0d0; soc0_p(4,5)= zi
      soc0_p(5,3)=-zi;    soc0_p(5,4)=-zi
      soc0_p(6,1)= 1.0d0; soc0_p(6,2)= zi

      soc0_p(1,2)= zi;    soc0_p(1,6)=1.0d0
      soc0_p(2,1)=-zi;    soc0_p(2,6)= zi
      soc0_p(3,4)=-1.0d0; soc0_p(3,5)=-zi
      soc0_p(4,3)=-1.0d0; soc0_p(4,5)=-zi
      soc0_p(5,3)= zi;    soc0_p(5,4)= zi
      soc0_p(6,1)= 1.0d0; soc0_p(6,2)=-zi

      !> soc term for d orbitals
      !> 1:5 up 6:10 dn
      !> d orbitals should range like this
      !> dxy, dyz, dxz, dx2-y2 dz2
      soc0_d(1, 4)= 2d0*zi; soc0_d(1, 7)= 1d0; soc0_d(1, 8)= -zi
      soc0_d(2, 3)= zi; soc0_d(2, 6)=-1d0; soc0_d(2, 9)=-zi; soc0_d(2,10)= -sqrt3*zi
      soc0_d(3, 6)= zi; soc0_d(3, 9)= -1; soc0_d(3, 10)= sqrt3
      soc0_d(4, 7)= zi; soc0_d(4, 8)=  1
      soc0_d(5, 7)= sqrt3*zi; soc0_d(5, 8)= -sqrt3
      soc0_d(6, 9)= -2d0*zi
      soc0_d(7, 8)= -zi
      do i=1, 10
         do j=i+1, 10
            soc0_d(j, i)= conjg(soc0_d(i, j))
         enddo
      enddo

      !> get soc
      soc_spd(2:4, 2:4)= soc0_p(1:3, 1:3)
      soc_spd(2:4, 11:13)= soc0_p(1:3, 4:6)
      soc_spd(11:13, 2:4)= soc0_p(4:6, 1:3)
      soc_spd(11:13, 11:13)= soc0_p(4:6, 4:6)

      !> get soc_d
      soc_spd(5:9, 5:9)= soc0_d(1:5, 1:5)
      soc_spd(5:9, 14:18)= soc0_d(1:5, 6:10)
      soc_spd(14:18, 5:9)= soc0_d(6:10, 1:5)
      soc_spd(14:18, 14:18)= soc0_d(6:10, 6:10)

      lambda = 0d0
      orbital_index= 1
      !> the orbital order for default is
      !> s  px py pz dxy  dyz dxz dx2-y2 dz2
      !> 1  2  3  4  5    6   7   8      9

      !> get the orbital order from user's input
      do ia=1, Num_atoms
         it= atom_type(ia)
         do i=1, nprojs(ia)
            select case (proj_name(i, ia))
            case ('s', 'S')
               orbital_index(i, ia)= 1
               lambda(i, ia)= 0d0
            case ('px', 'Px', 'PX')
               orbital_index(i, ia)= 2
               lambda(i, ia)= lambda_p(it)/2d0
            case ('py', 'Py', 'PY')
               orbital_index(i, ia)= 3
               lambda(i, ia)= lambda_p(it)/2d0
            case ('pz', 'Pz', 'PZ')
               orbital_index(i, ia)= 4
               lambda(i, ia)= lambda_p(it)/2d0
            case ('dxy', 'Dxy', 'DXY')
               orbital_index(i, ia)= 5
               lambda(i, ia)= lambda_d(it)/2d0
            case ('dyz', 'Dyz', 'DYZ')
               orbital_index(i, ia)= 6
               lambda(i, ia)= lambda_d(it)/2d0
            case ('dxz', 'Dxz', 'DXZ')
               orbital_index(i, ia)= 7
               lambda(i, ia)= lambda_d(it)/2d0
            case ('dx2-y2', 'Dx2-y2', 'DX2-Y2', 'dx2', 'DX2')
               orbital_index(i, ia)= 8
               lambda(i, ia)= lambda_d(it)/2d0
            case ('dz2', 'Dz2', 'DZ2')
               orbital_index(i, ia)= 9
               lambda(i, ia)= lambda_d(it)/2d0
            case default
               write(*, *) "ERROR: only support s px py pz dxy dyz dxz dx2-y2 dz2 &
                  orbitals"
               write(*, *)proj_name(i, ia)
               stop
            end select
         enddo
      enddo

      start(1)= 1
      do ia=2, Num_atoms
         start(ia)= start(ia-1)+ nprojs(ia-1)
      enddo

      do ia=1, Num_atoms
         do i=1, nprojs(ia)
            i1= start(ia)+ i- 1
            i2= orbital_index(i, ia)
            lambda0= lambda(i, ia) ! soc strength for each orbital of atoms
            do j=1, nprojs(ia)
               j1= start(ia)+ j- 1
               j2= orbital_index(j, ia)
               H_soc(i1, j1)= soc_spd(i2, j2)* lambda0
               H_soc(i1, j1+ Num_wann)= soc_spd(i2, j2+ LMMAX)* lambda0
               H_soc(i1+ Num_wann, j1)= soc_spd(i2+ LMMAX, j2)* lambda0
               H_soc(i1+ Num_wann, j1+ Num_wann)= &
                  soc_spd(i2+ LMMAX, j2+ LMMAX)* lambda0
            enddo ! j
         enddo ! i
      enddo ! ia

      do ir=1, nrpts
         if (irvec(1, ir).ne.0 .or. irvec(2, ir).ne.0 .or. irvec(3, ir).ne.0) cycle
         HmnR(:, :, ir)= HmnR(:, :, ir)+ H_soc* ndegen(ir)
      enddo

      !> write to new_hr.dat
      open(unit=105, file='new_hr.dat')
      write(105, *)'new HmnR from hand made soc'
      write(105, *)Num_wann_new
      write(105, *)nrpts
      write(105, '(15I5)')(ndegen(i), i=1, nrpts)
      do ir=1, nrpts
         do i=1, Num_wann_new
            do j=1, Num_wann_new
               write(105, '(5I5, 2f16.8)')irvec(:, ir), i, j, HmnR(i, j, ir)
            enddo
         enddo
      enddo
      close(105)

      return
   end subroutine  addsoc_all

!> add spin-orbital term for all kinds of situation
!> we can get the orbital order from the input file fit_soc.in
!> for p orbitals, the name of the orbital should be px, py, pz
!> for d orbitals, the name of the orbital should be dxy, dyz, dxz, dx2-y2, dz2
!> the order of these orbitals can be specified by users
   subroutine  addsoc_all_2D

      use para
      implicit none

      integer :: i, j, ir, ia
      integer :: i1, i2, j1, j2, it
      integer :: Num_wann_new

      !> includind s  px py pz dxy  dyz dxz dx2-y2 dz2
      integer :: LMMAX= 9

      real(dp), parameter :: sqrt3=1.732051d0

      real(dp) :: lambda0

      complex(dp), allocatable :: h_soc(:, :)

      complex(dp), allocatable :: soc0_p(:, :)
      complex(dp), allocatable :: soc0_d(:, :)
      complex(dp), allocatable :: soc_spd(:, :)

      integer, allocatable :: start(:)
      integer, allocatable :: orbital_index(:, :)
      real(dp), allocatable :: lambda(:, :)

      Num_wann_new= 2* Num_wann

      allocate(soc0_p(6, 6))
      allocate(soc0_d(10, 10))
      allocate(soc_spd(LMMAX*2, LMMAX*2))
      allocate(H_soc(Num_wann_new, Num_wann_new))
      allocate(start(Num_atoms))
      allocate(lambda(max_projs, Num_atoms))
      allocate(orbital_index(max_projs, Num_atoms))

      orbital_index= 0
      soc0_p= 0d0
      soc0_d= 0d0
      soc_spd= 0d0
      H_soc= 0d0

      !> soc term for p orbitals
      !> 1:3 up  4:6 dn
      !> px py pz
      soc0_p(1,2)=-zi;    soc0_p(1,6)=1.0d0
      soc0_p(2,1)= zi;    soc0_p(2,6)=-zi
      soc0_p(3,4)=-1.0d0; soc0_p(3,5)= zi
      soc0_p(4,3)=-1.0d0; soc0_p(4,5)= zi
      soc0_p(5,3)=-zi;    soc0_p(5,4)=-zi
      soc0_p(6,1)= 1.0d0; soc0_p(6,2)= zi

      !> soc term for d orbitals
      !> 1:5 up 6:10 dn
      !> d orbitals should range like this
      !> dxy, dyz, dxz, dx2-y2 dz2
      soc0_d(1, 4)= 2d0*zi; soc0_d(1, 7)= 1d0; soc0_d(1, 8)= -zi
      soc0_d(2, 3)= zi; soc0_d(2, 6)=-1d0; soc0_d(2, 9)=-zi; soc0_d(2,10)= -sqrt3*zi
      soc0_d(3, 6)= zi; soc0_d(3, 9)= -1; soc0_d(3, 10)= sqrt3
      soc0_d(4, 7)= zi; soc0_d(4, 8)=  1
      soc0_d(5, 7)= sqrt3*zi; soc0_d(5, 8)= -sqrt3
      soc0_d(6, 9)= -2d0*zi
      soc0_d(7, 8)= -zi
      do i=1, 10
         do j=i+1, 10
            soc0_d(j, i)= conjg(soc0_d(i, j))
         enddo
      enddo

      !> get soc
      soc_spd(2:4, 2:4)= soc0_p(1:3, 1:3)
      soc_spd(2:4, 11:13)= soc0_p(1:3, 4:6)
      soc_spd(11:13, 2:4)= soc0_p(4:6, 1:3)
      soc_spd(11:13, 11:13)= soc0_p(4:6, 4:6)

      !> get soc_d
      soc_spd(5:9, 5:9)= soc0_d(1:5, 1:5)
      soc_spd(5:9, 14:18)= soc0_d(1:5, 6:10)
      soc_spd(14:18, 5:9)= soc0_d(6:10, 1:5)
      soc_spd(14:18, 14:18)= soc0_d(6:10, 6:10)

      lambda = 0d0
      orbital_index= 1
      !> the orbital order for default is
      !> s  px py pz dxy  dyz dxz dx2-y2 dz2
      !> 1  2  3  4  5    6   7   8      9

      !> get the orbital order from user's input
      do ia=1, Num_atoms
         it= atom_type(ia)
         do i=1, nprojs(ia)
            select case (proj_name(i, ia))
            case ('s', 'S')
               orbital_index(i, ia)= 1
               lambda(i, ia)= 0d0
            case ('px', 'Px', 'PX')
               orbital_index(i, ia)= 2
               lambda(i, ia)= lambda_p(it)/2d0
            case ('py', 'Py', 'PY')
               orbital_index(i, ia)= 3
               lambda(i, ia)= lambda_p(it)/2d0
            case ('pz', 'Pz', 'PZ')
               orbital_index(i, ia)= 4
               lambda(i, ia)= lambda_p(it)/2d0
            case ('dxy', 'Dxy', 'DXY')
               orbital_index(i, ia)= 5
               lambda(i, ia)= lambda_d(it)/2d0
            case ('dyz', 'Dyz', 'DYZ')
               orbital_index(i, ia)= 6
               lambda(i, ia)= lambda_d(it)/2d0
            case ('dxz', 'Dxz', 'DXZ')
               orbital_index(i, ia)= 7
               lambda(i, ia)= lambda_d(it)/2d0
            case ('dx2-y2', 'Dx2-y2', 'DX2-Y2', 'dx2', 'DX2')
               orbital_index(i, ia)= 8
               lambda(i, ia)= lambda_d(it)/2d0
            case ('dz2', 'Dz2', 'DZ2')
               orbital_index(i, ia)= 9
               lambda(i, ia)= lambda_d(it)/2d0
            case default
               write(*, *) "ERROR: only support s px py pz dxy dyz dxz dx2-y2 dz2 &
                  orbitals"
               write(*, *)proj_name(i, ia)
               stop
            end select
         enddo
      enddo

      start(1)= 1
      do ia=2, Num_atoms
         start(ia)= start(ia-1)+ nprojs(ia-1)
      enddo

      do ia=1, Num_atoms
         do i=1, nprojs(ia)
            i1= start(ia)+ i- 1
            i2= orbital_index(i, ia)
            lambda0= lambda(i, ia) ! soc strength for each orbital of atoms
            do j=1, nprojs(ia)
               j1= start(ia)+ j- 1
               j2= orbital_index(j, ia)
               H_soc(i1, j1)= soc_spd(i2, j2)* lambda0
               H_soc(i1, j1+ Num_wann)= soc_spd(i2, j2+ LMMAX)* lambda0
               H_soc(i1+ Num_wann, j1)= soc_spd(i2+ LMMAX, j2)* lambda0
               H_soc(i1+ Num_wann, j1+ Num_wann)= &
                  soc_spd(i2+ LMMAX, j2+ LMMAX)* lambda0
            enddo ! j
         enddo ! i
      enddo ! ia

      do ir=1, nrpts
         if (irvec_2d(1, ir).ne.0 .or. irvec_2d(2, ir).ne.0) cycle
         HmnR(:, :, ir)= HmnR(:, :, ir)+ H_soc* ndegen(ir)
      enddo

      !> write to new_hr.dat
      open(unit=105, file='new_hr.dat')
      write(105, *)'new HmnR from hand made soc'
      write(105, *)Num_wann_new
      write(105, *)nrpts
      write(105, '(15I5)')(ndegen(i), i=1, nrpts)
      do ir=1, nrpts
         do i=1, Num_wann_new
            do j=1, Num_wann_new
               write(105, '(4I5, 2f16.8)')irvec_2d(:, ir), i, j, HmnR(i, j, ir)
            enddo
         enddo
      enddo
      close(105)

      return
   end subroutine  addsoc_all_2D

!> add spin-orbital term for all kinds of situation
!> we can get the orbital order from the input file fit_soc.in
!> for p orbitals, the name of the orbital should be px, py, pz
!> for d orbitals, the name of the orbital should be dxy, dyz, dxz, dx2-y2, dz2
!> the order of these orbitals can be specified by users
   subroutine  addsoc_magmom_all_2D

      use para
      implicit none

      integer :: i, j, ir, ia
      integer :: i1, i2, j1, j2, it
      integer :: Num_wann_new

      !> includind s  px py pz dxy  dyz dxz dx2-y2 dz2
      integer :: LMMAX= 9

      real(dp), parameter :: sqrt3=1.732051d0

      real(dp) :: lambda0

      complex(dp), allocatable :: h_soc(:, :)

      complex(dp), allocatable :: soc0_p(:, :)
      complex(dp), allocatable :: soc0_d(:, :)
      complex(dp), allocatable :: soc_spd(:, :)

      integer, allocatable :: start(:)
      integer, allocatable :: orbital_index(:, :)
      real(dp), allocatable :: lambda(:, :)
      real(dp), allocatable :: hx(:, :)
      real(dp), allocatable :: hy(:, :)
      real(dp), allocatable :: hz(:, :)
      real(dp), allocatable :: H_mag(:, :)

      Num_wann_new= 2* Num_wann

      allocate(soc0_p(6, 6))
      allocate(soc0_d(10, 10))
      allocate(soc_spd(LMMAX*2, LMMAX*2))
      allocate(H_soc(Num_wann_new, Num_wann_new))
      allocate(H_mag(Num_wann_new, Num_wann_new))
      allocate(start(Num_atoms))
      allocate(lambda(max_projs, Num_atoms))
      allocate(hx(max_projs, Num_atoms))
      allocate(hy(max_projs, Num_atoms))
      allocate(hz(max_projs, Num_atoms))
      allocate(orbital_index(max_projs, Num_atoms))

      orbital_index= 0
      soc0_p= 0d0
      soc0_d= 0d0
      soc_spd= 0d0
      H_soc= 0d0
      H_mag= 0d0
      Hx= 0d0
      Hy= 0d0
      Hz= 0d0

      !> soc term for p orbitals
      !> 1:3 up  4:6 dn
      !> px py pz
      soc0_p(1,2)=-zi;    soc0_p(1,6)=1.0d0
      soc0_p(2,1)= zi;    soc0_p(2,6)=-zi
      soc0_p(3,4)=-1.0d0; soc0_p(3,5)= zi
      soc0_p(4,3)=-1.0d0; soc0_p(4,5)= zi
      soc0_p(5,3)=-zi;    soc0_p(5,4)=-zi
      soc0_p(6,1)= 1.0d0; soc0_p(6,2)= zi

      !> soc term for d orbitals
      !> 1:5 up 6:10 dn
      !> d orbitals should range like this
      !> dxy, dyz, dxz, dx2-y2 dz2
      soc0_d(1, 4)= 2d0*zi; soc0_d(1, 7)= 1d0; soc0_d(1, 8)= -zi
      soc0_d(2, 3)= zi; soc0_d(2, 6)=-1d0; soc0_d(2, 9)=-zi; soc0_d(2,10)= -sqrt3*zi
      soc0_d(3, 6)= zi; soc0_d(3, 9)= -1; soc0_d(3, 10)= sqrt3
      soc0_d(4, 7)= zi; soc0_d(4, 8)=  1
      soc0_d(5, 7)= sqrt3*zi; soc0_d(5, 8)= -sqrt3
      soc0_d(6, 9)= -2d0*zi
      soc0_d(7, 8)= -zi
      do i=1, 10
         do j=i+1, 10
            soc0_d(j, i)= conjg(soc0_d(i, j))
         enddo
      enddo

      !> get soc
      soc_spd(2:4, 2:4)= soc0_p(1:3, 1:3)
      soc_spd(2:4, 11:13)= soc0_p(1:3, 4:6)
      soc_spd(11:13, 2:4)= soc0_p(4:6, 1:3)
      soc_spd(11:13, 11:13)= soc0_p(4:6, 4:6)

      !> get soc_d
      soc_spd(5:9, 5:9)= soc0_d(1:5, 1:5)
      soc_spd(5:9, 14:18)= soc0_d(1:5, 6:10)
      soc_spd(14:18, 5:9)= soc0_d(6:10, 1:5)
      soc_spd(14:18, 14:18)= soc0_d(6:10, 6:10)

      lambda = 0d0
      orbital_index= 1
      !> the orbital order for default is
      !> s  px py pz dxy  dyz dxz dx2-y2 dz2
      !> 1  2  3  4  5    6   7   8      9

      !> get the orbital order from user's input
      do ia=1, Num_atoms
         it= atom_type(ia)
         do i=1, nprojs(ia)
            select case (proj_name(i, ia))
            case ('s', 'S')
               orbital_index(i, ia)= 1
               lambda(i, ia)= 0d0
            case ('px', 'Px', 'PX')
               orbital_index(i, ia)= 2
               lambda(i, ia)= lambda_p(it)/2d0
               hx(i, ia)= magmomx_p(ia)
               hy(i, ia)= magmomy_p(ia)
               hz(i, ia)= magmomz_p(ia)
            case ('py', 'Py', 'PY')
               orbital_index(i, ia)= 3
               lambda(i, ia)= lambda_p(it)/2d0
               hx(i, ia)= magmomx_p(ia)
               hy(i, ia)= magmomy_p(ia)
               hz(i, ia)= magmomz_p(ia)
            case ('pz', 'Pz', 'PZ')
               orbital_index(i, ia)= 4
               lambda(i, ia)= lambda_p(it)/2d0
               hx(i, ia)= magmomx_p(ia)
               hy(i, ia)= magmomy_p(ia)
               hz(i, ia)= magmomz_p(ia)
            case ('dxy', 'Dxy', 'DXY')
               orbital_index(i, ia)= 5
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case ('dyz', 'Dyz', 'DYZ')
               orbital_index(i, ia)= 6
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case ('dxz', 'Dxz', 'DXZ')
               orbital_index(i, ia)= 7
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case ('dx2-y2', 'Dx2-y2', 'DX2-Y2', 'dx2', 'DX2')
               orbital_index(i, ia)= 8
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case ('dz2', 'Dz2', 'DZ2')
               orbital_index(i, ia)= 9
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case default
               write(*, *) "ERROR: only support s px py pz dxy dyz dxz dx2-y2 dz2 &
                  orbitals"
               write(*, *)proj_name(i, ia)
               stop
            end select
         enddo
      enddo

      start(1)= 1
      do ia=2, Num_atoms
         start(ia)= start(ia-1)+ nprojs(ia-1)
      enddo

      !> add soc
      do ia=1, Num_atoms
         do i=1, nprojs(ia)
            i1= start(ia)+ i- 1
            i2= orbital_index(i, ia)
            lambda0= lambda(i, ia) ! soc strength for each orbital of atoms
            do j=1, nprojs(ia)
               j1= start(ia)+ j- 1
               j2= orbital_index(j, ia)
               H_soc(i1, j1)= soc_spd(i2, j2)* lambda0
               H_soc(i1, j1+ Num_wann)= soc_spd(i2, j2+ LMMAX)* lambda0
               H_soc(i1+ Num_wann, j1)= soc_spd(i2+ LMMAX, j2)* lambda0
               H_soc(i1+ Num_wann, j1+ Num_wann)= &
                  soc_spd(i2+ LMMAX, j2+ LMMAX)* lambda0
            enddo ! j
         enddo ! i
      enddo ! ia

      !> add magnetic moment
      do ia=1, Num_atoms
         do i=1, nprojs(ia)
            i1= start(ia)+ i- 1
            H_mag(i1, i1)= hz(i, ia)
            H_mag(i1+Num_wann, i1+Num_wann)= -hz(i, ia)
            H_mag(i1, i1+Num_wann)= hx(i, ia)- zi*hy(i, ia)
            H_mag(i1+Num_wann, i1)= hx(i, ia)+ zi*hy(i, ia)
         enddo ! i
      enddo ! ia

      do ir=1, nrpts
         if (irvec_2d(1, ir).ne.0 .or. irvec_2d(2, ir).ne.0) cycle
         HmnR(:, :, ir)= HmnR(:, :, ir)+ (H_soc+ H_mag)* ndegen(ir)
      enddo

      !> write to new_hr.dat
      open(unit=105, file='new_hr.dat')
      write(105, *)'new HmnR from hand made soc'
      write(105, *)Num_wann_new
      write(105, *)nrpts
      write(105, '(15I5)')(ndegen(i), i=1, nrpts)
      do ir=1, nrpts
         do i=1, Num_wann_new
            do j=1, Num_wann_new
               write(105, '(4I5, 2f16.8)')irvec_2d(:, ir), i, j, HmnR(i, j, ir)
            enddo
         enddo
      enddo
      close(105)

      return
   end subroutine  addsoc_magmom_all_2D

!> add spin-orbital term for all kinds of situation
!> we can get the orbital order from the input file fit_soc.in
!> for p orbitals, the name of the orbital should be px, py, pz
!> for d orbitals, the name of the orbital should be dxy, dyz, dxz, dx2-y2, dz2
!> the order of these orbitals can be specified by users
   subroutine  addsoc_magmom_all_3D

      use para
      implicit none

      integer :: i, j, ir, ia
      integer :: i1, i2, j1, j2, it
      integer :: Num_wann_new

      !> includind s  px py pz dxy  dyz dxz dx2-y2 dz2
      integer :: LMMAX= 9

      real(dp), parameter :: sqrt3=1.732051d0

      real(dp) :: lambda0

      complex(dp), allocatable :: h_soc(:, :)

      complex(dp), allocatable :: soc0_p(:, :)
      complex(dp), allocatable :: soc0_d(:, :)
      complex(dp), allocatable :: soc_spd(:, :)

      integer, allocatable :: start(:)
      integer, allocatable :: orbital_index(:, :)
      real(dp), allocatable :: lambda(:, :)
      real(dp), allocatable :: hx(:, :)
      real(dp), allocatable :: hy(:, :)
      real(dp), allocatable :: hz(:, :)
      real(dp), allocatable :: H_mag(:, :)

      Num_wann_new= 2* Num_wann

      allocate(soc0_p(6, 6))
      allocate(soc0_d(10, 10))
      allocate(soc_spd(LMMAX*2, LMMAX*2))
      allocate(H_soc(Num_wann_new, Num_wann_new))
      allocate(H_mag(Num_wann_new, Num_wann_new))
      allocate(start(Num_atoms))
      allocate(lambda(max_projs, Num_atoms))
      allocate(hx(max_projs, Num_atoms))
      allocate(hy(max_projs, Num_atoms))
      allocate(hz(max_projs, Num_atoms))
      allocate(orbital_index(max_projs, Num_atoms))

      orbital_index= 0
      soc0_p= 0d0
      soc0_d= 0d0
      soc_spd= 0d0
      H_soc= 0d0
      H_mag= 0d0
      Hx= 0d0
      Hy= 0d0
      Hz= 0d0

      !> soc term for p orbitals
      !> 1:3 up  4:6 dn
      !> px py pz
      soc0_p(1,2)=-zi;    soc0_p(1,6)=1.0d0
      soc0_p(2,1)= zi;    soc0_p(2,6)=-zi
      soc0_p(3,4)=-1.0d0; soc0_p(3,5)= zi
      soc0_p(4,3)=-1.0d0; soc0_p(4,5)= zi
      soc0_p(5,3)=-zi;    soc0_p(5,4)=-zi
      soc0_p(6,1)= 1.0d0; soc0_p(6,2)= zi

      !> soc term for d orbitals
      !> 1:5 up 6:10 dn
      !> d orbitals should range like this
      !> dxy, dyz, dxz, dx2-y2 dz2
      soc0_d(1, 4)= 2d0*zi; soc0_d(1, 7)= 1d0; soc0_d(1, 8)= -zi
      soc0_d(2, 3)= zi; soc0_d(2, 6)=-1d0; soc0_d(2, 9)=-zi; soc0_d(2,10)= -sqrt3*zi
      soc0_d(3, 6)= zi; soc0_d(3, 9)= -1; soc0_d(3, 10)= sqrt3
      soc0_d(4, 7)= zi; soc0_d(4, 8)=  1
      soc0_d(5, 7)= sqrt3*zi; soc0_d(5, 8)= -sqrt3
      soc0_d(6, 9)= -2d0*zi
      soc0_d(7, 8)= -zi
      do i=1, 10
         do j=i+1, 10
            soc0_d(j, i)= conjg(soc0_d(i, j))
         enddo
      enddo

      !> get soc
      soc_spd(2:4, 2:4)= soc0_p(1:3, 1:3)
      soc_spd(2:4, 11:13)= soc0_p(1:3, 4:6)
      soc_spd(11:13, 2:4)= soc0_p(4:6, 1:3)
      soc_spd(11:13, 11:13)= soc0_p(4:6, 4:6)

      !> get soc_d
      soc_spd(5:9, 5:9)= soc0_d(1:5, 1:5)
      soc_spd(5:9, 14:18)= soc0_d(1:5, 6:10)
      soc_spd(14:18, 5:9)= soc0_d(6:10, 1:5)
      soc_spd(14:18, 14:18)= soc0_d(6:10, 6:10)

      lambda = 0d0
      orbital_index= 1
      !> the orbital order for default is
      !> s  px py pz dxy  dyz dxz dx2-y2 dz2
      !> 1  2  3  4  5    6   7   8      9

      !> get the orbital order from user's input
      do ia=1, Num_atoms
         it= atom_type(ia)
         do i=1, nprojs(ia)
            select case (proj_name(i, ia))
            case ('s', 'S')
               orbital_index(i, ia)= 1
               lambda(i, ia)= 0d0
            case ('px', 'Px', 'PX')
               orbital_index(i, ia)= 2
               lambda(i, ia)= lambda_p(it)/2d0
               hx(i, ia)= magmomx_p(ia)
               hy(i, ia)= magmomy_p(ia)
               hz(i, ia)= magmomz_p(ia)
            case ('py', 'Py', 'PY')
               orbital_index(i, ia)= 3
               lambda(i, ia)= lambda_p(it)/2d0
               hx(i, ia)= magmomx_p(ia)
               hy(i, ia)= magmomy_p(ia)
               hz(i, ia)= magmomz_p(ia)
            case ('pz', 'Pz', 'PZ')
               orbital_index(i, ia)= 4
               lambda(i, ia)= lambda_p(it)/2d0
               hx(i, ia)= magmomx_p(ia)
               hy(i, ia)= magmomy_p(ia)
               hz(i, ia)= magmomz_p(ia)
            case ('dxy', 'Dxy', 'DXY')
               orbital_index(i, ia)= 5
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case ('dyz', 'Dyz', 'DYZ')
               orbital_index(i, ia)= 6
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case ('dxz', 'Dxz', 'DXZ')
               orbital_index(i, ia)= 7
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case ('dx2-y2', 'Dx2-y2', 'DX2-Y2', 'dx2', 'DX2')
               orbital_index(i, ia)= 8
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case ('dz2', 'Dz2', 'DZ2')
               orbital_index(i, ia)= 9
               lambda(i, ia)= lambda_d(it)/2d0
               hx(i, ia)= magmomx_d(ia)
               hy(i, ia)= magmomy_d(ia)
               hz(i, ia)= magmomz_d(ia)
            case default
               write(*, *) "ERROR: only support s px py pz dxy dyz dxz dx2-y2 dz2 &
                  orbitals"
               write(*, *)proj_name(i, ia)
               stop
            end select
         enddo
      enddo

      start(1)= 1
      do ia=2, Num_atoms
         start(ia)= start(ia-1)+ nprojs(ia-1)
      enddo

      !> add soc
      do ia=1, Num_atoms
         do i=1, nprojs(ia)
            i1= start(ia)+ i- 1
            i2= orbital_index(i, ia)
            lambda0= lambda(i, ia) ! soc strength for each orbital of atoms
            do j=1, nprojs(ia)
               j1= start(ia)+ j- 1
               j2= orbital_index(j, ia)
               H_soc(i1, j1)= soc_spd(i2, j2)* lambda0
               H_soc(i1, j1+ Num_wann)= soc_spd(i2, j2+ LMMAX)* lambda0
               H_soc(i1+ Num_wann, j1)= soc_spd(i2+ LMMAX, j2)* lambda0
               H_soc(i1+ Num_wann, j1+ Num_wann)= &
                  soc_spd(i2+ LMMAX, j2+ LMMAX)* lambda0
            enddo ! j
         enddo ! i
      enddo ! ia

      !> add magnetic moment
      do ia=1, Num_atoms
         do i=1, nprojs(ia)
            i1= start(ia)+ i- 1
            H_mag(i1, i1)= hz(i, ia)
            H_mag(i1+Num_wann, i1+Num_wann)= -hz(i, ia)
            H_mag(i1, i1+Num_wann)= hx(i, ia)- zi*hy(i, ia)
            H_mag(i1+Num_wann, i1)= hx(i, ia)+ zi*hy(i, ia)
         enddo ! i
      enddo ! ia

      do ir=1, nrpts
         if (irvec(1, ir).ne.0 .or. irvec(2, ir).ne.0 .or. irvec(3, ir).ne.0) cycle
         HmnR(:, :, ir)= HmnR(:, :, ir)+ (H_soc+ H_mag)* ndegen(ir)
      enddo

      !> write to new_hr.dat
      open(unit=105, file='new_hr.dat')
      write(105, *)'new HmnR from hand made soc'
      write(105, *)Num_wann_new
      write(105, *)nrpts
      write(105, '(15I5)')(ndegen(i), i=1, nrpts)
      do ir=1, nrpts
         do i=1, Num_wann_new
            do j=1, Num_wann_new
               write(105, '(5I5, 2f16.8)')irvec(:, ir), i, j, HmnR(i, j, ir)
            enddo
         enddo
      enddo
      close(105)

      return
   end subroutine  addsoc_magmom_all_3D


      subroutine pnordr(x,y,net,l,ib,ie)
C...01/09/78
      dimension x(1),y(1),net(1)
      i1=ib+1
      ir=iabs(net(ib))
      k1=iabs(net(i1))
      s1=cosin2(y(l)-y(ir),x(ir)-x(l),x(k1)-x(l),y(k1)-y(l))
      c1=cosin2(x(ir)-x(l),y(ir)-y(l),x(k1)-x(l),y(k1)-y(l))
      m=i1+1
      do 1 i=m,ie
      k2=iabs(net(i))
      s2=cosin2(y(l)-y(ir),x(ir)-x(l),x(k2)-x(l),y(k2)-y(l))
      c2=cosin2(x(ir)-x(l),y(ir)-y(l),x(k2)-x(l),y(k2)-y(l))
      if(c1.ge.0) goto 3
      if(c2.ge.0) goto 5
      if(s1-s2) 6,1,1
    6 s1=s2
      c1=c2
      i1=i
      go to 1
    5 if(s2) 1,1,6
    3 if(c2.ge.0) goto 8
      if(s1) 6,1,1
    8 if(s1.ge.0) goto 10
      if(s2.ge.0) goto 6
      if(s1-s2) 1,1,6
   10 if(s2.le.0) goto 1
      if(s1-s2) 1,6,6
    1 continue
      i=net(i1)
      net(i1)=net(ib+1)
      net(ib+1)=i
      return
      end