! *****************************************************************
! *****************************************************************

subroutine cd(n,x,f,ftarget,maxit,totiter,inform)
  
  implicit none
  
  ! SCALAR ARGUMENTS
  integer, intent(in) :: maxit,n
  integer, intent(out) :: inform,totiter
  real(kind=8), intent(in) :: ftarget
  real(kind=8), intent(out) :: f

  ! ARRAY ARGUMENTS
  real(kind=8), intent(inout) :: x(n)

  ! COMMON BLOCKS
  character(len=4) :: fact
  common /facttype/ fact

  ! LOCAL SCALARS
  character :: uplo
  integer :: allocstat,iter,itrials,nb,nb1,nb2,lwork
  integer, parameter :: ntrials = 10
  real(kind=8) :: fbest,fprev
  
  ! LOCAL ARRAYS
  real(kind=8) :: xbest(n),xprev(n)
  real(kind=8), allocatable :: work(:)

  ! EXTERNAL FUNCTIONS
  integer, external :: ilaenv

  fact = 'MDMt'

  uplo = 'L'

  nb1 = ilaenv(1,'DSYTRF_RK',uplo,n,-1,-1,-1)
  nb2 = ilaenv(1,'DSYTRD',   uplo,n,-1,-1,-1)

  nb = max( nb1, nb2 )

  lwork = max( 1, n * ( nb + 2 ) )

  allocate( work(lwork),stat=allocstat )
  if ( allocstat .ne. 0 ) then
     write(*,*) 'Error in allocate statement!'
     stop
  end if

  call evalf(n,x,f)

  write(*,*) 'f(x0) = ',f

  itrials = 0
  totiter = 0
  fbest = huge( 1.0d0 )
  
10 continue

  itrials = itrials + 1
  
  fprev = f
  xprev(1:n) = x(1:n)
  
  call cd2cubreg(n,x,f,ftarget,maxit,iter,lwork,work,inform)
  write(*,*) 'f after cd2cubreg = ',f

  totiter = totiter + iter

  if ( f .gt. ftarget ) then
     if ( f .lt. fbest ) then
        fbest = f
        xbest(1:n) = x(1:n)
     end if

     if ( itrials .lt. ntrials ) then
        f = fprev
        x(1:n) = xprev(1:n)
        
        call cd2reflec(n,x,f)
        write(*,*) 'f after cd2reflec = ',f

        go to 10
     else
        f = fbest
        x(1:n) = xbest(1:n)
     end if
  end if
  
  deallocate(work,stat=allocstat)
  if ( allocstat .ne. 0 ) then
     write(*,*) 'Error in deallocate statement!'
     stop
  end if

end subroutine cd

! *****************************************************************
! *****************************************************************

subroutine cd2reflec(n,x,f)

  use mdgp, only: np,r,tau,dcolsta,drow,dval
  
  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: n
  real(kind=8), intent(inout) :: f
  
  ! ARRAY ARGUMENTS
  real(kind=8), intent(inout) :: x(n)

  ! LOCAL SCALARS
  logical :: imp
  integer :: i,indi,indj,j,i1,i2,i3,indi1,indi2,indi3
  real(kind=8) :: fsub,fadd,faddlst

  ! LOCAL ARRAYS
  real(kind=8) :: tmp(r),xrefl(r)

  do j = 1,np
     if ( dcolsta(j+1) - dcolsta(j) .ge. r ) then
        indj = ( j - 1 ) * r

        imp = .false.
  
        fsub = 0.0d0
        do i = dcolsta(j),dcolsta(j+1) - 1
           indi = ( drow(i) - 1 ) * r
           tmp(1:r) = x(indi+1:indi+r) - x(indj+1:indj+r)
           fsub = fsub + ( dot_product( tmp(1:r), tmp(1:r) ) - dval(i) ) ** 2
        end do

        do i1 = dcolsta(j),dcolsta(j+1) - 1
           indi1 = ( drow(i1) - 1 ) * r
           do i2 = i1+1,dcolsta(j+1) - 1
              indi2 = ( drow(i2) - 1 ) * r
              do i3 = i2+1,dcolsta(j+1) - 1
                 indi3 = ( drow(i3) - 1 ) * r
                 
                 call reflection(x(indj+1),x(indi1+1),x(indi2+1),x(indi3+1),xrefl)

                 fadd = 0.0d0
                 do i = dcolsta(j),dcolsta(j+1) - 1
                    indi = ( drow(i) - 1 ) * r
                    tmp(1:r) = x(indi+1:indi+r) - xrefl(1:r)
                    fadd = fadd + ( dot_product( tmp(1:r), tmp(1:r) ) - dval(i) ) ** 2
                 end do
                 
                 if ( fadd .lt. fsub ) then
                    x(indj+1:indj+r) = xrefl(1:r)
                    faddlst = fadd
                    imp = .true.
                 end if
              end do
           end do
        end do

        if ( imp ) then
           f = f + ( 2.0d0 / tau ) * ( faddlst - fsub )
        end if
        
     end if
  end do

