C     =================================================================
C     Module: Augmented Lagrangian subroutines
C     =================================================================

C     Last update of any of the component of this module: 
C
C     January 16, 2012.

C     ******************************************************************
C     ******************************************************************

      subroutine easyalabb(n,x,l,u,m,lambda,rho,equatn,linear,linvar,ml,
     +meq,alin,acol,aval,annz,b,fknown,epsgap,epsfeas,epsopt,sparse,
     +maxoutit,iprint,ncomp,macheps,bignum,f,cnorm,snorm,nlpsupn,gap,
     +ldh,H,outiter,totnnlp,totnnodes,inform,wi1,wd1,wd2,wd3,wd4,wd5,
     +wd6,wd7,wd8,wd9,wd10,wd11,wd12)

      implicit none

C     SCALAR ARGUMENTS
      logical sparse
      integer annz,inform,iprint,ldh,m,maxoutit,meq,ml,n,ncomp,outiter,
     +        totnnlp,totnnodes
      double precision bignum,epsgap,epsfeas,epsopt,f,fknown,macheps,
     +        nlpsupn,cnorm,snorm,gap

C     ARRAY ARGUMENTS
      integer alin(annz),acol(annz),wi1(ml)
      logical equatn(m),linear(m),linvar(n)
      double precision aval(annz),b(ml),l(n),lambda(m),H(ldh,n),rho(m),
     +        u(n),wd1(m),wd2(m),wd3(n),wd4(m),wd5(m),wd6(n),wd7(n),
     +        wd8(n),wd9(n),wd10(n),wd11(n),wd12(n),x(n)

C     LOCAL SCALARS
      integer maxitncp
      double precision epsgaini,epsgafin,epsgacon,rhomult,rhofrac,
     +        lammin,lammax

C     SET AUGMENTED-LAGRANGIAN PARAMETERS

      lammin    = - 1.0d+20         
      lammax    =   1.0d+20         

C     PARA TESTAR PENALIDAD PURA!!!!!!!!
c     lammin    =   0.0d0
c     lammax    =   0.0d0

      rhomult   =    10.0d0
      rhofrac   =     0.5d0

      epsgaini  =   1.0d-01
      epsgacon  =   1.0d-01
      epsgafin  =    epsgap

      maxitncp  =         9

C     CALL ALABB

      call alabb(n,x,l,u,m,lambda,rho,equatn,linear,linvar,ml,meq,alin,
     +acol,aval,annz,b,fknown,lammin,lammax,rhomult,rhofrac,epsgaini,
     +epsgafin,epsgacon,epsfeas,epsopt,sparse,maxitncp,maxoutit,
     +iprint,ncomp,macheps,bignum,f,wd1,cnorm,snorm,wd3,nlpsupn,gap,ldh,
     +H,outiter,totnnlp,totnnodes,inform,wd4,wi1,wd6,wd7,wd8,wd9,wd10,
     +wd11,wd12)

      end

C     ******************************************************************
C     ******************************************************************

      subroutine alabb(n,x,l,u,m,lambda,rho,equatn,linear,linvar,ml,meq,
     +alin,acol,aval,annz,b,fknown,lammin,lammax,rhomult,rhofrac,
     +epsgaini,epsgafin,epsgacon,epsfeas,epsopt,sparse,maxitncp,
     +maxoutit,iprint,ncomp,macheps,bignum,f,c,cnorm,snorm,nl,nlpsupn,
     +algap,ldh,H,outiter,totnnlp,totnnodes,inform,lambar,wi1,wd1,wd2,
     +wd3,wd4,wd5,wd6,wd7)

      implicit none

C     Solves the general-constrained minimization problem
C
C     min f(x)
C
C     subject to
C
C             c_j(x)  = 0, j in E,
C             c_j(x) <= 0, j in I,
C             l <= x <= u,
C
C     where E is the set of indices of the equality constraints, I is
C     the set of indices of the inequality constraints, and there are
C     n variables and m constraints, using the method of multipliers 
C     described in:
C 
C     R. Andreani, E. G. Birgin, J. M. Martínez and M. L. Schuverdt, 
C     "On Augmented Lagrangian methods with general lower-level 
C     constraints", Technical Report MCDO-050304, Department of Applied 
C     Mathematics, UNICAMP, Brazil, 2005.

