!***********************************************************************
! This file is part of OpenMolcas.                                     *
!                                                                      *
! OpenMolcas is free software; you can redistribute it and/or modify   *
! it under the terms of the GNU Lesser General Public License, v. 2.1. *
! OpenMolcas is distributed in the hope that it will be useful, but it *
! is provided "as is" and without any express or implied warranties.   *
! For more details see the full text of the license in the file        *
! LICENSE or in <http://www.gnu.org/licenses/>.                        *
!                                                                      *
! Copyright (C) 1996-2006, Thorstein Thorsteinsson                     *
!               1996-2006, David L. Cooper                             *
!***********************************************************************

subroutine symtrizorbs_cvb(orbs)

use casvb_global, only: corth, formAD, ifxorb, iorts, ipr, irels, nijrel, norb, nort, north, relorb
use stdalloc, only: mma_allocate, mma_deallocate
use Constants, only: Zero, One, Two, Four
use Definitions, only: wp, iwp, u6

implicit none
real(kind=wp), intent(inout) :: orbs(norb,norb)
integer(kind=iwp) :: icon, ierr, ioffs, iok, iorb, iorbmax, iort, iortorb, irel, j, jorb, jorbmax, jortorb, niprev, njprev, nortorb
real(kind=wp) :: a, b, c, c1, c2, cnrmi, cnrmj, cpm, cpp, d, discrpm, discrpp, dum(1), faci, facj, s, s1, s2, s3, smax, sovr
integer(kind=iwp), allocatable :: ihlp(:), ihlp2(:), ihlp3(:), iprev(:), jprev(:)
real(kind=wp), allocatable :: updi(:), updj(:)
real(kind=wp), parameter :: hund = 100.0_wp, thresh = 1.0e-10_wp
real(kind=wp), external :: ddot_

ioffs = 1
do iorb=1,norb
  if (north(iorb) /= 0) call schmidtd_cvb(corth(:,ioffs),north(iorb),orbs(:,iorb),1,dum,norb,0)
  ioffs = ioffs+north(iorb)
end do

call mma_allocate(ihlp,norb,label='ihlp')

! Now enforce orthogonality between specified orbitals:
! -----------------------------------------------------
ihlp(:) = -1
do iort=1,nort
  do j=1,2
    iorb = iorts(j,iort)
    ihlp(iorb) = min(north(iorb),norb)
  end do
end do
! Check feasibility
do irel=1,nijrel
  if (ihlp(irels(1,irel)) /= -1) then
    write(u6,'(a,i3)') ' WARNING - cannot perform orthogonalizations involving orbital ',irels(1,irel)
    write(u6,'(a,i3)') ' because this orbital is generated by symmetry operations from orbital ',irels(2,irel)
    write(u6,'(a)') ' Please simplify orthogonality constraints.'
    call abend_cvb()
  end if
end do
! Set up order of orthogonalization - first help arrays:
call mma_allocate(ihlp2,norb,label='ihlp2')
call mma_allocate(ihlp3,norb,label='ihlp3')
ihlp2(:) = 0
ihlp3(:) = 0
nortorb = 0
do icon=norb,0,-1
  do iorb=1,norb
    if (ihlp(iorb) == icon) then
      nortorb = nortorb+1
      ihlp2(nortorb) = iorb
      ihlp3(iorb) = nortorb
    end if
  end do
end do

call mma_deallocate(ihlp)
call mma_allocate(iprev,norb,label='iprev')
call mma_allocate(jprev,norb,label='jprev')
call mma_allocate(updi,norb,label='updi')
call mma_allocate(updj,norb,label='updj')