end subroutine cd2reflec

! *****************************************************************
! *****************************************************************

subroutine reflection(x,p1,p2,p3,xref)

  implicit none

  ! ARRAYS ARGUMENTS
  real(kind=8), intent(in) :: x(3),p1(3),p2(3),p3(3)
  real(kind=8), intent(out) :: xref(3)

  ! LOCAL SCALARS
  real(kind=8) :: alpha

  ! LOCAL ARRAYS
  real(kind=8) :: v(3),w(3)

  v(1:3) = p2(1:3) - p1(1:3)
  w(1:3) = p3(1:3) - p1(1:3)

  xref(1) = v(2) * w(3) - v(3) * w(2)
  xref(2) = v(3) * w(1) - v(1) * w(3)
  xref(3) = v(1) * w(2) - v(2) * w(1)

  xref(1:3) = xref(1:3) / norm2( xref(1:3) )

  alpha = dot_product( x(1:3) - p1(1:3), xref(1:3) )

  xref(1:3) = x(1:3) - 2.0d0 * alpha * xref(1:3)

end subroutine reflection

! *****************************************************************
! *****************************************************************

subroutine cd2cubreg(n,x,f,ftarget,maxit,iter,lwork,work,inform)
  
  use mdgp, only: np,r

  implicit none
  
  ! SCALAR ARGUMENTS
  integer, intent(in) :: lwork,maxit,n
  integer, intent(out) :: inform,iter
  real(kind=8), intent(in) :: ftarget
  real(kind=8), intent(inout) :: f

  ! ARRAY ARGUMENTS
  real(kind=8), intent(inout) :: x(n),work(lwork)

  ! PARAMETERS
  real(kind=8), parameter :: alpha = 1.0d-08

  ! LOCAL SCALARS
  logical :: nosolution
  integer :: indj,j,nlack
  real(kind=8) :: ftrial,sigma,sgmmin,sgmlnn

  ! LOCAL ARRAYS
  integer :: ipiv(r)
  real(kind=8) :: d(r),gbar(r),gund(r),strial(r),xtrial(r),ytrial(r),Hund(r,r)

  inform = 0

  iter   = 0
  nlack  = 0
  sgmlnn = 0.0d0
  
!!$  write(*,*) 'iter = ',iter,' f = ',f

100 continue

  if ( f .le. ftarget ) then
     write(*,*) 'ftarget reached!'
     inform = 1
     return
  end if

  if ( iter .ge. maxit ) then
     write(*,*) 'Maximum of iterations reached!'
     inform = 2
     return
  end if

  iter = iter + 1

  j = mod( iter - 1, np ) + 1
  indj = ( j - 1 ) * r

  call evalgund(n,x,j,gund)

  gbar(1:r) = gund(1:r)

  call evalhund(n,x,j,Hund)

  call computeMDMt(r,Hund,ipiv,d,work,lwork)

  call solveM(r,Hund,ipiv,gbar)

  sigma = 0.0d0

  sgmmin = max( 1.0d-08, 0.1d0 * sgmlnn )
  
200 continue

  if ( sigma .gt. 1.0d+20 ) then
!!$     inform = 2
!!$     write(*,*) 'Too large sigma.'
!!$     return
     nlack = nlack + 1
     go to 100
  end if
  
  call SolveSubproblem(r,d,gbar,sigma,nosolution,ytrial)

  if ( nosolution ) then
!!$     if ( mod( iter, 100 ) .eq. 0 ) then
!!$        write(*,*) 'The subproblem has no solution with sigma = 0.'
!!$     end if
     sigma = max( sgmmin, 1.0d+02 * sigma )
     go to 200
  end if

  strial(1:r) = ytrial(1:r)
  call solveMt(r,Hund,ipiv,strial)

  xtrial(1:r) = x(indj+1:indj+r) + strial(1:r)
  
  call evalfund(n,x,f,j,xtrial,ftrial)