C     SCALAR ARGUMENTS
      logical sparse
      integer annz,inform,iprint,ldh,m,maxitncp,maxoutit,meq,ml,n,ncomp,
     +        outiter,totnnlp,totnnodes
      double precision bignum,epsfeas,epsgaini,epsgafin,epsgacon,epsopt,
     +        f,fknown,lammax,lammin,macheps,nlpsupn,rhofrac,rhomult,
     +        cnorm,snorm,algap

C     ARRAY ARGUMENTS
      integer alin(annz),acol(annz),wi1(ml)
      logical equatn(m),linear(m),linvar(n)
      double precision aval(annz),b(ml),c(m),H(ldh,n),l(n),lambar(m),
     +        lambda(m),nl(n),rho(m),u(n),wd1(n),
     +        wd2(n),wd3(n),wd4(n),wd5(n),wd6(n),wd7(n),x(n)

C     LOCAL SCALARS
      logical infeassub
      integer flag,i,mprint,nprint,nnlp,nnodes,rhoincr
      double precision al,ck,desfeas,epsgak,gammak,snormprev,gapk,
     +        rhoini,sumc,fint(2),lu(1000),cnorma,cnormb

C     SET UP SOME CONSTANTS

C     just for printing
      nprint   = min0(n,ncomp)
      mprint   = min0(m,ncomp)

C     INTIALIZATION

      outiter   = 0
      totnnlp   = 0
      totnnodes = 0

C     Compute objective function and constraints

      call evalobjc(n,x,f,m,c,inform)
      if ( inform .lt. 0 ) then

          if ( iprint .ge. 1 ) then
              write(*, 2000) inform
              write(10,2000) inform
          end if

          return
      end if

C     Compute complementarity and feasibility violations

      snormprev = bignum

      cnorma  = 0.0d0
      cnormb  = 0.0d0
      snorm   = 0.0d0
      do i = 1,m
          if ( equatn(i) ) then
              cnorma  = cnorma + c(i) ** 2
              snorm   = max( snorm, abs( c(i) ) )
          else
              cnormb  = cnormb + max( 0.0d0, c(i) ) ** 2
              snorm   = max( snorm, 
     +                       abs( min( - c(i), lambda(i) / rho(1) ) ) )
          end if
      end do
      cnorm  = sqrt( cnorma ) + sqrt( cnormb )

C     PRINT INITIAL INFORMATION

      if  ( iprint .ge. 1 ) then
          write(*,  900) n,m
          write(*,  910) nprint,(l(i),i=1,nprint)
          write(*,  920) nprint,(u(i),i=1,nprint)
          write(*, 1020) f,cnorm,snorm

          write(10, 900) n,m
          write(10, 910) nprint,(l(i),i=1,nprint)
          write(10, 920) nprint,(u(i),i=1,nprint)
          write(10,1020) f,cnorm,snorm
      end if

      write(*,332) outiter,x(1),f,cnorm

C     ==================================================================
C     Main loop
C     ==================================================================

 100  continue

C     ==================================================================
C     Test stopping criteria
C     ==================================================================

      if ( outiter .gt. 0 ) then

C         Problem is infeasible

          if ( gammak + gapk .lt. - ck .or. infeassub ) then
              inform = 0

              if ( iprint .ge. 1 ) then
                  write(*, 1060) inform
                  write(10,1060) inform
              end if

              return
          end if

C         Solution found!

          if ( cnorm .le. epsfeas .and. gammak + gapk .le. epsopt ) then
              inform = 1

              if ( iprint .ge. 1 ) then
                  write(*, 1070) inform
                  write(10,1070) inform
              end if

              return
          end if

      end if

C     Number of iterations exhausted

      if ( outiter .ge. maxoutit ) then
          inform = 2

          if ( iprint .ge. 1 ) then
              write(*, 1080) inform,maxoutit
              write(10,1080) inform,maxoutit
          end if

          return
      end if
 
C     ==================================================================
C     Iteration
C     ==================================================================

      outiter = outiter + 1      

