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

subroutine CRNewton(n,x,f,g,gnorm,eps,ftarget,maxit,maxfcnt,iter,fcnt,pnewt,inform)

  implicit none

  ! SCALAR ARGUMENTS
  logical, intent(out) :: pnewt
  integer, intent(in) :: maxfcnt,maxit,n
  integer, intent(out) :: fcnt,inform,iter
  real(kind=8), intent(in) :: eps,ftarget
  real(kind=8), intent(out) :: f,gnorm
  
  ! ARRAY ARGUMENTS
  real(kind=8), intent(inout) :: x(n)
  real(kind=8), intent(out) :: g(n)

  ! PARAMETERS
  character(len=4) :: fact
  common /facttype/ fact
  
  ! LOCAL SCALARS
  character :: uplo
  integer :: allocstatus,nb,nb1,nb2,lwork
  
  ! LOCAL ARRAYS
  real(kind=8), dimension(:), allocatable :: work

  ! EXTERNAL FUNCTIONS
  integer :: ilaenv
  external ilaenv
  
  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=allocstatus )
  if ( allocstatus .ne. 0 ) then
     write(*,*) 'Error in allocate statement!'
     stop
  end if

  fact = 'MDMt'
  
  call CRNewton2(n,x,f,g,gnorm,eps,ftarget,maxit,maxfcnt,iter,fcnt,pnewt,work,lwork,inform)
  
  deallocate( work, stat=allocstatus )
  if ( allocstatus .ne. 0 ) then
     write(*,*) 'Error in deallocate statement!'
     stop
  end if

end subroutine CRNewton

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

subroutine CRNewton2(n,x,f,g,gnorm,eps,ftarget,maxit,maxfcnt,iter,fcnt,pnewt,work,lwork,inform)

  implicit none

  ! SCALAR ARGUMENTS
  logical, intent(out) :: pnewt
  integer, intent(in) :: lwork,maxfcnt,maxit,n
  integer, intent(out) :: fcnt,inform,iter
  real(kind=8), intent(in) :: eps,ftarget
  real(kind=8), intent(out) :: f,gnorm
  
  ! ARRAY ARGUMENTS
  real(kind=8), intent(inout) :: x(n),work(lwork)
  real(kind=8), intent(out) :: g(n)

  ! SCALAR PARAMETERS
  real(kind=8), parameter :: alpha = 1.0d-08, sgmsmall = 1.0d-08, sgmbig = 1.0d+08, xlarge = 1.0d+10, &
       macheps = epsilon( 1.0d0 ), macheps12 = sqrt( macheps )
  
  ! LOCAL SCALARS
  logical :: frstnnsgm,nosolution
  integer :: flag,nfnoprogress,i
  real(kind=8) :: eps12,eps14,eps18,ftrial,gtrialnrm,sgmini,sgmtrial,strialnrm,stnrmlim,xnorm

  ! LOCAL ARRAYS
  integer :: countg(4),ipiv(n)
  real(kind=8) :: d(n),gbar(n),H(n,n),gtrial(n),xtrial(n),ytrial(n),strial(n)

  iter = 0
  fcnt = 0
  pnewt = .true.
  
  countg(1:4) = 0

  nfnoprogress = 0
  
  inform = 99
  
  eps12 = sqrt( eps )
  eps14 = sqrt( eps12 )
  eps18 = sqrt( eps14 )
  
  frstnnsgm = .true.
  
  call evalf(n,x,f,flag)
  fcnt = fcnt + 1
  if ( flag .ne. 0 ) then
     inform = - 90
     return
  end if

