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

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

  use hsl_ma57_double
  
  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: hnnz,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
  integer, intent(in) :: hcol(hnnz),hrow(hnnz)
  real(kind=8), intent(inout) :: x(n)
  real(kind=8), intent(out) :: g(n)

  ! LOCAL SCALARS
  integer :: allocstatus,info
  type(zd11_type) :: matrix
  type(ma57_control) :: control
  type(ma57_factors) :: factors
  type(ma57_ainfo) :: ainfo

  matrix%n = n
  matrix%ne = hnnz

  allocate(matrix%val(hnnz),matrix%row(hnnz),matrix%col(hnnz),stat=allocstatus)
  if ( allocstatus .ne. 0 ) then
     write(*,*) 'Error in allocate statement!'
     stop
  end if

  matrix%row(1:hnnz) = hrow(1:hnnz)
  matrix%col(1:hnnz) = hcol(1:hnnz)

  call ma57_initialize(factors,control)
  control%scaling = 0
  control%u = 0.5d0
  
  call ma57_analyse(matrix,factors,control,ainfo)
  write(*,*) 'ma57_analyse flag = ',ainfo%flag
  if ( ainfo%flag .lt. 0 ) then
     write(*,*) 'Failure of ma57_analyse with ainfo%flag = ',ainfo%flag
     stop
  end if
  
  call CRNewton2(n,x,f,g,gnorm,eps,ftarget,maxit,maxfcnt,hnnz,hrow,hcol, &
       matrix,control,factors,iter,fcnt,inform)
  
  deallocate(matrix%val,matrix%row,matrix%col,stat=allocstatus)
  if ( allocstatus .ne. 0 ) then
     write(*,*) 'Error in deallocate statement!'
     stop
  end if

  call ma57_finalize(factors,control,info)
  if ( info .ne. 0 ) then
     write(*,*) 'Error in ma57_finalize, info = ',info
     stop
  end if
  
end subroutine CRNewton

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

subroutine CRNewton2(n,x,f,g,gnorm,eps,ftarget,maxit,maxfcnt,hnnz,hrow,hcol, &
     matrix,control,factors,iter,fcnt,inform)

  use hsl_ma57_double
  
  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: hnnz,maxfcnt,maxit,n
  integer, intent(out) :: fcnt,inform,iter
  real(kind=8), intent(in) :: eps,ftarget
  real(kind=8), intent(out) :: f,gnorm
  type(zd11_type), intent(inout) :: matrix
  type(ma57_control), intent(inout) :: control
  type(ma57_factors), intent(inout) :: factors

  ! ARRAY ARGUMENTS
  integer, intent(in) :: hcol(hnnz),hrow(hnnz)
  real(kind=8), intent(inout) :: x(n)
  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 )

  ! PARAMETERS
  integer, parameter :: lnnzmax = 200000000
  
  ! LOCAL SCALARS
  logical :: frstnnsgm,nosolution,sing
  integer :: flag,lnnz,nfnoprogress,i
  real(kind=8) :: eps12,eps14,eps18,ftrial,gtrialnrm,sgmini,sgmtrial,strialnrm,stnrmlim,xnorm
  type(ma57_sinfo) :: sinfo

  ! LOCAL ARRAYS
  integer :: bstr(n),countg(4),iperm(n),lcolsta(n+1),lrow(lnnzmax),perm(n)
  real(kind=8) :: d(n),gbar(n),gtrial(n),hval(hnnz),lval(lnnzmax),q(n),rhs(n),scale(n),xtrial(n),ytrial(n),strial(n)
  
  iter = 0
  fcnt = 0
  
  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,hnnz,hrow,hcol,hval,flag)
  if ( flag .ne. 0 ) then
     inform = - 92
     return
  end if