C     ==================================================================
C     Set Lagrange multipliers and penalty parameter
C     ==================================================================

C     Compute safeguarded Lagrange multipliers

      if ( outiter .ne. 1 ) then
          do i = 1,m
              call evaldpdy(c(i),rho(1),lambar(i),equatn(i),lambda(i))
          end do
      end if

      do i = 1,m
          lambar(i) = max( lammin, min( lambda(i), lammax ) )
      end do

C     Set (or update) penalty parameter

      if ( outiter .ne. 1 ) then
c         desfeas = max( epsfeas, rhofrac * snormprev )
          desfeas = rhofrac * snormprev

          if ( snorm .gt. desfeas .and. cnorm .gt. epsfeas ) then
              do i = 1,m
                  rho(i) = rhomult * rho(i)
              end do

              if ( iprint .ge. 1 ) then
                  write(*, 1030) desfeas,rhomult
                  write(10,1030) desfeas,rhomult
              end if
          else
              if ( iprint .ge. 1 ) then
                  write(*,*) 'snorm = ',snorm,' desfeas = ',desfeas
                  write(*, 1050)
                  write(10,1050)
              end if
          end if

      else
          sumc = 0.0d0
          do i = 1,m
              if ( equatn(i) .or. c(i) .gt. 0.0d0 ) then
                  sumc = sumc + 0.5d0 * c(i) ** 2
              end if
          end do

          if ( sumc .eq. 0.0d0 ) then
              rhoini = 10.0d0
          else
              rhoini = 10.0d0 * max( 1.0d0, abs( f ) ) / sumc
              rhoini = max( 1.0d-06, min( rhoini, 10.0d0 ) )
          end if 

          do i = 1,m
              rho(i) = rhoini
          end do
      end if

C     ==================================================================
C     Solve the subproblem
C     ==================================================================

C     Set optimality requeriment for the subproblem

      if ( outiter .eq. 1 ) then
          epsgak = epsgaini
      else
          epsgak = max( epsgafin, epsgak * epsgacon )
          if ( epsgak .le. 2.0d0 * epsgafin ) epsgak = epsgafin
      end if

C     Call the inner-solver

      call abb(n,l,u,m,lambar,rho,equatn,linear,linvar,ml,meq,alin,
     +acol,aval,annz,b,fknown,epsgak,epsfeas,epsopt,sparse,al,x,nl,
     +nlpsupn,gapk,nnodes,inform)

      nnlp = 0

      totnnlp   = totnnlp   + nnlp
      totnnodes = totnnodes + nnodes

      if ( inform .lt. 0 ) then

          if ( iprint .ge. 1 ) then
              write(*, 2000) inform
              write(10,2000) inform
          end if

          return
      end if

C     TEMPORARIO !!!!!!!!!!!!!!
      infeassub = .false.
      if ( al .eq. 1.0d+20 ) then
          write(*,*) 'Subproblem was detected to be infeasible!'
          infeassub = .true.
      end if

C     ==================================================================
C     Prepare for the next iteration
C     ==================================================================

C     Compute objective function and constraints

      call evalobjc(n,x,f,m,c,inform)
      if ( inform .lt. 0 ) then

          if ( iprint .ge. 1 ) then
              write(*, 2000) inform
              write(10,2000) inform
          end if

          return
      end if

C     Compute gammak, ck, and complementarity and feasibility violations

      do i = 1,n
          lu(2*i-1) = l(i)
          lu(2*i)   = u(i)
      end do

      call evalfi(n,lu,fint,flag)

      ck = max( fint(2) - f, f - fint(1) )

      snormprev = snorm

      cnorma = 0.0d0
      cnormb = 0.0d0
      snorm  = 0.0d0
      gammak = 0.0d0
      do i = 1,m
          if ( equatn(i) ) then
              cnorma = cnorma + c(i) ** 2
              snorm  = max( snorm, abs( c(i) ) )
              gammak = gammak 
     +               + ( lambda(i) / rho(1) ) ** 2
     +               - ( lambda(i) / rho(1) + c(i) ) ** 2
          else
              cnormb = cnormb + max( 0.0d0, c(i) ) ** 2
              snorm  = max( snorm, 
     +                      abs( min( - c(i), lambda(i) / rho(1) ) ) )
              gammak = gammak 
     +               + ( lambda(i) / rho(1) ) ** 2
     +               - max( 0.0d0, ( lambda(i) / rho(1) + c(i) ) ) ** 2
          end if
      end do
      cnorm  = sqrt( cnorma ) + sqrt( cnormb )
      gammak = 0.5d0 * rho(1) * gammak

      algap = gammak + gapk