10 continue

  if ( f .le. ftarget ) then
     inform = 6
     return
  end if
  
  call evalg(n,x,g,flag)
  if ( flag .ne. 0 ) then
     inform = - 91
     return
  end if

  gnorm = maxval( abs( g(1:n) ) )

  ! count(1): is the number of consecutive iterations with |gk|<eps
  ! count(2): is the number of consecutive iterations with |gk|<eps^1/2
  ! count(3): is the number of consecutive iterations with |gk|<eps^1/4
  ! count(4): is the number of consecutive iterations with |gk|<eps^1/8
  
  if ( gnorm .le. eps ) then
     countg(1:4) = countg(1:4) + 1
  else
     countg(1:1) = 0
     if ( gnorm .le. eps12 ) then
        countg(2:4) = countg(2:4) + 1
     else
        countg(2:2) = 0
        if ( gnorm .le. eps14 ) then
           countg(3:4) = countg(3:4) + 1
        else
           countg(3:3) = 0
           if ( gnorm .le. eps18 ) then
              countg(4:4) = countg(4:4) + 1
           else
              countg(4:4) = 0
           end if
        end if
     end if
  end if

  if ( countg(1) .ge. 1 ) then
     inform = 0
     return
  else if ( countg(2) .ge. 100 ) then
     inform = 1
     return
  else if ( countg(3) .ge. 1000 ) then
     inform = 2
     return
  else if ( countg(4) .ge. 5000 ) then
     inform = 3
     return
  end if
  
  xnorm = norm2( x(1:n) )
  if ( xnorm .ge. xlarge ) then
     inform = 10
     return
  end if
  
  if ( iter .ge. maxit ) then
     inform = 11
     return
  end if

  if ( fcnt .ge. maxfcnt ) then
     inform = 12
     return
  end if
  
  write(*,*)
  write(*,*) 'iter = ',iter,' f = ',f,' gnorm = ',gnorm,' fcnt = ',fcnt
  write(*,*) 'xnorm = ',xnorm,' x = ',x(1:min(n,10))

  call evalh(n,x,H,flag)
  if ( flag .ne. 0 ) then
     inform = - 92
     return
  end if

!!$  write(*,*) 'H = '
!!$  do i = 1,n
!!$     write(*,*) H(i,1:n)
!!$  end do
  
  iter = iter + 1

  call computeMDMt(n,H,ipiv,d,work,lwork)
  
  gbar(1:n) = g(1:n)

  call solveM(n,H,ipiv,gbar)
  
  call SolveSubproblem(n,d,gbar,0.0d0,nosolution,ytrial)

  if ( .not. nosolution ) then
     strial(1:n) = ytrial(1:n)
     call solveMt(n,H,ipiv,strial)

     xtrial(:n) = x(1:n) + strial(1:n)

     strialnrm = norm2( strial(1:n) )
  
     call evalf(n,xtrial,ftrial,flag)
     fcnt = fcnt + 1
     if ( flag .lt. 0 ) then
        inform = - 90
        return
     end if

     write(*,*) '      sgmtrial = 0  (i.e. Newton direction)  ftrial = ',ftrial,' strialnrm = ',strialnrm

     if ( ftrial .le. f - alpha * sum( abs( ytrial(1:n) ) ** 3 ) ) then
        call CheckStopping1(n,x,f,g,gnorm,xtrial,ftrial,fcnt,nfnoprogress,inform)
        if ( inform .ne. 99 ) return

        f = ftrial
        x(1:n) = xtrial(1:n)
        go to 10
     end if

     call CheckStopping2(n,x,f,g,gnorm,0.0d0,strialnrm,xtrial,ftrial,ftarget,eps,inform)
     if ( inform .ne. 99 ) return
  end if

  pnewt = .false.
  
  if ( frstnnsgm ) then
     frstnnsgm = .false.

     sgmtrial = sgmsmall
     
     call SolveSubproblem(n,d,gbar,sgmtrial,nosolution,ytrial)
  
     strial(1:n) = ytrial(1:n)
     call solveMt(n,H,ipiv,strial)

     strialnrm = norm2( strial(1:n) )
!!$     write(*,*) '(a) sgmtrial = ',sgmtrial,' strialnrm = ',strialnrm
 
  else
     sgmtrial = sgmini
     
     call SolveSubproblem(n,d,gbar,sgmtrial,nosolution,ytrial)
  
     strial(1:n) = ytrial(1:n)
     call solveMt(n,H,ipiv,strial)

     strialnrm = norm2( strial(1:n) )
