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

program cdma

  use mdgp, only: np,r,tau,x0,xgiven,inimdgp,endmdgp,procrustes
  
  implicit none

  ! LOCAL SCALARS
  integer :: allocstat,flag,indj,inform,iter,j,maxit,n
  real(kind=8) :: f,finish,ftarget,start,timecd,timeiniguess,xerror

  ! LOCAL ARRAYS
  character(len=80) :: probname
  real(kind=8), allocatable :: x(:)

  ! COMMON BLOCKS
  integer :: counters(4)
  common /cntdata/ counters

  write(*,*) 'Enter problem name: '
  read(*,*) probname

  write(*,*) 'Initializing problem ...'
  
  call cpu_time(start)

  call inimdgp(probname,flag)
  if ( flag .ne. 0 ) then
     open(unit=10, file='cd-tabline.txt')
     write(10,1010) probname,'inimdgp',flag
     close(10)
     stop
  end if
  
  call cpu_time(finish)

  timeiniguess = finish - start

  write(*,*) 'Solving optimization problem ...'
  
  n = np * r

  allocate(x(n),stat=allocstat)
  if ( allocstat .ne. 0 ) then
     write(*,*) 'Allocation error in cdma.'
     stop
  end if

  x(1:n) = x0(1:n)

  counters(1:4) = 0

  maxit = huge( 1 )

  ftarget = 1.0d-10

  call cpu_time(start)

  call cd(n,x,f,ftarget,maxit,iter,inform)

  call cpu_time(finish)

  timecd = finish - start

  write(*,*) 'Saving solution and comparing with known solution (via Procrustes) ...'
  
  call procrustes(n,x,flag)
  if ( flag .ne. 0 ) then
     open(unit=10, file='cd-tabline.txt')
     write(10,1010) probname,'procrustes',flag
     close(10)
     stop
  end if

  open(unit=10,file=trim(probname)//'-computedsol-cd.txt')

  do j = 1,np
     indj = ( j - 1 ) * r
     write(10,*) x(indj+1:indj+r)
  end do

  close(10)
  
  open(unit=10,file=trim(probname)//'-givensol-transl.txt')

  do j = 1,np
     indj = ( j - 1 ) * r
     write(10,*) xgiven(indj+1:indj+r)
  end do

  close(10)
  
  xerror = 0.0d0
  do j = 1,np
     indj = ( j - 1 ) * r
     xerror = max( xerror, maxval( abs( x(indj+1:indj+r) - xgiven(indj+1:indj+r) ) ) / &
          max( 1.0d0, maxval( abs( xgiven(indj+1:indj+r) ) ) ) )
  end do

  write(*,*) 'Problem                                  = ',probname
  write(*,*) 'np                                       = ',np
  write(*,*) 'tau                                      = ',tau
  write(*,*) 'Density                                  = ',100.0d0 * tau / ( np ** 2 - np )
  write(*,*) 'CPU time for computing initial guess     = ',timeiniguess
  write(*,*) 'Number of iterations                     = ',iter
  write(*,*) 'Number of evaluations of f_under         = ',counters(2)
  write(*,*) 'CPU time of coordinate descent           = ',timecd
  write(*,*) 'Final objective function value           = ',f
  write(*,*) 'Error with respect to the known solution = ',xerror
  
  open(unit=10, file='cd-tabline.txt')
  write(10,1000) probname,np,tau,100.0d0*tau/(np**2-np),timeiniguess,iter,counters(2),timecd,f,xerror
  close(10)
  
  deallocate(x,stat=allocstat)
  if ( allocstat .ne. 0 ) then
     write(*,*) 'Deallocation error in cdma.'
     stop
  end if

  call endmdgp(flag)

1000 format(A20,1X,I10,1X,I10,1X,0PF5.2,1X,0PF8.2,1X,I10,1X,I10,0PF8.2,1X,1PD17.10,1X,1PD8.2)
1010 format(A20,1X,A20,1X,I10)
  
end program cdma

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

subroutine evalf(n,x,f)

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

  implicit none

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

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

  ! COMMON BLOCKS
  integer :: counters(4)
  common /cntdata/ counters

  ! LOCAL SCALARS
  integer :: i,indi,indj,j

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

  counters(1) = counters(1) + 1

  f = 0.0d0

  do j = 1,np
     indj = ( j - 1 ) * r

     do i = dcolsta(j),dcolsta(j+1) - 1
        if ( drow(i) .gt. j ) then
           indi = ( drow(i) - 1 ) * r
           tmp(1:r) = x(indi+1:indi+r) - x(indj+1:indj+r)
           f = f + ( dot_product( tmp(1:r), tmp(1:r) ) - dval(i) ) ** 2
        end if
     end do
  end do
  
  f = ( 2.0d0 / tau ) * f

end subroutine evalf

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

subroutine evalfund(n,x,f,j,xplus,fplus)

  use mdgp, only: r,tau,dcolsta,drow,dval

  implicit none

  ! SCALAR ARGUMENTS
  integer, intent(in) :: n,j
  real(kind=8), intent(in) :: f
  real(kind=8), intent(out) :: fplus

  ! ARRAY ARGUMENTS
  real(kind=8), intent(in) :: x(n),xplus(r)

  ! COMMON BLOCKS
  integer :: counters(4)
  common /cntdata/ counters

  ! LOCAL SCALARS
  integer :: i,indi,indj
  real(kind=8) :: fdiff

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

  counters(2) = counters(2) + 1

  fdiff = 0.0d0
  
  indj = ( j - 1 ) * r

  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)
     fdiff = fdiff - ( dot_product( tmp(1:r), tmp(1:r) ) - dval(i) ) ** 2

     tmp(1:r) = x(indi+1:indi+r) - xplus(1:r)
     fdiff = fdiff + ( dot_product( tmp(1:r), tmp(1:r) ) - dval(i) ) ** 2
  end do

  fplus = f + ( 2.0d0 / tau ) * fdiff
  
end subroutine evalfund

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

subroutine evalgund(n,x,j,gund)

  use mdgp, only: r,tau,dcolsta,drow,dval

  implicit none

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

  ! ARRAY ARGUMENTS
  real(kind=8), intent(in) :: x(n)
  real(kind=8), intent(out) :: gund(r)

  ! COMMON BLOCKS
  integer :: counters(4)
  common /cntdata/ counters

  ! LOCAL SCALARS
  integer :: i,indi,indj

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

  counters(3) = counters(3) + 1

  indj = ( j - 1 ) * r

  gund(1:r) = 0.0d0

  do i = dcolsta(j),dcolsta(j+1) - 1
     indi = ( drow(i) - 1 ) * r

     tmp(1:r) = x(indj+1:indj+r) - x(indi+1:indi+r)
     gund(1:r) = gund(1:r) + ( dot_product( tmp(1:r), tmp(1:r) ) - dval(i) ) * tmp(1:r)
  end do

  gund(1:r) = ( 8.0d0 / tau ) * gund(1:r)

end subroutine evalgund

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

subroutine evalHund(n,x,j,Hund)

  use mdgp, only: r,tau,dcolsta,drow,dval

  implicit none

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

  ! ARRAY ARGUMENTS
  real(kind=8), intent(in) :: x(n)
  real(kind=8), intent(out) :: Hund(r,r)

  ! COMMON BLOCKS
  integer :: counters(4)
  common /cntdata/ counters

  ! LOCAL SCALARS
  integer :: i,indi,indj,s,t

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

  counters(4) = counters(4) + 1

  indj = ( j - 1 ) * r
     
  do t = 1,r
     Hund(t,t) = 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)
        Hund(t,t) = Hund(t,t) + 2.0d0 * ( x(indj+t) - x(indi+t) ) ** 2 + dot_product( tmp(1:r), tmp(1:r) ) - dval(i)
     end do
     Hund(t,t) = ( 8.0d0 / tau ) * Hund(t,t)

     do s = t + 1,r
        Hund(s,t) = 0.0d0
        do i = dcolsta(j),dcolsta(j+1) - 1
           indi = ( drow(i) - 1 ) * r
           Hund(s,t) = Hund(s,t) + ( x(indj+s) - x(indi+s) ) * ( x(indj+t) - x(indi+t) )
        end do
        Hund(s,t) = ( 16.0d0 / tau ) * Hund(s,t)
     end do
  end do

end subroutine evalHund