!!$  if ( mod( iter, 100 ) .eq. 0 ) then
!!$     write(*,*) '    sigma = ',sigma,' ftrial = ',ftrial
!!$  end if
  
  if ( .not. ftrial .le. f - alpha * sum( abs( ytrial(1:r) ) ** 3 ) ) then
     sigma = max( sgmmin, 1.0d+02 * sigma )
     go to 200
  end if

  if ( sigma .gt. 0.0d0 ) then
     sgmlnn = sigma
  end if

  if ( f - ftrial .le. 1.0d-08 * min( 1.0d0, abs( f ) ) ) then
     nlack = nlack + 1
     if ( nlack .ge. np ) then
        write(*,*) 'lack of progress.'
        inform = 2
        return
     end if
  else
     nlack = 0
  end if
     
  x(indj+1:indj+r) = xtrial(1:r)
  f = ftrial

!!$  if ( mod( iter, 100 ) .eq. 0 ) then
!!$     write(*,*) 'iter = ',iter,' f = ',f
!!$  end if
  
  go to 100

end subroutine cd2cubreg

! *****************************************************************
! *****************************************************************

subroutine SolveSubproblem(n,d,gbar,sigma,nosolution,y)

  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: n
  real(kind=8), intent(in) :: sigma
  logical, intent(out) :: nosolution

  ! ARRAY ARGUMENTS
  real(kind=8), intent(in) :: d(n),gbar(n)
  real(kind=8), intent(out) :: y(n)

  ! PARAMETERS
  real(kind=8), parameter :: macheps12 = sqrt( epsilon( 1.0d0 ) )

  ! LOCAL SCALARS
  integer :: i
  real(kind=8) :: aa,bb,cc,tmp

  nosolution = .false.

  if ( sigma .eq. 0.0d0 ) then

     if ( any( d(1:n) .lt. 0.0d0 ) .or. any( d(1:n) .eq. 0.0d0 .and. abs( gbar(1:n) ) .gt. macheps12 ) ) then
        nosolution = .true.
        return
     end if

     where( d(1:n) .eq. 0.0d0 )
        y(1:n) = 0.0d0
     elsewhere
        y(1:n) = - gbar(1:n) / d(1:n)
     end where

  else
     do i = 1,n
        if ( gbar(i) .ne. 0.0d0 ) then
           aa = sign( 3.0d0 * sigma, - gbar(i) )
           bb = d(i)
           cc = gbar(i)

           tmp = maxval( abs( (/ aa, bb, cc /) ) )
           if ( tmp .gt. 1.0d0 ) then
              aa = aa / tmp
              bb = bb / tmp
              cc = cc / tmp
           end if

           if ( bb .le. 0.0d0 ) then
              y(i) = ( - bb + sqrt( bb * bb - 4.0d0 * aa * cc ) ) / ( 2.0d0 * aa )
           else
              y(i) = ( - bb - sqrt( bb * bb - 4.0d0 * aa * cc ) ) / ( 2.0d0 * aa )
              y(i) = cc / ( aa * y(i) )
           end if

        else ! if ( gbar(i) .eq. 0.0d0 ) then
           if ( d(i) .ge. 0.0d0 ) then
              y(i) = 0.0d0
           else ! if ( d(i) .lt. 0.0d0 ) then
              y(i) = - d(i) / ( 3.0d0 * sigma ) ! - y(i) is also an option here
           end if
        end if
     end do
  end if

!!$  write(*,*) 'In SolveSubproblem, sigma = ',sigma,' y = ',y(1:n)

end subroutine SolveSubproblem

! *****************************************************************
! *****************************************************************

subroutine computeMDMt(n,A,ipiv,d,work,lwork)

  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: lwork,n

  ! ARRAY ARGUMENTS
  integer, intent(out) :: ipiv(n)
  real(kind=8), intent(inout) :: A(n,n),work(lwork)
  real(kind=8), intent(out) :: d(n)

  ! Given a symmetric matrix A, this subroutine computes a Bunch and
  ! Parlett factorization
  !
  ! A = Pbp Lbp Dbp Lbp^t Pbp^t of A,
  !
  ! where Pbp is a permutation matrix, Lbp is lower triangular and Dbp
  ! is diagonal with 1x1 or 2x2 blocks.
  !
  ! In addition, for each 2x2 block W, it computes an orthogonal
  ! matrix Q and a diagonal matriz D such that W = Q D Q^t.
  !
  ! Only the lower triangle of A is used.

  ! PARAMETERS
  character(len=4) :: fact
  common /facttype/ fact

  ! LOCAL SCALARS
  character :: uplo
  integer :: info,k
  real(kind=8) :: Dblock11,Dblock21,Dblock22

  ! LOCAL ARRAYS
  real(kind=8) :: e(n)

  uplo = 'L'

  if ( fact .eq. 'QDQt' ) then
     call dsyev('V',uplo,n,A,n,d,work,lwork,info)
     return
  end if

  call dsytrf_rk(uplo,n,A,n,e,ipiv,work,lwork,info)