!!$     write(*,*) '(b) sgmtrial = ',sgmtrial,' strialnrm = ',strialnrm

     if ( strialnrm .lt. macheps12 * max( 1.0d0, norm2( x(1:n) ) ) ) then
        sgmtrial = sgmsmall

        call SolveSubproblem(n,d,gbar,sgmtrial,nosolution,ytrial)
  
        strial(1:n) = ytrial(1:n)
        call solveMt(n,H,ipiv,strial)
        
        strialnrm = norm2( strial(1:n) )
!!$        write(*,*) '(c) sgmtrial = ',sgmtrial,' strialnrm = ',strialnrm
     end if
  end if

  if ( sgmtrial .eq. sgmsmall ) then
     stnrmlim = max( 1.0d0, norm2( x(1:n) ) )
     
     do while ( strialnrm .gt. stnrmlim .and. 10.0d0 * sgmtrial .le. sgmbig )
        sgmtrial = 10.0d0 * sgmtrial
     
        call SolveSubproblem(n,d,gbar,sgmtrial,nosolution,ytrial)

        strial(1:n) = ytrial(1:n)
        call solveMt(n,H,ipiv,strial)
     
        strialnrm = norm2( strial(1:n) )
!!$        write(*,*) '(d) sgmtrial = ',sgmtrial,' strialnrm = ',strialnrm
     end do
  end if
  
20 continue
  
  xtrial(:n) = x(1:n) + strial(1:n)

  call evalf(n,xtrial,ftrial,flag)
  fcnt = fcnt + 1
  if ( flag .lt. 0 ) then
     inform = - 90
     return
  end if

  write(*,*) '      sgmtrial = ',sgmtrial,' ftrial = ',ftrial,' strialnrm = ',strialnrm

  if ( ftrial .le. f - alpha * sum( abs( ytrial(1:n) ) ** 3 ) ) then
     call CheckStopping1(n,x,f,g,gnorm,xtrial,ftrial,fcnt,nfnoprogress,inform)
     if ( inform .ne. 99 ) return
     
     f = ftrial
     x(1:n) = xtrial(1:n)

     sgmini = max( sgmsmall, 0.5d0 * sgmtrial )

     go to 10
  end if
     
  call CheckStopping2(n,x,f,g,gnorm,sgmtrial,strialnrm,xtrial,ftrial,ftarget,eps,inform)
  if ( inform .ne. 99 ) return
     
  if ( ftrial .le. ftarget ) then
     call evalg(n,xtrial,gtrial,flag)
     if ( flag .ne. 0 ) then
        inform = - 91
        return
     end if

     gtrialnrm = maxval( abs( gtrial(1:n) ) )

     f = ftrial
     x(1:n) = xtrial(1:n)
     g(1:n) = gtrial(1:n)
     gnorm = gtrialnrm
     
     inform = 7
     return
  end if
     
  sgmtrial = 10.0d0 * sgmtrial
     
  call SolveSubproblem(n,d,gbar,sgmtrial,nosolution,ytrial)

  strial(1:n) = ytrial(1:n)
  call solveMt(n,H,ipiv,strial)
     
  strialnrm = norm2( strial(1:n) )
     
  go to 20

end subroutine CRNewton2

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