! Loop all pairs that may be orthogonalized
do iortorb=1,nortorb
  iorb = ihlp2(iortorb)
  ! Create list of previous orthonormalisations involving IORB
  niprev = 0
  do iort=1,nort
    if (iorts(1,iort) == iorb) then
      if (ihlp3(iorts(2,iort)) < iortorb) then
        niprev = niprev+1
        iprev(niprev) = iorts(2,iort)
      end if
    else if (iorts(2,iort) == iorb) then
      if (ihlp3(iorts(1,iort)) < iortorb) then
        niprev = niprev+1
        iprev(niprev) = iorts(1,iort)
      end if
    end if
  end do
  do jortorb=iortorb+1,nortorb
    jorb = ihlp2(jortorb)
    ! Create list of previous orthonormalisations involving JORB
    iok = 0
    njprev = 0
    do iort=1,nort
      if (iorts(1,iort) == jorb) then
        if (ihlp3(iorts(2,iort)) < iortorb) then
          njprev = njprev+1
          jprev(njprev) = iorts(2,iort)
        else if (ihlp3(iorts(2,iort)) == iortorb) then
          iok = 1
        end if
      else if (iorts(2,iort) == jorb) then
        if (ihlp3(iorts(1,iort)) < iortorb) then
          njprev = njprev+1
          jprev(njprev) = iorts(1,iort)
        else if (ihlp3(iorts(1,iort)) == iortorb) then
          iok = 1
        end if
      end if
    end do
    if (iok == 0) cycle
    sovr = ddot_(norb,orbs(:,iorb),1,orbs(:,jorb),1)
    if (abs(sovr) >= thresh) then
      ! Now ready to orthogonalise
      ! Update vectors
      call updvec_cvb(updi,iorb,jorb,niprev,iprev,orbs,north,corth)
      cnrmi = ddot_(norb,updi,1,updi,1)
      if (ifxorb(iorb) == 1) cnrmi = Zero
      call updvec_cvb(updj,jorb,iorb,njprev,jprev,orbs,north,corth)
      cnrmj = ddot_(norb,updj,1,updj,1)
      if (ifxorb(jorb) == 1) cnrmj = Zero

      if ((cnrmi > thresh) .and. (cnrmj > thresh)) then
        faci = One/sqrt(cnrmi)
        updi(:) = faci*updi(:)
        facj = One/sqrt(cnrmj)
        updj(:) = facj*updj(:)
        s1 = ddot_(norb,updi,1,orbs(:,jorb),1)
        s2 = ddot_(norb,updj,1,orbs(:,iorb),1)
        s3 = ddot_(norb,updi,1,updj,1)
        ! Initialize cpp & cpm to suppress compiler warning ...
        cpp = Zero
        cpm = Zero
        ! Same magnitudes of updates, either ++ or +-:
        if (abs(s3) > thresh) then
          a = s3
          b = s1+s2
          c = sovr
          discrpp = b*b-four*a*c
          if (discrpp >= Zero) then
            c1 = (-b+sqrt(discrpp))/(Two*a)
            c2 = (-b-sqrt(discrpp))/(Two*a)
            if (abs(c1) < abs(c2)) cpp = c1
            if (abs(c2) <= abs(c1)) cpp = c2
          else
            cpp = hund
          end if
          a = -s3
          b = s1-s2
          discrpm = b*b-four*a*c
          if (discrpm >= Zero) then
            c1 = (-b+sqrt(discrpm))/(Two*a)
            c2 = (-b-sqrt(discrpm))/(Two*a)
            if (abs(c1) < abs(c2)) cpm = c1
            if (abs(c2) <= abs(c1)) cpm = c2
          else
            cpm = hund
          end if
        else
          if (abs(s1+s2) > thresh) then
            cpp = sovr/(s1+s2)
          else
            cpp = hund
          end if
          if (abs(s1-s2) > thresh) then
            cpm = sovr/(s1+s2)
          else
            cpm = hund
          end if
        end if
        if (abs(cpp) < abs(cpm)) then
          c = cpp
        else
          c = cpm
        end if
        if ((c == hund) .or. (abs(s2+c*s3) < thresh)) then
          write(u6,'(a,2i3)') ' Could not orthogonalize orbitals',iorb,jorb
          write(u6,'(a)') ' Please simplify orthogonality constraints.'
          call abend_cvb()
        end if
        d = -(sovr+c*s1)/(s2+c*s3)
        updi(:) = c*updi(:)
        orbs(:,iorb) = orbs(:,iorb)+updi(:)
        updj(:) = d*updj(:)
        orbs(:,jorb) = orbs(:,jorb)+updj(:)
      else if (cnrmi > thresh) then
        faci = One/sqrt(cnrmi)
        updi(:) = faci*updi(:)
        s1 = ddot_(norb,updi,1,orbs(:,jorb),1)
        c = -sovr/s1
        updi(:) = c*updi(:)
        orbs(:,iorb) = orbs(:,iorb)+updi(:)
      else if (cnrmj > thresh) then
        facj = One/sqrt(cnrmj)
        updj(:) = facj*updj(:)
        s2 = ddot_(norb,updj,1,orbs(:,iorb),1)
        d = -sovr/s2
        updj(:) = d*updj(:)
        orbs(:,jorb) = orbs(:,jorb)+updj(:)
      else
        write(u6,'(a,2i3)') ' Could not orthogonalize orbitals',iorb,jorb
        write(u6,'(a)') ' Please simplify orthogonality constraints.'
        call abend_cvb()
      end if
    end if
    niprev = niprev+1
    iprev(niprev) = jorb
  end do
end do

call mma_deallocate(ihlp2)
call mma_deallocate(ihlp3)
call mma_deallocate(iprev)
call mma_deallocate(jprev)
call mma_deallocate(updi)
call mma_deallocate(updj)

ierr = 0
call nize_cvb(orbs,norb,dum,norb,0,ierr)
smax = -One
do iort=1,nort
  iorb = iorts(1,iort)
  jorb = iorts(2,iort)
  s = abs(ddot_(norb,orbs(:,iorb),1,orbs(:,jorb),1))
  if (s > smax) then
    smax = s
    iorbmax = iorb
    jorbmax = jorb
  end if
end do
if ((ipr(3) >= 2) .and. (smax > 1.0e-10_wp)) then
  write(u6,'(a,2i3)') ' Maximum overlap for orthogonalized orbitals :',iorbmax,jorbmax
  write(u6,formAD) ' Value : ',ddot_(norb,orbs(:,iorbmax),1,orbs(:,jorbmax),1)
end if

do irel=1,nijrel
  iorb = irels(1,irel)
  jorb = irels(2,irel)
  call mxatb_cvb(relorb(:,:,irel),orbs(:,jorb),norb,norb,1,orbs(:,iorb))
end do

return

end subroutine symtrizorbs_cvb