!!$  write(*,*) 'D = '

  k = 1
  do while ( k .le. n )
     if ( ipiv(k) .gt. 0 ) then
        d(k) = A(k,k)
!!$        write(*,*) '1x1 block = ',d(k)

        k = k + 1

     else
        Dblock11 = A(k,k)
        Dblock22 = A(k+1,k+1)
        Dblock21 = e(k)
!!$        write(*,*) '2x2 block: Dblock11 = ',Dblock11,' Dblock21 = ',Dblock21,' Dblock22 = ',Dblock22

        call computeSQDQtS(Dblock11,Dblock21,Dblock22,A(k,k),A(k+1,k+1),d(k),d(k+1))

        k = k + 2
     end if
  end do

end subroutine computeMDMt

! *****************************************************************
! *****************************************************************

! *****************************************************************
! *****************************************************************

subroutine computeSQDQtS(a11,a21,a22,q1,q2,d1,d2)

  implicit none

  ! SCALAR ARGUMENTS
  real(kind=8), intent(in) :: a11,a21,a22
  real(kind=8), intent(out) :: d1,d2,q1,q2

  ! Given the symmetric 2 x 2 matrix
  !
  !        a11    a21
  !   A =
  !        a21    a22
  !
  ! this subroutine computes a an orthogonal matrix
  ! Q, and a diagonal matrix D such that A = S Q D ( S Q )^t.
  !
  ! On output: D = diag(d1,d2), and Q is such that
  ! its first column is given by (q1,q2)^t and its second column is
  ! given by (-q2,q1)^t. (A is overwritten.)

  ! SCALAR PARAMETERS
  logical, parameter :: debugging = .true.

  ! LOCAL SCALARS
  integer :: ind
  real(kind=8) :: delta,det,trace,qsupn,qnorm

  trace = a11 + a22

  det = a11 * a22 - a21 * a21

  delta = 0.25d0 * trace * trace - det

!!$  if ( debugging ) then
!!$     if ( delta .lt. 0.0d0 ) then
!!$        write(*,*) 'A negative discriminant (due to round off errors) ', &
!!$             'was replaced by zero. Discriminant = ',delta
!!$     end if
!!$  end if

  delta = sqrt( max( 0.0d0, delta ) )

  if ( trace .ge. 0.0d0 ) then
     d1 = 0.5d0 * trace + delta
     d2 = trace - d1
  else
     d2 = 0.5d0 * trace - delta
     d1 = trace - d2
  end if

!!$  if (debugging ) then
!!$     write(*,*) 'Distance of lambda1 + lambda2 to the trace = ', &
!!$          abs( d1 + d2 - trace ),' (should be close to zero)'
!!$     write(*,*) 'Distance of lambda1 * lambda2 to the determinant = ', &
!!$          abs( d1 * d2 - det   ),' (should be close to zero)'
!!$  end if

  ind = maxloc( abs( (/ d1 - a11, d1 - a22, d2 - a11, d2 - a22 /) ), dim=1 )

  if ( ind .eq. 1 ) then
     q1 = a21
     q2 = d1 - a11

  else if ( ind .eq. 2 )  then
     q1 = d1 - a22
     q2 = a21

  else if ( ind .eq. 3 ) then
     q1 = a11 - d2
     q2 = a21

  else ! if ( ind .eq. 4 ) then
     q1 = a21
     q2 = a22 - d2
  end if

  qsupn = max( abs( q1 ), abs( q2 ) )

  q1 = q1 / qsupn
  q2 = q2 / qsupn

  qnorm = sqrt( q1 * q1 + q2 * q2 )

  q1 = q1 / qnorm
  q2 = q2 / qnorm

end subroutine computeSQDQtS

! *****************************************************************
! *****************************************************************