subroutine CheckStopping1(n,x,f,g,gnorm,xtrial,ftrial,fcnt,nfnoprogress,inform)

  ! SCALAR ARGUMENTS
  integer, intent(inout) :: fcnt,inform,nfnoprogress
  real(kind=8), intent(inout) :: f,ftrial,gnorm

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

  ! PARAMETERS
  real(kind=8), parameter :: macheps = epsilon( 1.0d0 )
  
  ! LOCAL SCALARS
  integer :: flag,i
  real(kind=8) :: fbest,ftmp,gtrialnrm,step,tmp
  
  ! LOCAL ARRAYS
  real(kind=8) :: gtrial(n),xbest(n)
  
  if ( all ( xtrial(1:n) .eq. x(1:n) ) ) then
     fbest = ftrial
     xbest(1:n) = xtrial(1:n)

     do i = 1,n
        tmp = xtrial(i)

        step = macheps * max( 1.0d0, abs( tmp ) )

        xtrial(i) = tmp + step

        call evalf(n,xtrial,ftmp,flag)
        fcnt = fcnt + 1
           
        if ( .not. flag .lt. 0 ) then
           if ( ftmp .lt. fbest ) then
              fbest = ftmp
              xbest(1:n) = xtrial(1:n)
           end if
        end if
        
        xtrial(i) = tmp - step

        call evalf(n,xtrial,ftmp,flag)
        fcnt = fcnt + 1
     
        if ( .not. flag .lt. 0 ) then
           if ( ftmp .lt. fbest ) then
              fbest = ftmp
              xbest(1:n) = xtrial(1:n)
           end if
        end if

        xtrial(i) = tmp
     end do

     if ( fbest .ge. ftrial ) then
        call evalg(n,xtrial,gtrial,flag)
        if ( flag .ne. 0 ) then
           inform = - 91
           return
        end if
        
        gtrialnrm = maxval( abs( gtrial(1:n) ) )
        
        f = ftrial
        x(1:n) = xtrial(1:n)
        g(1:n) = gtrial(1:n)
        gnorm = gtrialnrm
        
        inform = 8
        return
     end if
     
     ftrial = fbest
     xtrial(1:n) = xbest(1:n)
  end if

  if ( ftrial .eq. f ) then
     nfnoprogress = nfnoprogress + 1
     if ( nfnoprogress .ge. 10 ) then
        inform = 9
        return
     end if
  else
     nfnoprogress = 0
  end if

end subroutine CheckStopping1

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

subroutine CheckStopping2(n,x,f,g,gnorm,sgmtrial,strialnrm,xtrial,ftrial,ftarget,eps,inform)

  ! SCALAR ARGUMENTS
  integer, intent(inout) :: inform
  real(kind=8), intent(in) :: eps,ftarget,ftrial,sgmtrial,strialnrm
  real(kind=8), intent(inout) :: f,gnorm

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

  ! PARAMETERS
  real(kind=8), parameter :: macheps = epsilon( 1.0d0 )
  
  ! LOCAL SCALARS
  integer :: flag
  real(kind=8) :: gtrialnrm
  
  ! LOCAL ARRAYS
  real(kind=8) :: gtrial(n)
  
  if ( sgmtrial .eq. 0.0d0 .and. strialnrm .le. eps ) then
     call evalg(n,xtrial,gtrial,flag)
     if ( flag .ne. 0 ) then
        inform = - 91
        return
     end if

     gtrialnrm = maxval( abs( gtrial(1:n) ) )

     f = ftrial
     x(1:n) = xtrial(1:n)
     g(1:n) = gtrial(1:n)
     gnorm = gtrialnrm

     if ( gtrialnrm .le. eps ) then
        inform = 4
     else
        inform = 5
     end if
     
     return
  end if

  if ( ftrial .le. ftarget ) then
     call evalg(n,xtrial,gtrial,flag)
     if ( flag .ne. 0 ) then
        inform = - 91
        return
     end if

     gtrialnrm = maxval( abs( gtrial(1:n) ) )

     f = ftrial
     x(1:n) = xtrial(1:n)
     g(1:n) = gtrial(1:n)
     gnorm = gtrialnrm
     
     inform = 7
     return
  end if

end subroutine CheckStopping2

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

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 applyMDMt(n,A,ipiv,d,x)

  implicit none

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

  ! On output: x = MDMt x
  
  call ComputeMtx(n,A,ipiv,x)
  
  call ComputeDx(n,d,x)

  call ComputeMx(n,A,ipiv,x)

end subroutine applyMDMt

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

subroutine solveMDMt(n,A,ipiv,d,b,sing,flag)

  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: n
  logical, intent(out) :: sing
  integer, intent(out) :: flag
  
  ! ARRAY ARGUMENTS
  integer, intent(in) :: ipiv(n)
  real(kind=8), intent(in) :: A(n,n),d(n)
  real(kind=8), intent(inout) :: b(n)

  ! On output:
  !
  ! If A is non-singular, sing=F and flag=0
  ! If A is singular but the system is compatible, sing=T, flag=0
  ! If A is singular and the system has no solution, sing=T and flag=-1
  !
  ! When Ax=b has a solution, the solution is in b.
  
  call solveM(n,A,ipiv,b)

  call solveD(n,d,b,sing,flag)
        
  call solveMt(n,A,ipiv,b)