!!$  write(*,*) 'hnnz = ',hnnz
!!$  write(*,*) 'hrow = ',hrow(1:hnnz)
!!$  write(*,*) 'hcol = ',hcol(1:hnnz)
!!$  write(*,*) 'hval = ',hval(1:hnnz)
  
  iter = iter + 1

  matrix%val(1:hnnz) = hval(1:hnnz)
  call computeMDMt(matrix,factors,control,lnnzmax,lnnz,lcolsta,lrow,lval,n,perm,iperm,scale,bstr,q,d)

  !===========================
  !===========================
!!$  write(*,*) 'With ma57_solve:'
!!$  strial(1:n) = - g(1:n)
!!$  call ma57_solve(matrix,factors,strial,control,sinfo)
!!$  strialnrm = norm2( strial(1:n) )
!!$  write(*,*) 'strialnrm: ',strialnrm
!!$  xtrial(1:n) = x(1:n) + strial(1:n)
!!$  call evalf(n,xtrial,ftrial,flag)
!!$  write(*,*) 'f(x+s) = ',ftrial
  !===========================
  !===========================

  
!!$  write(*,*) 'Computed using applyMDMt: '
!!$  do i = 1,n
!!$     gbar(1:n) = 0.0d0
!!$     gbar(i) = 1.0d0
!!$     call applyMDMt(lnnz,lcolsta,lrow,lval,n,perm,iperm,scale,bstr,q,d,gbar)
!!$     write(*,*) gbar(1:n)
!!$  end do
  
  gbar(1:n) = g(1:n)
  call solveM(lnnz,lcolsta,lrow,lval,n,perm,scale,bstr,q,gbar)
  
  call SolveSubproblem(n,d,gbar,0.0d0,nosolution,ytrial)

  if ( .not. nosolution ) then
     strial(1:n) = ytrial(1:n)
     call solveMt(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,strial)

     rhs(1:n) = - g(1:n)
     call ma57_solve(matrix,factors,strial,control,sinfo,rhs)
     
     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

  if ( frstnnsgm ) then
     frstnnsgm = .false.

     sgmtrial = sgmsmall
     
     call SolveSubproblem(n,d,gbar,sgmtrial,nosolution,ytrial)
  
     strial(1:n) = ytrial(1:n)
     call solveMt(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,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(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,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(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,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(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,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(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,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 sup norm = ',maxval(abs(y(1:n)))
  
end subroutine SolveSubproblem

! *****************************************************************
! *****************************************************************
subroutine computeMDMt(matrix,factors,control,lnnzmax,lnnz,lcolsta,lrow,lval,n,perm,iperm,scale,bstr,q,d)

  use hsl_ma57_double
    
  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: lnnzmax,n
  type(zd11_type), intent(in) :: matrix
  type(ma57_control), intent(in) :: control
  type(ma57_factors), intent(inout) :: factors

  ! ARRAY ARGUMENTS
  integer, intent(out) :: bstr(n),lcolsta(n+1),lrow(lnnzmax),iperm(n),perm(n)
  real(kind=8), intent(out) :: d(n),lval(lnnzmax),q(n),scale(n)
  
  ! LOCAL SCALARS
  integer :: dnnz,ini,ini2,j,lnnz,nelem,nelem2 !,i,k
  real(kind=8) :: Dblock11,Dblock21,Dblock22
  type(ma57_finfo) :: finfo
  type(ma57_sinfo) :: sinfo

  ! LOCAL ARRAYS
  integer :: dcolsta(n+1),drow(2*n)
  real(kind=8) :: dval(2*n) !,Lmat(n,n),Dmat(n,n),res(n,n)
  
!!$  write(*,*) 'Within computeMDMt:'
!!$  write(*,*) 'matrix%n = ',matrix%n
!!$  write(*,*) 'matrix%ne = ',matrix%ne
!!$  write(*,*) 'matrix%row = ',matrix%row(1:matrix%ne)
!!$  write(*,*) 'matrix%col = ',matrix%col(1:matrix%ne)
!!$  write(*,*) 'matrix%val = ',matrix%val(1:matrix%ne)
  
  call ma57_factorize(matrix,factors,control,finfo)
  write(*,*) 'Within computeMDMt, ma57_factorize says finfo%flag = ',finfo%flag,' finfo%rank = ',finfo%rank
  if ( finfo%flag .lt. 0 ) then
     write(*,*) 'Within computeMDMt, ma57_factorize return a negative finfo%flag, finfo%flag = ',finfo%flag
     stop
  end if

  write(*,*) 'lnnzmax = ',lnnzmax,' and finfo%nebdu = ',finfo%nebdu
  
  if ( lnnzmax .lt. finfo%nebdu ) then
     write(*,*) 'Increase lnnzmax and re-run'
     stop
  end if
  
  call ma57_get_factors(factors,control,lnnz,lcolsta,lrow,lval,dnnz,dcolsta,drow,dval,perm,iperm,scale,sinfo)

!!$  Lmat(1:n,1:n) = 0.0d0
!!$  do j = 1,n
!!$     do k = lcolsta(j),lcolsta(j+1) - 1
!!$        i = lrow(k)
!!$        Lmat(i,j) = lval(k)
!!$     end do
!!$  end do
!!$
!!$  Dmat(1:n,1:n) = 0.0d0
!!$  do j = 1,n
!!$     do k = dcolsta(j),dcolsta(j+1) - 1
!!$        i = drow(k)
!!$        Dmat(i,j) = dval(k)
!!$     end do
!!$  end do

  j = 1
  do while ( j .le. n )
     ini = dcolsta(j)
     nelem = dcolsta(j+1) - ini
     
     if ( nelem .eq. 1 ) then
        bstr(j) = 1
        
        if ( drow(ini) .ne. j ) then
           write(*,*) 'There is something wrong in compute MDMt (place 1)'
           stop
        end if
        
        d(j) = dval(ini)
        
        j = j + 1
        
     else if ( nelem .eq. 2 ) then
        if ( j .eq. n ) then
           write(*,*) 'There is something wrong in compute MDMt (place 2)'
           stop
        end if
        
        bstr(j:j+1) = 2

        if ( drow(ini) .ne. j .or. drow(ini+1) .ne. j + 1 ) then
           write(*,*) 'There is something wrong in compute MDMt (place 3)'
           stop
        end if
        
        ini2 = dcolsta(j+1)
        nelem2 = dcolsta(j+2) - ini2

        if ( nelem2 .ne. 2 .or. drow(ini2) .ne. j .or. drow(ini2+1) .ne. j + 1 .or. dval(ini2) .ne. dval(ini+1) ) then
           write(*,*) 'There is something wrong in compute MDMt  (place 4)'
           stop
        end if
           
        Dblock11 = dval(ini)
        Dblock21 = dval(ini+1)
        Dblock22 = dval(ini+3)
        
        call computeSQDQtS(Dblock11,Dblock21,Dblock22,q(j),q(j+1),d(j),d(j+1))

        j = j + 2
        
     else
        write(*,*) 'There is something wrong in compute MDMt  (place 5)'
        stop
     end if
  end do
  
  if ( count( bstr(1:n) .eq. 1 ) .ne. n - 2 * finfo%ntwo ) then
     write(*,*) 'There is something wrong in compute MDMt  (place 6)'
     write(*,*) 'count( bstr(1:n) .eq. 1 ) = ',count( bstr(1:n) .eq. 1 )
     write(*,*) 'count( bstr(1:n) .eq. 2 ) = ',count( bstr(1:n) .eq. 2 )
     write(*,*) 'n = ',n
     write(*,*) 'finfo%ntwo = ',finfo%ntwo
     stop
  end if
        
!!$  write(*,*) 'L = ',Lmat(1:n,1:n)
!!$  write(*,*) 'D = ',Dmat(1:n,1:n)
!!$  res(1:n,1:n) = matmul( Lmat(1:n,1:n), matmul( Dmat(1:n,1:n), transpose( Lmat(1:n,1:n) ) ) )
!!$  do j = 1,n
!!$     res(1:n,j) = res(iperm(1:n),j)
!!$  end do
!!$  do i = 1,n
!!$     res(i,1:n) = res(i,perm(1:n))
!!$  end do
!!$  do j = 1,n
!!$     do i = 1,n
!!$        res(i,j) = res(i,j) / scale(i) / scale(j)
!!$     end do
!!$  end do
!!$  write(*,*) 'LDLt = '
!!$  do i = 1,n
!!$     write(*,*) res(i,1:n)
!!$  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 solveMDMt(lnnz,lcolsta,lrow,lval,n,perm,iperm,scale,bstr,q,d,b,sing,flag)

  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: lnnz,n
  logical, intent(out) :: sing
  integer, intent(out) :: flag

  ! ARRAY ARGUMENTS
  integer, intent(in) :: bstr(n),iperm(n),lcolsta(n+1),lrow(lnnz),perm(n)
  real(kind=8), intent(in) :: d(n),lval(lnnz),q(n),scale(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(lnnz,lcolsta,lrow,lval,n,perm,scale,bstr,q,b)

  call solveD(n,d,b,sing,flag)

  call solveMt(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,b)

end subroutine solveMDMt

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

subroutine solveM(lnnz,lcolsta,lrow,lval,n,perm,scale,bstr,q,b)

  implicit none

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

  ! ARRAY ARGUMENTS
  integer, intent(in) :: bstr(n),lcolsta(n+1),lrow(lnnz),perm(n)
  real(kind=8), intent(in) :: lval(lnnz),q(n),scale(n)
  real(kind=8), intent(inout) :: b(n)

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

  ! M = S^-1 P^t L Q

  ! b <- S b
  
  b(1:n) = scale(1:n) * b(1:n)

  ! b <- P b
  
  b(1:n) = b(perm(1:n))

  ! b <- L^-1 b
  
  do j = 1,n
     if ( lrow(lcolsta(j)) .ne. j ) then
        write(*,*) 'There is something wrong in solveM'
        stop
     end if
     kini = lcolsta(j) + 1 ! the + 1 is to avoid the element in the diagonal (whose value is 1)
     kend = lcolsta(j+1) - 1
     b(lrow(kini:kend)) = b(lrow(kini:kend)) - b(j) * lval(kini:kend)
  end do

  ! b <- Q^t b
  
  i = 1
  do while ( i .le. n - 1 )
     if ( bstr(i) .eq. 1 ) then
        i = i + 1

     else if ( bstr(i) .eq. 2 ) then
        ! Q = [ v1 v2 ] with v1 = ( q(i), q(i+1) )^t and v2 = ( -q(i+1), q(i) )^t
        ! b(i:i+1) <- Q^t 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 
        write(*,*) 'There is something wrong in solveM'
        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(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,b)

  implicit none

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

  ! ARRAY ARGUMENTS
  integer, intent(in) :: bstr(n),iperm(n),lcolsta(n+1),lrow(lnnz)
  real(kind=8), intent(in) :: lval(lnnz),q(n),scale(n)
  real(kind=8), intent(inout) :: b(n)

  ! LOCAL SCALARS
  integer :: i,j,kini,kend
  real(kind=8) :: b1,b2
  
  ! M^t = ( S^-1 P^t L Q )^t = Q^t L^t P S^-1

  ! b <- Q b
  
  i = 1
  do while ( i .le. n - 1 )
     if ( bstr(i) .eq. 1 ) then
        i = i + 1
        
     else if ( bstr(i) .eq. 2 ) then

        ! Q = [ v1 v2 ] with v1 = ( q(i), q(i+1) )^t and v2 = ( -q(i+1), q(i) )^t
        ! b(i:i+1) <- Q 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 
        write(*,*) 'There is something wrong in solveM'
        stop
     end if
  end do

  ! b <- L^-t b
  
  do j = n,1,-1
     if ( lrow(lcolsta(j)) .ne. j ) then
        write(*,*) 'There is something wrong in solveMt'
        stop
     end if
     kini = lcolsta(j) + 1 ! the + 1 is to avoid the element in the diagonal (whose value is 1)
     kend = lcolsta(j+1) - 1
     b(j) = b(j) - dot_product( b(lrow(kini:kend)), lval(kini:kend) )
  end do

  ! b <- P^-1 b

  b(1:n) = b(iperm(1:n))

  ! b <- S b
  
  b(1:n) = scale(1:n) * b(1:n)

end subroutine solveMt

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

subroutine applyMDMt(lnnz,lcolsta,lrow,lval,n,perm,iperm,scale,bstr,q,d,x)

  implicit none

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

  ! ARRAY ARGUMENTS
  integer, intent(in) :: bstr(n),iperm(n),lcolsta(n+1),lrow(lnnz),perm(n)
  real(kind=8), intent(in) :: d(n),lval(lnnz),q(n),scale(n)
  real(kind=8), intent(inout) :: x(n)

  ! On output: x = MDMt x
  
  call ComputeMtx(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,x)
  
  call ComputeDx(n,d,x)

  call ComputeMx(lnnz,lcolsta,lrow,lval,n,perm,scale,bstr,q,x)

end subroutine applyMDMt

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

subroutine computeMx(lnnz,lcolsta,lrow,lval,n,iperm,scale,bstr,q,x)

  implicit none

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

  ! ARRAY ARGUMENTS
  integer, intent(in) :: bstr(n),iperm(n),lcolsta(n+1),lrow(lnnz)
  real(kind=8), intent(in) :: lval(lnnz),q(n),scale(n)
  real(kind=8), intent(inout) :: x(n)

  ! LOCAL SCALARS
  integer :: i,j,kini,kend
  real(kind=8) :: x1,x2

  ! LOCAL ARRAYS
  real(kind=8) :: y(n)
  
  ! M = S^-1 P^t L Q

  ! x <- Q x

  i = 1
  do while ( i .le. n - 1 )
     if ( bstr(i) .eq. 2 ) 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 <- L x
  
  y(1:n) = 0.0d0
  do j = 1,n
     kini = lcolsta(j)
     kend = lcolsta(j+1) - 1
     y(lrow(kini:kend)) = y(lrow(kini:kend)) + x(j) * lval(kini:kend)
  end do

  ! x <- S^-1 P^t y
  
  x(1:n) = y(iperm(1:n)) / scale(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(lnnz,lcolsta,lrow,lval,n,perm,scale,bstr,q,x)

  implicit none

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

  ! ARRAY ARGUMENTS
  integer, intent(in) :: bstr(n),lcolsta(n+1),lrow(lnnz),perm(n)
  real(kind=8), intent(in) :: lval(lnnz),q(n),scale(n)
  real(kind=8), intent(inout) :: x(n)

  ! LOCAL SCALARS
  integer :: i,j,kini,kend
  real(kind=8) :: x1,x2

  ! LOCAL ARRAYS
  real(kind=8) :: y(n)
  
  ! M^t = ( S^-1 P^t L Q )^t = Q^t L^t P S^-1

  ! x <- S^-1 x

  x(1:n) = x(1:n) / scale(1:n)

  ! x <- P x

  x(1:n) = x(perm(1:n))

  ! x <- L^t x

  do j = 1,n
     kini = lcolsta(j)
     kend = lcolsta(j+1) - 1
     x(j) = dot_product( x(lrow(kini:kend)), lval(kini:kend) )
  end do

  ! x <- Q^t x

  i = 1
  do while ( i .le. n - 1 )
     if ( bstr(i) .eq. 1 ) then
        i = i + 1

     else if ( bstr(i) .eq. 2 ) then
        ! Q = [ v1 v2 ] with v1 = ( q(i), q(i+1) )^t and v2 = ( -q(i+1), q(i) )^t
        ! x(i:i+1) <- Q^t x(i:i+1) 

        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 + 2

     else 
        write(*,*) 'There is something wrong in computeMtx'
        stop
     end if
  end do

end subroutine computeMtx