subroutine solveM(n,A,ipiv,b)

  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: n

  ! ARRAY ARGUMENTS
  integer, intent(in) :: ipiv(n)
  real(kind=8), intent(in) :: A(n,n)
  real(kind=8), intent(inout) :: b(n)

  ! PARAMETERS
  character(len=4) :: fact
  common /facttype/ fact

  ! LOCAL SCALARS
  integer :: i,ip,j
  real(kind=8) :: b1,b2,tmp

  ! LOCAL ARRAY
  real(kind=8) :: q(n)

  if ( fact .eq. 'QDQt' ) then
     b(1:n) = matmul( transpose( A(1:n,1:n) ), b(1:n) )
     return
  end if

  ! Compute P^t b -> b

  do i = 1,n
     ip = abs( ipiv(i) )
     if ( ip .ne. i ) then
        tmp   = b(i)
        b(i)  = b(ip)
        b(ip) = tmp
     end if
  end do

  ! Solve L y = P^t b -> b (L is unit lower triangular)

  do j = 1,n
!!$     do i = j + 1,n
!!$        b(i) = b(i) - b(j) * A(i,j)
!!$     end do
     b(j+1:n) = b(j+1:n) - b(j) * A(j+1:n,j)
  end do

  ! Solve Q y = L^-1 P^t b -> b

  q(1:n) = (/ ( A(i,i), i = 1,n ) /)

  i = 1
  do while ( i .le. n - 1 )
     if ( ipiv(i) .gt. 0 ) then
        i = i + 1

     else ! if ( ipiv(i) .lt. 0 ) then
        ! Q = [ v1 v2 ] with v1 = ( q(i), q(i+1) )^t and v2 = ( -q(i+1), q(i) )^t
        ! Q^t b(i:i+1) -> b(i:i+1)

        b1 = b(i)
        b2 = b(i+1)

        b(i)   = b1 * q(i) + b2 * q(i+1)
        b(i+1) = b2 * q(i) - b1 * q(i+1)

        i = i + 2

!!$     else ! if ( ipiv(i) .eq. 0 ) then
!!$        write(*,*) 'Null ipiv, position: ',i
!!$        stop
     end if
  end do

end subroutine solveM

subroutine solveMt(n,A,ipiv,b)

  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: n

  ! ARRAY ARGUMENTS
  integer, intent(in) :: ipiv(n)
  real(kind=8), intent(in) :: A(n,n)
  real(kind=8), intent(inout) :: b(n)

  ! This subroutine solves M^t x = b, with M = P L Q.

  ! PARAMETERS
  character(len=4) :: fact
  common /facttype/ fact

  ! LOCAL SCALARS
  integer :: i,ip,j
  real(kind=8) :: b1,b2,tmp

  ! LOCAL ARRAY
  real(kind=8) :: q(n)

  if ( fact .eq. 'QDQt' ) then
     b(1:n) = matmul( A(1:n,1:n), b(1:n) )
     return
  end if

  ! Solve Q^t y = b -> b

  q(1:n) = (/ ( A(i,i), i = 1,n ) /)

  i = 1
  do while ( i .le. n - 1 )
     if ( ipiv(i) .gt. 0 ) then
        i = i + 1

     else ! if ( ipiv(i) .lt. 0 ) then

        ! Q = [ v1 v2 ] with v1 = ( q(i), q(i+1) )^t and v2 = ( -q(i+1), q(i) )^t
        ! Q b -> b

        b1 = b(i)
        b2 = b(i+1)

        b(i)   = b1 * q(i) - b2 * q(i+1)
        b(i+1) = b2 * q(i) + b1 * q(i+1)

        i = i + 2
!!$     else ! if ( ipiv(i) .eq. 0 ) then
!!$        write(*,*) 'Null ipiv, position: ',i
!!$        stop
     end if
  end do

  ! Solve L^t y = Q b -> b

  do j = n,1,-1
!!$     do i = j + 1,n
!!$        b(j) = b(j) - b(i) * A(i,j)
!!$     end do
     b(j) = b(j) - dot_product( b(j+1:n), A(j+1:n,j) )
  end do

  ! Solve P^t y = L^-t Q b -> b

  do i = n,1,-1
     ip = abs( ipiv(i) )
     if ( ip .ne. i ) then
        tmp   = b(i)
        b(i)  = b(ip)
        b(ip) = tmp
     end if
  end do

end subroutine solveMt