end subroutine solveMDMt

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

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 solveD(n,d,b,sing,flag)

  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: n
  logical, intent(out) :: sing
  integer, intent(out) :: flag
  
  ! ARRAY ARGUMENTS
  real(kind=8), intent(in) :: d(n)
  real(kind=8), intent(inout) :: b(n)

  ! LOCAL SCALARS
  integer :: i
  
  flag = 0
  sing = .false.

  do i = 1,n
     if ( d(i) .eq. 0.0d0 ) then
        sing = .true.
        if ( b(i) .ne. 0.0d0 ) then
           flag = - 1
           return
        end if
     else
        b(i) = b(i) / d(i)
     end if
  end do

end subroutine solveD

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

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

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

subroutine computeMx(n,A,ipiv,x)

  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) :: x(n)

  ! PARAMETERS
  character(len=4) :: fact
  common /facttype/ fact
  
  ! LOCAL SCALARS
  integer :: i,ip,j
  real(kind=8) :: x1,x2,tmp

  ! LOCAL ARRAYS
  real(kind=8) :: q(n),y(n)
  
  if ( fact .eq. 'QDQt' ) then
     x(1:n) = matmul( A(1:n,1:n), x(1:n) )
     return
  end if
  
  ! M = P L Q
  
  q(1:n) = (/ ( A(i,i), i = 1,n ) /)
  
  i = 1
  do while ( i .le. n - 1 )
     if ( ipiv(i) .lt. 0 ) then
        x1 = x(i)
        x2 = x(i+1)
        
        x(i)   = x1 * q(i) - x2 * q(i+1)
        x(i+1) = x2 * q(i) + x1 * q(i+1)

        i = i + 1
     end if

     i = i + 1
  end do

  y(1:n) = 0.0d0
  
  do j = 1,n
     y(j) = y(j) + x(j)
     y(j+1:n) = y(j+1:n) + A(j+1:n,j) * x(j)
  end do
  
  do i = n,1,-1
     ip = abs( ipiv(i) )
     if ( ip .ne. i ) then
        tmp   = y(i)
        y(i)  = y(ip)
        y(ip) = tmp
     end if
  end do

  x(1:n) = y(1:n)
  
end subroutine computeMx

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

subroutine computeDx(n,d,x)

  implicit none

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

  x(1:n) = d(1:n) * x(1:n)
  
end subroutine computeDx

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

subroutine computeMtx(n,A,ipiv,x)

  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) :: x(n)

  ! PARAMETERS
  character(len=4) :: fact
  common /facttype/ fact
  
  ! LOCAL SCALARS
  integer :: i,ip,j
  real(kind=8) :: x1,x2,tmp

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

  if ( fact .eq. 'QDQt' ) then
     x(1:n) = matmul( transpose( A(1:n,1:n) ), x(1:n) )
     return
  end if
  
  ! M = P L Q
  
  do i = 1,n
     ip = abs( ipiv(i) )
     if ( ip .ne. i ) then
        tmp   = x(i)
        x(i)  = x(ip)
        x(ip) = tmp
     end if
  end do

  do j = 1,n
     x(j) = x(j) + dot_product( A(j+1:n,j), x(j+1:n) )
  end do

  q(1:n) = (/ ( A(i,i), i = 1,n ) /)
  
  i = 1
  do while ( i .le. n - 1 )
     if ( ipiv(i) .lt. 0 ) then
        x1 = x(i)
        x2 = x(i+1)
        
        x(i)   = x1 * q(i) + x2 * q(i+1)
        x(i+1) = x2 * q(i) - x1 * q(i+1)

        i = i + 1
     end if

     i = i + 1
  end do

end subroutine computeMtx