C     ==================================================================
C     Print information of this iteration
C     ==================================================================

      if ( iprint .ge. 1 ) then
          write(* , 970) outiter
          write(* , 980) mprint,(lambda(i),i=1,mprint)
          write(* , 990) rho(1)
          write(* ,1000) nprint,(x(i),i=1,nprint)
          write(*, 1020) f,cnorm,snorm

          write(10, 970) outiter
          write(10, 980) mprint,(lambda(i),i=1,mprint)
          write(10, 990) rho(1)
          write(10,1000) nprint,(x(i),i=1,nprint)
          write(10,1020) f,cnorm,snorm
      end if

      write(*,333) outiter,rho(1),lambda(1),x(1),f,cnorm,ck,gammak,gapk,
     +gammak+gapk

C     ==================================================================
C     Iterate
C     ==================================================================

      go to 100

C     ==================================================================
C     End of main loop
C     ==================================================================

C     NON-EXECUTABLE STATEMENTS

 900  format( /,' Entry to ALABB.',/,
     +        /,' Number of variables: ',I7,
     +          ' Number of constraints: ',I7)
 910  format( /,' Lower bounds (first ',I7, ' components): ',
     +        /,6(1X,1PD11.4))
 920  format( /,' Upper bounds (first ',I7, ' components): ',
     +        /,6(1X,1PD11.4))
 970  format( /,' ALABB outer iteration: ',I7,' was completed.')
 980  format( /,' Used Lagrange multipliers (first ',I7,
     +          ' components): ',
     +        /,6(1X,1PD11.4))
 990  format( /,' Used penalty parameter = ',1PD11.4)
 1000 format( /,' Computed point (first ',I7, ' components): ',
     +        /,6(1X,1PD11.4))
 1020 format( /,' Objective function value',38X,' = ',1PD11.4,
     +        /,' Euc-norm of constraints',39X,' = ',1PD11.4,
     +        /,' Sup-norm of infeasibility-complementarity',21X,
     +          ' = ',1PD11.4)
 1030 format( /,' The desired infeasibility was not achieved (',
     +            1PD11.4,'). The penalty',
     +        /,' parameter will be increased multiplying by rhofrac',
     +          ' = ',1PD11.4,'.')
 1050 format( /,' Desired feasibility improvement was achieved,',
     +        /,' so, penalty parameter(s) will not be modified.')
 1060 format( /,' Flag of ALABB = ',I2,
     +          ' (Problem is infeasible)')
 1070 format( /,' Flag of ALABB = ',I2,
     +          ' (Solution was found!)')
 1080 format( /,' Flag of ALABB = ',I2,
     +          ' (It was exceeded the maximum allowed number of outer',
     +        /,' iterations (maxoutit =',I7,').)')
 2000 format( /,' Flag of ALABB = ',I3,' Fatal Error',/,
     +        /,' The following codes means: ',/,
     +        /,' -90 : error in evalf   subroutine', 
     +        /,' -91 : error in evalc   subroutine', 
     +        /,' -92 : error in evalg   subroutine', 
     +        /,' -93 : error in evaljac subroutine', 
     +        /,' -94 : error in evalh   subroutine', 
     +        /,' -95 : error in evalhc  subroutine',/) 

 332  format('aqui: ',1X,I2,18X,1P,D11.4,1X,1P,D11.4,1X,1P,D7.1)
 333  format('aqui: ',1X,I2,1X,1P,D7.1,1X,1P,D8.1,1X,1P,D11.4,1X,1P,
     +       D11.4,1X,1P,D7.1,1X,1P,D7.1,1X,1P,D8.1,1X,1P,D8.1,1X,1P,
     +       D8.1)

      end
