! This Fortran95 Code/Module performs Machine Learning Force prediction based on XYZ structural Files;
! Sorting/Selecting Algorithms are used to construct sub training database during heavy machine learning task;
! Database can be updated according to an error_threshold, e.g. pred_err;
! Internal Vector representation is used for describing the atomic structures;
! Any issues or bugs regarding this code please contact Zhenwei Li (zhenwei.li@unibas.ch)
! Reference article : Molecular Dynamics with On-the-Fly Machine Learning of Quantum-Mechanical Forces, Zhenwei Li, James Kermode, Alessandro De Vita, PRL, 114, 096405 (2015)
module force_machine_learning_module

  use libAtoms_module
  use potential_module
  implicit none

  type optimise_likelihood
      logical use_qr
      real(dp), dimension(:,:), pointer :: distance_matrix, covariance_matrix, force_covariance_matrix,  t
  endtype optimise_likelihood

public :: write_distance
interface  write_distance
   module procedure write_distance_file
   module procedure write_distance_file_real
end interface

  contains

  function likelihood(x,am_data)
      real(dp), dimension(:) :: x
      character(len=1), dimension(:), optional :: am_data
      real(dp) :: likelihood

      type(optimise_likelihood) :: am
      real(dp) :: sigma_error, sigma_covariance
      real(dp), dimension(:,:), allocatable :: ICF
      integer :: i
      type(LA_Matrix) :: LA_covariance

      am = transfer(am_data,am)
      sigma_error = x(1)
      sigma_covariance = x(2)

      am%covariance_matrix = exp(-0.5_dp * am%distance_matrix/sigma_covariance**2)
      do i = 1, size(am%covariance_matrix,1)
         am%covariance_matrix(i,i) = am%covariance_matrix(i,i) + sigma_error**2
      enddo

      LA_covariance = am%covariance_matrix
      allocate(ICF(size(am%covariance_matrix,1),size(am%covariance_matrix,1)))

      if (am%use_qr) then
         call Matrix_QR_Solve(LA_covariance,am%force_covariance_matrix,ICF)
      else
         call Matrix_Solve(LA_covariance,am%force_covariance_matrix,ICF)
      end if

      likelihood = 1.5_dp * LA_Matrix_LogDet(LA_covariance) + 0.5_dp * trace(ICF)
!      write(*,*) "DEBUG Likelihood value: ", likelihood

      call finalise(LA_covariance)
      if(allocated(ICF)) deallocate(ICF)

   endfunction likelihood

   function dlikelihood(x,am_data)
      real(dp), dimension(:) :: x
      character(len=1), dimension(:), optional :: am_data
      real(dp), dimension(size(x)) :: dlikelihood

      type(optimise_likelihood) :: am
      real(dp) :: sigma_error, sigma_covariance
      real(dp), dimension(:,:), allocatable :: ICF, IC, ICDCDS, DCDS
      integer :: i, n
      type(LA_Matrix) :: LA_covariance

      am = transfer(am_data,am)
      sigma_error = x(1)
      sigma_covariance = x(2)

      n = size(am%covariance_matrix,1)
      allocate(ICF(n,n),DCDS(n,n),IC(n,n),ICDCDS(n,n))

      am%covariance_matrix = exp(-0.5_dp * am%distance_matrix/sigma_covariance**2)
      DCDS = am%covariance_matrix * am%distance_matrix / sigma_covariance**3

      do i = 1, n
         am%covariance_matrix(i,i) = am%covariance_matrix(i,i) + sigma_error**2
      enddo

      LA_covariance = am%covariance_matrix

      if (am%use_qr) then
         call Matrix_QR_Solve(LA_covariance,am%force_covariance_matrix,ICF)
         call Matrix_QR_Solve(LA_covariance,DCDS,ICDCDS)
         call LA_Matrix_QR_Inverse(LA_covariance,IC)
      else
         call Matrix_Solve(LA_covariance,am%force_covariance_matrix,ICF)
         call Matrix_Solve(LA_covariance,DCDS,ICDCDS)
         call LA_Matrix_Inverse(LA_covariance,IC)
      end if

      dlikelihood(1) = sigma_error * (3*trace(IC) - sum(IC*ICF) )
      dlikelihood(2) = 1.5_dp * trace(ICDCDS) - 0.5_dp * sum(ICDCDS*transpose(ICF))
 !    write(*,*) "DEBUG dlikelihood", dlikelihood(1)**2, dlikelihood(2)**2

      call finalise(LA_covariance)
      if(allocated(ICF)) deallocate(ICF)
      if(allocated(DCDS)) deallocate(DCDS)
      if(allocated(IC)) deallocate(IC)
      if(allocated(ICDCDS)) deallocate(ICDCDS)

   endfunction dlikelihood

 function  weight_neighbour(r, r0, m, weight_index)

     real(dp), intent(in)                    :: r, r0, m
     integer, intent(in), optional           :: weight_index
     integer                                 :: selector
     real(dp)                                :: weight_neighbour, kernel

     if (present(weight_index)) then
           selector=weight_index
     else
         selector=0
     endif

     SELECT CASE (selector)
     CASE (0)
        weight_neighbour = exp(-((r/r0)**m)) /r
     CASE (1)
        weight_neighbour = exp(-((r/r0)**m))
     !CASE (2)
     !   kernel=sqrt(m*2_dp)*(r/r0)
     !   weight_neighbour= (1_dp/GAMMA(m) / (2_dp**(m-1)) ) * (kernel**m) * BESYN(int(m), kernel)
     END SELECT

 end function weight_neighbour

 function ionic_charge(z)
     ! no partial charges
     integer                           ::		  i, ionic_charge
     integer                           ::		  x(8)
     integer, allocatable              ::                 i_index(:)
     real(dp), allocatable             ::                 xx(:)
     integer, intent(in)               ::		  z
     !
     x=[0, 2, 10, 18, 36, 64, 86, 118]
     allocate(xx(size(x)))
     allocate(i_index(size(x)))
     do i=1, size(x)
		i_index(i) = i
		xx(i) = 1.0_dp*abs(x(i) - z)
     enddo
     call heap_sort(xx,i_data=i_index)
     ionic_charge = z- x(i_index(1))
     !
     deallocate(xx, i_index)
 endfunction ionic_charge

function char_weight(z1, z2, do_atz_index)
  integer, intent(in)                    :: z1, z2
  integer, intent(in), optional          :: do_atz_index
  real(dp)                               :: char_weight
  integer                                :: selector

  if (present(do_atz_index) ) then
      selector=do_atz_index
  else
      selector=0
 endif

select case (selector)
case (0)
   char_weight=1.0_dp
case (1)
    char_weight=real(z1*z2, dp)
case (2)
      char_weight=real(ionic_charge(z1)*ionic_charge(z2), dp)
end select
!
endfunction char_weight

!
function internal_vector(at, r0, m, species_number, n_center_atom, weight_index, do_atz_index)
      real(dp)                            :: internal_vector(3), delta_r(3), delta_r_len
      real(dp), intent(in)                :: r0, m
      type(Atoms), intent(in)             :: at
      integer, intent(in), optional       :: weight_index, do_atz_index
      integer, intent(in)                 :: species_number
      integer                             :: i, j, n_center_atom
      !
      internal_vector = 0.0_dp
      do i=1, n_neighbours(at, n_center_atom)
          j = neighbour(at, n_center_atom, i, distance=delta_r_len, diff=delta_r)
          !
          if ((at%z(j) .eq. species_number) .or. (species_number.eq.0))  then
                 internal_vector = internal_vector + delta_r * weight_neighbour(delta_r_len, r0, m, weight_index) * char_weight(at%z(j), at%z(n_center_atom), do_atz_index=do_atz_index)
          endif
      enddo
      !
endfunction  internal_vector

 subroutine load_iv_params(data_dir, r_grid, m_grid, species, k, add_vector, n_data)

   ! reads r and m from iv_params file. The first line must contain the number
   ! of internal vector, and the following lines should be formatted in two columns
   ! with commas as spacers (a common csv file)
   character(STRING_LENGTH), intent(in)                            :: data_dir
   real(dp), dimension(:), intent(out), allocatable                :: r_grid, m_grid
   integer, dimension(:), intent(out), allocatable                 :: species
   integer                                                         :: i, m
   integer, intent(out)                                            :: k, add_vector
   integer, intent(out), optional                                  :: n_data

   open (unit=22, file=trim(data_dir)//'grid.dat', status='old', action='read')
   read(22,*) k, add_vector, m

   call print("Number of iv: "//k)
   if (present(n_data)) n_data=m
   if (.not. allocated(r_grid))  allocate(r_grid(k))
   if (.not. allocated(m_grid))  allocate(m_grid(k))
   if (.not. allocated(species)) allocate(species(k))

   call print(" Reading internal vectors parameters from file "//trim(data_dir)//"grid.dat")

   do i=1, k
      read(22,*) r_grid(i), m_grid(i), species(i)
      call print("Vector "//i//" : "//r_grid(i)//" "//m_grid(i)//" "//species(i))
   end do
   close(22)

 end subroutine load_iv_params

 subroutine write_iv_params(data_dir, r_grid, m_grid, species, k_in, add_vector, n_data)
  character(STRING_LENGTH), intent(in)                            :: data_dir
  real(dp), dimension(:), intent(in)                              :: r_grid, m_grid
  integer,  dimension(:), intent(in)                              :: species
  integer                                                         :: i
  integer, intent(in)                                             :: k_in, n_data, add_vector

  open (unit=22, status='replace', file=trim(data_dir)//'grid.dat', form='FORMATTED')

   write(22, *) k_in,  add_vector, n_data
  do i=1, k_in
     write(22, *) r_grid(i), m_grid(i), species(i)
  enddo
  close(unit=22)

 end subroutine write_iv_params

 function cov_dij(sigma_cov, dist, func_type)
    real(dp), intent(in)                  ::        sigma_cov, dist
    real(dp)                              ::        cov_dij, d_sq
    integer, intent(in), optional         ::        func_type
    integer                               ::        selector

    d_sq = dist

    if (present(func_type)) then
         selector=func_type
    else
         selector=0
    endif

    SELECT CASE (selector)
    CASE (0)
       cov_dij = exp(-0.5_dp*d_sq/sigma_cov**2)
    CASE (1)
       cov_dij = sqrt(d_sq)
    CASE (2)
       cov_dij = d_sq
    CASE (3)
       cov_dij = (sqrt(d_sq))**3
    CASE (4)
       cov_dij = d_sq**2
    END SELECT

 end function cov_dij

 function cov(feature_matrix1, feature_matrix2, bent_space1, bent_space2, iv_weights, sigma_cov, distance, func_type)

    real(dp), intent(in)                  ::        feature_matrix1(:,:), feature_matrix2(:,:), bent_space1(:,:), bent_space2(:,:), iv_weights(:), sigma_cov
    real(dp), intent(out), optional       ::        distance
    real(dp)                              ::        cov, d_sq
    integer, intent(in), optional         ::        func_type
    integer                               ::        i, k, selector

    k = size(feature_matrix1(:,1))
    d_sq = 0.0_dp
    do i=1, k
       d_sq = d_sq + (distance_in_bent_space(feature_matrix1(i,:), feature_matrix2(i,:), bent_space1, bent_space2))**2 / (2.0_dp * (iv_weights(i)**2))
    enddo

    ! norm with the dimensionality k of the Internal Space
    d_sq = d_sq/real(k,dp)

    if (present(distance))  distance = d_sq  ! N.B. distance squared

    if (present(func_type)) then
         selector=func_type
    else
         selector=0
    endif

    SELECT CASE (selector)
    CASE (0)
       cov = exp(-0.5_dp*d_sq / sigma_cov**2)
    CASE (1)
       cov = sqrt(d_sq)
    CASE (2)
       cov = d_sq
    CASE (3)
       cov = (sqrt(d_sq))**3
    CASE (4)
       cov = d_sq**2
    CASE (5)  ! Laplace
       cov = exp(- sqrt(d_sq) / sigma_cov)
    END SELECT

 endfunction cov

 function  distance_in_bent_space(vect1, vect2, bent_space1, bent_space2)

    real(dp), intent(in)                    :: vect1(3), vect2(3), bent_space1(:,:), bent_space2(:,:)
    real(dp)                                :: distance_in_bent_space

    distance_in_bent_space = norm( (bent_space1 .mult. vect1) - (bent_space2 .mult. vect2))

 endfunction distance_in_bent_space

! function eval_lhood(covariance_matrix,FCM)
!   real(dp), dimension(:,:), intent(in) :: covariance_matrix, FCM
!   real(dp) :: eval_lhood
!   real(dp), dimension(:,:), allocatable :: ICF
!   type(LA_Matrix) :: LA_covariance

!   LA_covariance = covariance_matrix
!   allocate(ICF(size(FCM,1),size(FCM,1)))

!   call Matrix_QR_Solve(LA_covariance, FCM, ICF)
   !write(*,*) "DEBUG ICF: ", ICF

!   eval_lhood = 1.5_dp * LA_Matrix_LogDet(LA_covariance) + 0.5_dp * trace(ICF)

!   call finalise(LA_covariance)
!   if(allocated(ICF)) deallocate(ICF)

! endfunction eval_lhood

 subroutine matrix_statistics(in_matrix, max_value, mean_value, deviation_value, n)

   real(dp), intent(in)                    ::  in_matrix(:,:)
   integer, intent(in)                     ::  n
   integer                                 ::  i, j, t
   real(dp), intent(out)                   ::  max_value, mean_value, deviation_value
   real(dp), allocatable                   ::  data_array(:)

   allocate(data_array(n*(n-1)/2))

    t=0
    do i=1, n
       do j=1,n
          if (i>j) then
             t=t+1
             data_array(t) = in_matrix(i,j)
          endif
       enddo
    enddo
    mean_value = sum(data_array)/real(size(data_array),dp)
    max_value  = maxval(data_array)

    deviation_value = 0.0_dp
    do i=1, size(data_array)
       deviation_value = deviation_value + ( (data_array(i) - mean_value)**2 )
    enddo
    deviation_value = sqrt(deviation_value/real(size(data_array),dp))

    deallocate(data_array)

 end subroutine  matrix_statistics

 subroutine sorting_configuration(matrix_predict, matrix_data, matrix_predict_norm, matrix_data_norm, sigma, distance_confs_unsorted, distance_index)

   ! returns a list of indexes corresponding to the configurations ordered according to their distance to target
   real(dp), intent(in)                        ::  matrix_predict(:,:), matrix_data(:,:,:), matrix_predict_norm(:,:), matrix_data_norm(:,:,:), sigma(:)
   real(dp), intent(inout)                     ::  distance_confs_unsorted(:)
   real(dp), allocatable                       ::  distance_confs(:) ! only for sorting
   integer,  intent(inout)                     ::  distance_index(:)
   real(dp)                                    ::  cov_tmp
   integer                                     ::  i

   call system_timer('Distance Calculation for sorting')

   !$omp parallel default(none) shared(matrix_data, matrix_data_norm, matrix_predict, matrix_predict_norm, sigma, distance_confs_unsorted, distance_index) private(i, cov_tmp)
   !$omp do
   do i=1, size(matrix_data(1,1,:))
      cov_tmp = cov(matrix_predict, matrix_data(:,:,i), matrix_predict_norm, matrix_data_norm(:,:,i), sigma, 1.0_dp, distance=distance_confs_unsorted(i))
      distance_index(i) = i
   enddo
   !$omp end parallel

   call system_timer('Distance Calculation for sorting')

   call system_timer('Sorting the DATABASE')

!  call insertion_sort(distance_confs_unsorted, idx=distance_index)
!  heap sorting below
   allocate(distance_confs(size(distance_confs_unsorted)))
   distance_confs = distance_confs_unsorted
   call heap_sort(distance_confs, i_data=distance_index)
   call system_timer('Sorting the DATABASE')
   deallocate(distance_confs)
 end subroutine sorting_configuration

 subroutine  real_expection_sampling_components(ivs_direction_matrix, internal_component_matrix, target_force)

    real(dp), intent(in)                        ::  ivs_direction_matrix(:,:), internal_component_matrix(:)
    real(dp), intent(out)                       ::  target_force(3)
    real(dp), allocatable                       ::  force_array(:,:)
    real(dp)                                    ::  const=0.1_dp, f_deviv(3), converting_matrix(3,3), converting_matrix_inv(3,3), force_value_matrix(3)
    integer                                     ::  i, j, t, f_counter, s

    f_counter=0

    s=size(internal_component_matrix)  ! the number of internal directions, equavalent to 'k'
    allocate(force_array(s*(s-1)*(s-2)/6,3)) ! number of combinations: C_{k}^{3}
    force_array=0.0_dp

    do i=1, s-2
       do j=i+1, s-1
          do t=j+1, s
               f_counter = f_counter + 1
               converting_matrix(:,1)=ivs_direction_matrix(i,:)
               converting_matrix(:,2)=ivs_direction_matrix(j,:)
               converting_matrix(:,3)=ivs_direction_matrix(t,:)
               force_value_matrix(1)=internal_component_matrix(i)
               force_value_matrix(2)=internal_component_matrix(j)
               force_value_matrix(3)=internal_component_matrix(t)

               if ((norm(converting_matrix(:,1) .cross. converting_matrix(:,2)) > const) .and. (norm(converting_matrix(:,2) &
                           .cross. converting_matrix(:,3)) > const) .and. (norm(converting_matrix(:,3) .cross. converting_matrix(:,1)) > const)) then
                 f_counter = f_counter + 1

                 write(*,*) "the converting matrix", converting_matrix
                 call inverse_svd_threshold(converting_matrix, 10.0_dp, result_inv=converting_matrix_inv)
                 force_array(f_counter,:)=transpose(converting_matrix_inv) .mult. force_value_matrix
                 write(*,*) "the inverse matrix : ", converting_matrix_inv
                 write(*,*) "the number of sequence of ivs:", i, j, t
                 call print("Target Force Distribution : "//force_array(f_counter,1)//" "//force_array(f_counter,2)//" "//force_array(f_counter,3))
               endif
           enddo
       enddo
     enddo

   do i=1, 3
      target_force(i) = sum(force_array(:,i)) / real(f_counter, dp)
   enddo
   f_deviv=0.0_dp
   do j=1, 3
      do i=1,f_counter
        f_deviv(j) = f_deviv(j) + (force_array(i,j)-target_force(j))**2
     enddo
   enddo
   do j=1, 3
      f_deviv(j) = sqrt(f_deviv(j)/ real(f_counter,dp))
   enddo
   write(*,*) "deviation of the prediction: ", f_deviv
   deallocate(force_array)
 end subroutine real_expection_sampling_components

 subroutine internal_dimension_mark(in_matrix, TOL, mark)

    real(dp), intent(in)                ::  in_matrix(:,:), TOL
    integer, intent(out)                ::  mark(3)
    integer                             ::  i

    mark(:)=1
    do i=1,3
       !if (sum(abs(in_matrix(:,i))/size(in_matrix(:,1)))< TOL)  then ! using Mean as Indicator
       if ( maxval(abs(in_matrix(:,i))) < TOL ) then ! using Max as indicator
              call print("dimension index: "//maxval(abs(in_matrix(:,i))) )
              mark(i)=0
              call print('the '//i//'-th dimension is zero')
       endif
    enddo

 end subroutine  internal_dimension_mark

 subroutine rank_lowered_inverse(in_matrix, out_inv)

   real(dp), intent(in)               ::    in_matrix(3,3)
   real(dp), intent(out)              ::    out_inv(3,3)
   real(dp)                           ::    a, b, c, d, m_factor

   a=in_matrix(1,1)
   b=in_matrix(1,2)
   c=in_matrix(2,1)
   d=in_matrix(2,2)
   m_factor = a*d-b*c
   out_inv=0.0_dp

   out_inv(1,1) =  d/m_factor
   out_inv(1,2) = -b/m_factor
   out_inv(2,1) = -c/m_factor
   out_inv(2,2) =  a/m_factor

 end subroutine rank_lowered_inverse

 subroutine do_optimise_likelihood(sigma_error, sigma_covariance)

   type(optimise_likelihood)           :: am_likelihood
   logical                             :: use_qr
   integer                             :: am_data_size
   character(len=1), dimension(1)      :: am_mold
   character(len=1), allocatable       :: am_data(:)
   real(dp)                            :: x_sigma(2), covariance_tol
   real(dp), intent(inout)             :: sigma_error, sigma_covariance
 !  real(dp), dimension(:,:), target    :: distance_matrix, covariance_matrix, force_covariance_matrix

   covariance_tol = 1.0e-2
   use_qr=.true.

!   am_likelihood%use_qr = use_qr ! filling the am_likelihood container
!   am_likelihood%distance_matrix => distance_matrix
!   am_likelihood%covariance_matrix => covariance_matrix
!   am_likelihood%force_covariance_matrix => force_covariance_matrix

   am_data_size = size(transfer(am_likelihood,am_mold))
   allocate(am_data(am_data_size))
   am_data = transfer(am_likelihood,am_mold) ! pouring the content of am_likelihood into am_data

   ! Testing the implementation of likelihood and dlikelihood.
   if(.false.) then
      call print('starting test gradient')
      call verbosity_push(PRINT_VERBOSE)
      write (*,*) test_gradient((/sigma_error,sigma_covariance/),likelihood,dlikelihood,data=am_data)
      call verbosity_pop()
   endif
   call print_title('likelihood optimisation')

   if (use_qr) then
      call print('doing likelihood optimisation with QR decomposition')
   else
      call print('doing likelihood optimisation with Cholesky decomposition')
   end if
   x_sigma = (/sigma_error, sigma_covariance/)    ! starting points for the values
   ! n = minim( x_sigma, likelihood, dlikelihood, method='cg', convergence_tol=covariance_tol, max_steps = 100, data=am_data)
   ! x_sigma is the input vector, lh is the target function to minimise (using dlh), using conjugate gradient, with that tolerance,
   ! with that max number of steps, and using the data contained in am_data
   write(*,*) "Fixed LIKELIHOOD", likelihood(x_sigma, am_data), dlikelihood(x_sigma, am_data)
   deallocate(am_data)

   sigma_error = x_sigma(1)
   sigma_covariance = x_sigma(2)   ! return the values of sigma_error and sigma_covariance after the optimisation
   call print('Optimised hypers. ERROR = '//sigma_error//' COVARIANCE = '//sigma_covariance)

 end subroutine do_optimise_likelihood

subroutine sigma_factor_from_single_dimension(ivs_set, sigma, print_verbosity, TOL_REAL, data_dir)
	! calculation normalisation factor on IV_i
	integer                             :: t, i, j, n, k
	real(dp)                            :: dist_primitive, distance_ivs_stati, feature_len
	real(dp), intent(inout)             :: sigma(:)
real(dp), allocatable               :: distance_ivs(:,:), mean_value(:), max_value(:), deviation_value(:), ivs_set_norm(:,:,:)
	real(dp), intent(in)                :: ivs_set(:,:,:), TOL_REAL
	logical, intent(in)                 :: print_verbosity
	character(STRING_LENGTH),intent(in) :: data_dir

	n=size(ivs_set(1,1,:))
	k=size(ivs_set(:,1,1))
allocate(ivs_set_norm(k,3,n))

	do j=1, n
	do t=1, k
feature_len=norm(ivs_set(t,:,j))
	if (feature_len < TOL_REAL)  then
	feature_len=1.0_dp
	call print("WARNING: Numerical Limit in getting the unit direction of IVs : Generating sigma.dat")
	endif
	ivs_set_norm(t,:,j) = ivs_set(t,:,j)/feature_len
	enddo
	enddo

allocate(distance_ivs(n,n), mean_value(k), max_value(k), deviation_value(k))

	call system_timer('Getting Hyper-parameters from the DATABASE')

	dist_primitive=0.0_dp
	do t = 1, k
	do i = 1, n
	do j=1, n
distance_ivs(i,j) = distance_in_bent_space(ivs_set(t,:,i), ivs_set(t,:,j), ivs_set_norm(:,:,i), ivs_set_norm(:,:,j))
	if ((i>j) .and. (print_verbosity)) call print("Dimensional Distance : "//distance_ivs(i, j)//"  dimension : "//t)
	enddo   ! i
	enddo     ! j

call matrix_statistics(distance_ivs, max_value(t), mean_value(t), deviation_value(t), n)
	call print("Statistics max value: "//max_value(t)//" mean value: "//mean_value(t)//" deviation_value: "//deviation_value(t))
	dist_primitive = dist_primitive + (mean_value(t)/deviation_value(t)/2.0_dp)**2
	enddo

dist_primitive = sqrt(dist_primitive / real(k,dp))
	call print("primitive_distance : "//dist_primitive)

	if (print_verbosity) then
	! norm distance with hyperparameters derived from statistics analysis
	do i=1,n
	do j=1,n
	distance_ivs_stati = 0.0_dp
	do t=1, k
	distance_ivs_stati = distance_ivs_stati + &
	(distance_in_bent_space(ivs_set(t,:,i), ivs_set(t,:,j), ivs_set_norm(:,:,i), ivs_set_norm(:,:,j))/2.0_dp/deviation_value(t))**2
	enddo
	distance_ivs_stati = sqrt(distance_ivs_stati/real(k,dp))          ! to take the square root and get a norm distance
	if (i>j)   call print("Normalised Dimensional Distance : "//distance_ivs_stati//"  Normalised Value : "//distance_ivs_stati/dist_primitive)
	enddo
	enddo
	endif ! print_verbosity

    ! to write the derived sigma vector into file "sigma.dat" for later use
    ! sigma array is derived from the standard deviation on each single dimension of the IVs space
    sigma = deviation_value
    open(unit=1, status='replace', file=trim(data_dir)//'sigma.dat')
    write(1,*) sigma
    close(unit=1)

    deallocate(distance_ivs, mean_value, max_value, deviation_value, ivs_set_norm)
    call system_timer('Getting hyper-parameters from the DATABASE')
end subroutine sigma_factor_from_single_dimension

subroutine write_distance_file_real(d, data_dir)

   character(STRING_LENGTH), intent(in)                            :: data_dir
   real(dp), intent(in)                                            :: d

!   OPEN(2, status='replace',file=trim(data_dir)//'Dist.dat',  form='UNFORMATTED')
!   !write(2)  d
!   close(2)
    call fwrite_array_d(1, (/0.0_dp/), trim(data_dir)//'Dist.dat')
end subroutine write_distance_file_real

!subroutine  refresh_info(data_dir, f_proj, f_norm, ivs_set)
!
!      character(STRING_LENGTH), intent(in)                            ::   data_dir
!      real(dp), intent(in)                                            ::   f_proj(:), ivs_set(:,:)
!      real(dp), intent(in)                                            ::   f_norm
!      integer                                                         ::   i
!      call write_distance(0.0_dp, data_dir) ! always refresh the Dist.dat whenever print_data
!
!      OPEN(2,status='replace',file=trim(data_dir)//'Force.dat', form='UNFORMATTED')
!      OPEN(3,status='replace',file=trim(data_dir)//'IV.dat', form='UNFORMATTED')
!
!      write(2)  f_proj(:), f_norm
!
!      do i=1,3
!             write(3)  ivs_set(:,i)
!      enddo
!      close(2)
!      close(3)
!end subroutine refresh_info

subroutine write_distance_file(distance_confs_unsorted, data_dir)

   character(STRING_LENGTH), intent(in)                            :: data_dir
   real(dp), intent(in), dimension(:)                              :: distance_confs_unsorted

   call system_timer('Writing Distance File : Trivial Usually')
!   OPEN(2, status='old',file=trim(data_dir)//'Dist.dat', form='UNFORMATTED', position='APPEND')
!   write(2)  distance_confs_unsorted(:)
!   close(2)
   call fwrite_array_d(size(distance_confs_unsorted), distance_confs_unsorted(:), trim(data_dir)//'Dist.dat')
   call system_timer('Writing Distance File : Trivial Usually')

end subroutine write_distance_file

subroutine  read_distance_file(distance_matrix, data_dir)

       character(STRING_LENGTH), intent(in)                               :: data_dir
       real(dp), intent(inout), dimension(:, :)                           :: distance_matrix
       integer                                                            :: i, j

       call system_timer('Read Pair Distance and Bulid Covariance')
 !      OPEN(2,status='old',file=trim(data_dir)//'Dist.dat',form='UNFORMATTED')
       call fread_array_d(1, distance_matrix(1,1:1), trim(data_dir)//'Dist.dat')

       do i=2, size(distance_matrix(:,1))
              call  fread_array_d(i-1, distance_matrix(i,:(i-1)), trim(data_dir)//'Dist.dat')
  !           read(2) distance_matrix(i,:(i-1))                 ! for row <= column
              distance_matrix(i,i)=0.0_dp
       enddo

       do i=1, size(distance_matrix(:,1))
            do j= i, size(distance_matrix(:,1))
                distance_matrix(i,j) = distance_matrix(j,i)  ! for row > column
            enddo
        enddo
  !      close(2)
       call system_timer('Read Pair Distance and Bulid Covariance')

end subroutine read_distance_file
!
function fgp_calc(at_in)

   type(Atoms)         :: at_in, fgp_calc
   type(Potential)     :: pot
   type(Dictionary)    :: params
   real(dp)            :: r_cut, feature_len, thresh, sigma_error, error_ls, error_gp, error_gp_sq, time, error_ls_max
   real(dp)            ::  sigma_covariance, sigma_cov, dist_shift_factor, dim_tol, error_frame, force_magntd_pred, dist
   real(dp), dimension(:), allocatable           :: r_grid, m_grid, sigma, covariance_pred,force_magntd_data,force_magntd_data_select
   real(dp), dimension(:), allocatable           :: force_proj_test, force_proj_ivs_pred, distance_confs_unsorted
   real(dp), parameter                           :: TOL_REAL=1e-7_dp, SCALE_IVS=100.0_dp
   real(dp)                                      :: force(3), variance_xyz(3), feature_inner_matrix(3,3), feature_inv(3,3), kappa
   real(dp), dimension(:,:,:), allocatable       :: ivs_set_norm, ivs_set, ivs_set_pred, nearest_data
   real(dp), dimension(:,:), allocatable         :: force_proj_ivs, force_proj_ivs_select, inv_covariance, out_u, out_vt, geometry_matrix
   real(dp), dimension(:,:), allocatable, target :: covariance, distance_matrix, distance_matrix_dij, force_covariance_matrix, covariance_tiny
   real(dp), dimension(:,:), allocatable         :: ivs_set_norm_pred, ivs_set_norm_pred_t
   real(dp), dimension(:,:), pointer             :: force_ptr, force_ptr_mm, force_ptr_tff
   real(dp), dimension(:), pointer               :: pred_err
   integer                                       :: i,j, k, n,  t,  n_center_atom, error
   integer                                       :: add_vector, n_teach, func_type, weight_index, temp_integer, ii, do_atz_index
   integer	                 		 :: local_ml_optim_size, dimension_mark(3)
   integer, dimension(:), allocatable            :: distance_index, mark_zero_ivs, species
   logical                         ::             do_gp, do_sigma, least_sq, do_svd, print_verbosity,  do_insert, do_dynamic_x, write_data_dij, read_data_dij
   character(STRING_LENGTH)                      :: data_dir

   ! set by default exc. at_in
   call initialise(params)
   call param_register(params, 'r_cut',  '8.0', r_cut, "the cutoff radius for the spherical atomic environment")
   call param_register(params, 'thresh', '10.0', thresh, "the threshold for doing the Singular Value Decompostion of the Covariance Matrix")
   call param_register(params, 'weight_index', '0', weight_index, "Type of weight function in deriving IVs")
   call param_register(params, 'sigma_error', '0.05', sigma_error, "the noise assumed on the teaching data")
   call param_register(params, 'func_type', '0', func_type, "which kernel function is used to build the covariance matrix")
   call param_register(params, 'do_gp',  'T', do_gp, "true for doing a gaussian processes")
   call param_register(params, 'n_teach', '1000', n_teach, "the number of relevant confs you would like to do machine learning with")
   call param_register(params, 'do_sigma', 'F', do_sigma, "set true to print out the distance on every single dimension of the IVs space")
   call param_register(params, 'do_dynamic_x', 'F', do_dynamic_x, "true for updating sigma.dat every test configuration based on nearest data points")
   !call param_register(params, 'top_teach', 'T', top_teach, "only the first configuration is considered when doing teaching")
   call param_register(params, 'least_sq',    'T', least_sq, "if true, the internal force components will be tranformed to real force using least squares")
   call param_register(params, 'do_svd', 'F', do_svd, "if true, doing inverting by SVD")
   call param_register(params, 'sigma_cov', '1.0', sigma_cov, "correlation length for pairs of data points")
   call param_register(params, 'dist_shift_factor', '0.2', dist_shift_factor, "distance shifted with a given factor")
   ! optional for updating sigma_cov
   call param_register(params, 'do_atz_index', '0', do_atz_index, "0: charge-weight disabled; 1: using at%Z, 2: using ionic charge instead")
   call param_register(params, 'dim_tol', '0.00005', dim_tol, "threshold for determing the dimensionality of the group of IVs")
   !call param_register(params, 'test_file', 'test.xyz', test_file, "file to read the testing configurations from")
   call param_register(params, 'data_dir', './info/', data_dir, "the directory where data files locate")
   call param_register(params, 'do_insert', 'F', do_insert, "INsert any addiitional vector as force_plus")
   call param_register(params, 'print_verbosity', 'F', print_verbosity, "if true, print out verbosity stuff")
   !call param_register(params, 'out_file', 'out.xyz', out_file, "output configuration file containing the predicted forces")
   call param_register(params, 'read_data_dij', 'F', read_data_dij, 'whether or not writing the distance file')
   call param_register(params, 'write_data_dij', 'F', write_data_dij, 'whether or not reading the pair distnace matrix')
   call param_register(params, 'local_ml_optim_size', '0', local_ml_optim_size, "Optimise sigma_error and sigma_cov using local_ml_optim_size LS confs. If 0, no optimisation is performed")

   if (.not. param_read_args(params, task="fgp command line params")) then
      call print("Usage: fgp [options]")
      call system_abort("Confused by command line arguments")
   end if

   call print_title('params')
   call param_print(params)
   call finalise(params)

    call print_title('READING TEACHING INFORMATION')
    call system_timer('Collecting Information From Files')
    call load_iv_params(data_dir, r_grid, m_grid, species, k, add_vector, n_data=n)
    k = k + add_vector  ! adding the Classical Force Vectors

    allocate(force_proj_ivs(k,n))
    allocate(ivs_set(k,3,n))
    allocate(ivs_set_norm(k,3,n))
    allocate(force_magntd_data(n))
    allocate(sigma(k))

    OPEN(2,status='old',file=trim(data_dir)//'Force.dat',form='UNFORMATTED')
    OPEN(3,status='old',file=trim(data_dir)//'IV.dat',form='UNFORMATTED')

    do t=1, n
       read(2) force_proj_ivs(:, t), force_magntd_data(t)
       !write(*,*) "%", force_proj_ivs(:, t), force_magntd_data(t)
       do i=1, 3
          read(3) ivs_set(:,i,t)
       enddo
       !write(*,*) "$", ivs_set(:,:,t)
    enddo
    close(2)
    close(3)

  ! calculating the normalised vectors
  do t=1, n
    do j=1, k
     feature_len = norm(ivs_set(j,:, t))
     if (feature_len < TOL_REAL)  then
             feature_len=1.0_dp
             call print("WARNING: TEACHING, encountered the numerical limit in getting the unit direction of IVs")
     endif
     ivs_set_norm(j,:,t)=ivs_set(j,:, t)/feature_len
    enddo
 enddo

    ! reading sigma file
    open(unit=1, status='old', file=trim(data_dir)//'sigma.dat', form='formatted')
    read(1, *) sigma
    close(unit=1)
    call system_timer('Collecting Information From Files')
    call print('sigma is:    '//sigma)

!
if (n_teach > n) then
  n_teach = n
  call print("the Actual Number of Configs for Prediction: "//n_teach)
  call print("WARNNING: Trying to select more configurations than the database has")
endif
!
allocate(covariance_pred(n_teach))
allocate(force_proj_ivs_pred(k))
allocate(mark_zero_ivs(k))
allocate(geometry_matrix(3,k))
allocate(covariance(n_teach,n_teach))
allocate(distance_confs_unsorted(n), distance_index(n))
if (local_ml_optim_size > 0) then
allocate(distance_matrix(local_ml_optim_size,local_ml_optim_size), force_covariance_matrix(local_ml_optim_size,local_ml_optim_size), covariance_tiny(local_ml_optim_size,local_ml_optim_size))
endif
allocate(inv_covariance(n_teach,n_teach))
allocate(force_proj_ivs_select(k, n_teach))
allocate(force_magntd_data_select(n_teach))
allocate(ivs_set_norm_pred(k,3))
if (read_data_dij) allocate(distance_matrix_dij(n, n))

if (read_data_dij) then
          call system_timer('Read Pair Distance and Bulid Covariance')
          call read_distance_file(distance_matrix_dij(:,:), data_dir)
          !write(*,*)  'Distance Read', distance_matrix_dij
          call system_timer('Read Pair Distance and Bulid Covariance')
endif

   if (.not. get_value(at_in%params, 'time', time))  then
     call print("TIME of FRAME : UNKOWN")
   else
     call print("TIME of FRAME :"//time)
   endif
   call add_property(at_in, 'force', 0.0_dp, n_cols=3, overwrite=.false.) ! false in case DFT force is reading in
   call assign_property_pointer(at_in, 'force', ptr=force_ptr)

   ! for writing out the predicted error
   call add_property(at_in, 'pred_err', 0.0_dp, n_cols=1, overwrite=.true.)
   call assign_property_pointer(at_in, 'pred_err', ptr=pred_err)

   if (add_vector > 0) then
           if (do_insert) then
              call assign_property_pointer(at_in, 'force_plus', ptr=force_ptr_mm)
           else
             call Potential_Filename_Initialise(pot, args_str='IP SW', param_filename='SW.xml')
             call calc(pot, at_in, args_str='force=force_mm',  error=error)
             call assign_property_pointer(at_in, 'force_mm', ptr=force_ptr_mm)
             call finalise(pot)
           endif
   endif
   if (add_vector>1) call assign_property_pointer(at_in, 'force_i', ptr=force_ptr_tff)

   call set_cutoff(at_in, r_cut)
   call calc_connect(at_in)

   allocate(ivs_set_pred(k,3,at_in%N))

  error_frame=0.0_dp           ! for calculating the average error of the system.

  do n_center_atom=1, at_in%N     ! loop over atoms in the Frame, make prediction (incl. error)

     do j=1, k                  ! reset the mark for zero internal vectors
         mark_zero_ivs(j)=1
     enddo

     do j= 1, k-add_vector
        ivs_set_pred(j,:,n_center_atom) = internal_vector(at_in, r_grid(j), m_grid(j), species(j), n_center_atom, weight_index, do_atz_index) *SCALE_IVS
        call print("internal vectors ( "//r_grid(j)//" "//m_grid(j)//" "//species(j)//" ):  "//ivs_set_pred(j,1,n_center_atom)//"  "//ivs_set_pred(j,2,n_center_atom)//"  "//ivs_set_pred(j,3,n_center_atom))
       feature_len = norm(ivs_set_pred(j,:,n_center_atom))

        if (feature_len < TOL_REAL)  then
            feature_len=1.0_dp
            mark_zero_ivs(j)=0
            call print("WARNING: PREDICTION, encountered the numerical limit in getting the unit direction of IVs")
        endif

        ivs_set_norm_pred(j,:) = ivs_set_pred(j,:,n_center_atom)/feature_len
     enddo

     if (add_vector>0) then
         do j = k-add_vector+1, k
            temp_integer=k-j
            select case (temp_integer)
            case (0)
               ivs_set_pred(j,:,n_center_atom)=force_ptr_mm(:, n_center_atom)
            case (1)
               ivs_set_pred(j,:,n_center_atom)=force_ptr_tff(:, n_center_atom)
            end select
            feature_len = norm(ivs_set_pred(j,:,n_center_atom))

            if (feature_len < TOL_REAL) then
               feature_len=1.0_dp
               mark_zero_ivs(j)=0
               call print("WARNING: PREDICTION, encountered the numerical limit in getting the unit direction of IVs")
            endif

            ivs_set_norm_pred(j,:) = ivs_set_pred(j,:,n_center_atom)/feature_len
            if (print_verbosity) call print("added internal vectors : "//ivs_set_pred(j,1,n_center_atom)//"  "//ivs_set_pred(j,2,n_center_atom)//"  "//ivs_set_pred(j,3,n_center_atom))
         enddo
      endif

     if (print_verbosity) then
       do j=1, k
          do ii=1, k
           if(print_verbosity) then
              call print("ATOM : "//n_center_atom//" ivs_set: ("//j//" "//ii//") "//dot_product(ivs_set_pred(j,:,n_center_atom), ivs_set_norm_pred(ii,:)) )
           endif
          enddo
       enddo
     endif

      ! initialise distance_index(:)
      do t=1, n
         distance_index(t) =t
      enddo

        ! do the sorting and selection
      call sorting_configuration(ivs_set_pred(:,:,n_center_atom), ivs_set, ivs_set_norm_pred, ivs_set_norm, sigma, distance_confs_unsorted, distance_index)
      call print("Min and Max DISTANCE with Index after Sorting: "//distance_confs_unsorted(distance_index(1))//" and "// &
	                                    distance_confs_unsorted(distance_index(n_teach))//"  the INDEX: "//distance_index(1)//" and "//distance_index(n_teach))
       !!!!!!!!!!!!!
    if (do_dynamic_x) then
        allocate(nearest_data(k,3,n_teach))
        do ii=1, n_teach
           nearest_data(:,:,ii)=ivs_set(:,:,distance_index(ii))
        enddo
        call sigma_factor_from_single_dimension(nearest_data, sigma, print_verbosity, TOL_REAL, data_dir)
        ! update 'sigma.dat'
        deallocate(nearest_data)
     endif

     if ( distance_confs_unsorted( distance_index(1) ) > dist_shift_factor) then
                ! distance_confs_unsorted: the pair distance betwen test and database.
                ! sigma_cov: the original input for covariance length
                sigma_covariance = sigma_cov*distance_confs_unsorted(distance_index(1))/dist_shift_factor
                ! this intrinsically changes the value of SIGMA_COVARIANCE and enters next loop
                call print("sigma_covariance are shifted by a factor of :"//distance_confs_unsorted(distance_index(1))/dist_shift_factor)
                call print("Modified sigma_covariance :"//sigma_covariance)
     else
               sigma_covariance = sigma_cov
               ! the sigma_covariance used in GP
     endif

     if (.false.) then
         ! massive output, only for testing use
         call print("Detail on the teaching information")
         do ii = 1, n_teach
            call print("Index: "//distance_index(ii)//" Distance: "//distance_confs_unsorted(ii))
            call print("Force in IVs space: "//force_proj_ivs(:,distance_index(ii)))
         enddo
     endif

     ! max marginal likelihood
      if (local_ml_optim_size > 0) call print("Optimise: "//local_ml_optim_size)
      !!! ML optimisation here: first calculates the necessary input matrices, then optimises
      if (local_ml_optim_size > 0) then
         call print("Local Maximum Likelihood selected")
         do t = 1, local_ml_optim_size  ! loop to generate the force_cov_mat and the distance_matrix of the restricted subset of the LS
            do j = t, local_ml_optim_size
               force_covariance_matrix(t,j) = dot_product(force_proj_ivs(:,distance_index(t)), force_proj_ivs(:,distance_index(j)))
               ! check if possible, may be a wreck because IVs make absolute representation of the force impossible
               force_covariance_matrix(j,t) = force_covariance_matrix(t,j)
               covariance_tiny(t,j) = cov(ivs_set(:,:,distance_index(t)), ivs_set(:,:,distance_index(j)), &
                    ivs_set_norm(:,:,distance_index(t)), ivs_set_norm(:,:,distance_index(j)), sigma, sigma_covariance, func_type=func_type, &
                    distance=distance_matrix(t,j))
               covariance_tiny(j,t) = covariance_tiny(t,j)
               distance_matrix(j,t) = distance_matrix(t,j)
            enddo
         enddo
         call do_optimise_likelihood(sigma_error, sigma_covariance)
      endif

      if (read_data_dij) then
          do t = 1, n_teach  ! loop to generate the covariance matrix of the learning set
                 dist=distance_matrix_dij(distance_index(t), distance_index(t))
                 covariance(t,t) = cov_dij(sigma_covariance, dist, func_type)
           !      write(*,*) 'diagoanl value', covariance(t,t)
                 if (do_gp) covariance(t,t) = covariance(t,t) + sigma_error**2
                 do j = t+1, n_teach ! above diagonal
                    dist=distance_matrix_dij(distance_index(t),distance_index(j))
                    covariance(t,j) = cov_dij(sigma_covariance, dist, func_type)
                    covariance(j,t) = covariance(t,j)
                 enddo
          enddo
      else
	      call system_timer('Evaluating the Cov')
	      !Generate covariance matrix
	      !$omp parallel default(none) shared(covariance, n_teach, ivs_set, ivs_set_norm, distance_index, sigma, sigma_error, sigma_covariance, do_gp, func_type), private(j)
	      !$omp do
	      do t = 1, n_teach  ! loop to generate the covariance matrix of the learning set
		 covariance(t,t) = cov(ivs_set(:,:,distance_index(t)), ivs_set(:,:,distance_index(t)), &
			ivs_set_norm(:,:,distance_index(t)), ivs_set_norm(:,:,distance_index(t)), sigma, sigma_covariance, func_type=func_type)
		 if (do_gp) covariance(t,t) = covariance(t,t) + sigma_error**2
		 do j = t+1, n_teach ! above diagonal
		    covariance(t,j) = cov(ivs_set(:,:,distance_index(t)), ivs_set(:,:,distance_index(j)), &
			 ivs_set_norm(:,:,distance_index(t)), ivs_set_norm(:,:,distance_index(j)), sigma, sigma_covariance, func_type=func_type)
		    covariance(j,t) = covariance(t,j)
		enddo
	       enddo
	      !$omp end parallel
	       call system_timer('Evaluating the Cov')
      endif ! not read_data_dij

     call system_timer('Inverting the Covariance Matrix')
      if (do_gp) then
         call inverse(covariance, inv_covariance)
      else
         ! To Do Sigular Value Decomposition (SVD): A = U*SIGMA*VT
         call inverse_svd_threshold(covariance, thresh, result_inv=inv_covariance)
      endif
      call system_timer('Inverting the Covariance Matrix')

      do t=1, n_teach
         force_proj_ivs_select(:,t)=force_proj_ivs(:,distance_index(t))
         force_magntd_data_select(t) = force_magntd_data(distance_index(t))
      enddo

      ! the test configuration covariance vector
      do t= 1, n_teach
        covariance_pred(t) = cov(ivs_set_pred(:,:,n_center_atom), ivs_set(:,:,distance_index(t)), ivs_set_norm_pred, &
                                         ivs_set_norm(:,:,distance_index(t)), sigma, sigma_covariance, func_type=func_type)
      enddo

      ! the predicted force components on each of the internal directions
      force_proj_ivs_pred(:) = matmul(covariance_pred, matmul(inv_covariance, transpose(force_proj_ivs_select(:,:)) ))

      ! Predict the Force Magnitude
      force_magntd_pred =  dot_product(covariance_pred, matmul(inv_covariance, force_magntd_data_select))

      ! we need to mannually set the force components to be zero, projected on the zero internal-vector directions
      do j=1, k
         force_proj_ivs_pred(j)=force_proj_ivs_pred(j)*real(mark_zero_ivs(j), dp)
      enddo

     if (print_verbosity) then
         do j=1, k
               call print("ivs_set_norm_pred"//ivs_set_norm_pred(j,:))
         enddo
     endif

     allocate(out_u(3,3), out_vt(3,3))      ! to obtain the transformation matrix with the principal axis
     call inverse_svd_threshold(transpose(ivs_set_norm_pred) .mult. ivs_set_norm_pred, thresh, u_out=out_u, vt_out=out_vt)

     allocate(ivs_set_norm_pred_t(k,3))

     call internal_dimension_mark(ivs_set_pred(:,:,n_center_atom) .mult. out_u, dim_tol, dimension_mark)

     ivs_set_norm_pred_t = ivs_set_norm_pred .mult. out_u

  ! ivs_set_pred_t contains the Internal-directions after PCA transformation.
  if (print_verbosity) then
       do j=1, k
           write(*,*) "ivs_set_norm_pred_t : ", ivs_set_norm_pred_t(j, :)
       enddo
  endif

  select case (sum(dimension_mark(:)))
    case(0)
         force= 0.0_dp
    case(1)
         force=(transpose(ivs_set_norm_pred) .mult. force_proj_ivs_pred )/real(sum(mark_zero_ivs),dp)
    case(2)
         feature_inner_matrix=transpose(ivs_set_norm_pred_t) .mult. ivs_set_norm_pred_t
         call rank_lowered_inverse(feature_inner_matrix, feature_inv)
         force = out_u .mult. feature_inv .mult. transpose(ivs_set_norm_pred_t) .mult. force_proj_ivs_pred
    case(3)

         feature_inner_matrix=transpose(ivs_set_norm_pred) .mult. ivs_set_norm_pred
         if (do_svd) then
             if (print_verbosity) write(*,*)  "feature inner matrix :", feature_inner_matrix
             call inverse_svd_threshold(feature_inner_matrix, thresh, result_inv=feature_inv)
             if (print_verbosity) write(*,*)  "inverse feature inner matrix :", feature_inv
         else
             if (print_verbosity) write(*,*)  "feature_inner_matrix :", feature_inner_matrix
             call inverse(feature_inner_matrix, feature_inv)
             if (print_verbosity) write(*,*)  "inverse feature inner matrix :", feature_inv
         endif

         if (least_sq) then
            force = feature_inv .mult. transpose(ivs_set_norm_pred) .mult. force_proj_ivs_pred
         else
            call real_expection_sampling_components(ivs_set_norm_pred, force_proj_ivs_pred, force)
         endif
   end select

  deallocate(out_u)
  deallocate(out_vt)
  deallocate(ivs_set_norm_pred_t)

   call print("Predicted Force Magnitude: "//force_magntd_pred)
   call print("force in external space : "//force//" Magnitude: "//norm(force))
   call print("the original force:"//force_ptr(:, n_center_atom)//" Magnitude: "//norm(force_ptr(:, n_center_atom)))
   call print("max error :    "//maxval(abs(force_ptr(:,n_center_atom)-force))//" norm  error :  "//norm(force_ptr(:,n_center_atom)-force))

   ! measure the consistence between the least-square inference and the components obtained from GP processes
   if (least_sq) then
       error_ls=0.0_dp
       do j=1, k
            error_ls = error_ls + (dot_product(ivs_set_norm_pred(j,:), force(:)) - force_proj_ivs_pred(j))**2
       enddo
       error_ls = sqrt(error_ls / real(k,dp))
       call print("deviation of least-square inference : "//error_ls)
   error_ls_max=maxval(abs( matmul(ivs_set_norm_pred, force) - force_proj_ivs_pred(:) ))
   call print("Max deviation Component of LS : "//error_ls_max)
   endif

   kappa = cov(ivs_set_pred(:,:,n_center_atom), ivs_set_pred(:,:,n_center_atom), ivs_set_norm_pred, ivs_set_norm_pred, sigma, sigma_covariance, func_type=func_type) + sigma_error**2
   ! the passing of  uncertainty from  Gaussian Processes to the Force Vector
   error_gp_sq = kappa - covariance_pred .dot. matmul(inv_covariance, covariance_pred)
   error_gp = sqrt(abs(error_gp_sq))
   geometry_matrix = feature_inv .mult. transpose(ivs_set_norm_pred)

   do j=1, 3  ! Deviation in External Cartesian Coordinate
      variance_xyz(j)= sqrt(dot_product(geometry_matrix(j,:), geometry_matrix(j,:))) * (error_gp)
      !call print("Predicted Error Factor :"//sum(abs(geometry_matrix(j,:)))//" Dimension :"//j)
   enddo

   call print("GP Uncertainty in Cartesian Coordinate: "//variance_xyz//" GP variance: "//error_gp//" variance Squre :"//error_gp_sq)
   pred_err(n_center_atom)=norm(variance_xyz)
   force_ptr(:,n_center_atom)=force
!
enddo  ! loop over positions

error_frame = maxval(pred_err) ! Cautious. could generate crazy numbers
call print("Error Bar of the Frame :"//error_frame)

deallocate(ivs_set_pred)
deallocate(ivs_set_norm_pred)
deallocate(geometry_matrix)
deallocate(r_grid, m_grid, species, sigma)
deallocate(force_proj_ivs_pred)
deallocate(ivs_set_norm)
deallocate(force_magntd_data, force_magntd_data_select)
deallocate(ivs_set)
deallocate(mark_zero_ivs)
deallocate(force_proj_ivs)
deallocate(force_proj_ivs_select)
deallocate(covariance, inv_covariance)
deallocate(covariance_pred)
deallocate(distance_index, distance_confs_unsorted)
if(allocated(force_covariance_matrix)) deallocate(force_covariance_matrix)
if(allocated(covariance_tiny)) deallocate(covariance_tiny)
if(allocated(distance_matrix)) deallocate(distance_matrix)
if (allocated(distance_matrix_dij)) deallocate(distance_matrix_dij)
!
fgp_calc=at_in  ! output the updated atomic object
!
end function fgp_calc ! return updated at_in
!
!subroutine update_database(at) ! update database based on all info in at
!!
!          type(Atoms)                              ::   at
!          real(dp), allocatable, dimension(:)      ::   r_grid, m_grid, force_proj_test
!          real(dp), allocatable, dimension(:,:,:)  ::   ivs_set_pred
!          character(STRING_LEN)                    ::		  data_dir
!          integer                                  ::		  i, j, k, add_vector, n, n_center_atom, n0
!          logical                                  ::		  write_data_dij
!          real(dp), dimension(:,:), pointer             ::  force_ptr
!          integer, dimension(:), allocatable            ::  species
!
!          call system_timer('Updating the DATABSE : grid.dat force.dat ivs.dat, exclud. dist.dat')
!          ! probably first load the grid file
!          call load_iv_params(data_dir, r_grid, m_grid, species, k, add_vector, n_data=n_0)
!          ! the current k is number of defined internal vectors
!          if (add_vector > 0)   k=k+add_vector
!          ! becomes the number of internal dimensions
!          allocate(force_proj_test(k))
!
!          do i=1, at%N
!
!          do j = 1, k
!               force_proj_test(j) = dot_product(force_ptr(:,n_center_atom), ivs_set_norm_pred(j,:) )
!          enddo
!
!          call  write_iv_params(data_dir, r_grid, m_grid, species, k-add_vector, add_vector, n+1)
!
!          !if (write_data_dij) call write_distance(distance_confs_unsorted, data_dir) ! append distance file
!          !write(*,*) 'Distance', distance_confs_unsorted(:)
!
!          OPEN(2,status='old',file=trim(data_dir)//'Force.dat', form='UNFORMATTED', position='APPEND')
!          OPEN(3,status='old',file=trim(data_dir)//'IV.dat', form='UNFORMATTED',  position='APPEND')
!          write(2)  force_proj_test(:), norm(force_ptr(:,n_center_atom))
!          do j=1, 3
!                write(3)  ivs_set_pred(:,j,n_center_atom)
!          enddo
!
!          enddo ! loop on positions
!          close(2)
!          close(3)
!          deallocate(force_proj_test)
!          call system_timer('Updating the DATABSE : dist.dat force.dat ivs.dat')
!!
!end subroutine update_database
!
subroutine optimisation_ivs_subset(data_dir, x_index)
   ! giving x_index in the order of decreasing weight
   !
   integer                                         ::  k, t, add_vector, n, i, j
   real(dp), dimension(:,:,:), allocatable         ::  ivs_set
   real(dp), dimension(:,:), allocatable           ::  force_proj_ivs
   real(dp), dimension(:), allocatable             ::  force_magntd_data,  uni_ave, norm_ave, r_grid, m_grid
   integer, dimension(:), allocatable              ::  species
   integer, dimension(:), allocatable, intent(out)             ::  x_index
   real(dp)                                        ::  max_corr
   type(Dictionary)                                ::  params
   character(STRING_LENGTH), intent(in)            ::  data_dir

   call   load_iv_params(data_dir, r_grid, m_grid, species, k, add_vector, n_data=n)

   k=k+add_vector  ! adding the Classical Force Vectors
   call print_title('Starting optimise the IVs directions')

   allocate(force_proj_ivs(k,n), ivs_set(k,3,n), force_magntd_data(n))
   allocate(uni_ave(k), norm_ave(k), x_index(k))

   OPEN(2,status='old',file=trim(data_dir)//'Force.dat',form='UNFORMATTED')
   OPEN(3,status='old',file=trim(data_dir)//'IV.dat',form='UNFORMATTED')

   do t=1, n
       read(2) force_proj_ivs(:, t), force_magntd_data(t)
       do i=1, 3
          read(3) ivs_set(:,i,t)
       enddo
   enddo
   close(2)
   close(3)

   ! output force_proj_ivs, force_magntd_data, norm of ivs
   do i=1, k
      uni_ave(i)  = -1.0_dp* abs(sum(force_proj_ivs(i,:)) / real(n, dp))
      ! universal average. to be confirmed
      norm_ave(i) = sum(abs(force_proj_ivs(i,:))) / real(n, dp)
      x_index(i) =i
   enddo
   !
   call heap_sort(uni_ave, i_data=x_index)
   ! what if reversed order is required.
   !
   deallocate(force_proj_ivs, ivs_set, force_magntd_data, uni_ave, norm_ave)
   call print_title('Starting optimise the IVs directions')
   !
end subroutine optimisation_ivs_subset
!
end module force_machine_learning_module


!> @file
!!  Define routne FFT 2D
!! @author
!!  Copyright (C) Stefan Goedecker, Lausanne, Switzerland, August 1, 1991
!!  Copyright (C) Stefan Goedecker, Cornell University, Ithaca, USA, 1994
!!  Copyright (C) Stefan Goedecker, MPI Stuttgart, Germany, 1999
!!  Copyright (C) Stefan Goedecker, CEA Grenoble, 2002
!!  Copyright (C) 2002-2009 BigDFT group
!!  This file is distributed under the terms of the
!!  GNU General Public License, see ~/COPYING file
!!  or http://www.gnu.org/copyleft/gpl.txt .
!!  For the list of contributors, see ~/AUTHORS

!> CALCULATES THE DISCRETE FOURIER TRANSFORM F(I1,I2)=
!!    S_(j1,j2) EXP(isign*i*2*pi*(j1*i1/n1+j2*i2/n2)) R(j1,j2)
!!    INPUT:
!!        n1,n2:physical dimension of the transform. It must be a
!!          product of the prime factors 2,3,5, but greater than 3.
!!                 If two ni's are equal it is recommended to place them
!!                 behind each other.
!!        nd1,nd2:memory dimension of Z. ndi must always be greater or
!!            equal than ni. It is recomended to chose ndi=ni
!!            if ni is odd and ndi=ni+1 if ni is even to obtain
!!                   optimal execution speed. For small ni it can however
!!                   sometimes be advantageous to set ndi=ni
!!           inzee=1: first part of Z is data array, second part work array
!!           inzee=2: first part of Z is work array, second part data array
!!        Z(1,i1,i2,inzee)=real(R(i1,i2))
!!        Z(2,i1,i2,inzee)=imag(R(i1,i2))
!!    OUTPUT:
!!           inzee=1: first part of Z is data array, second part work array
!!           inzee=2: first part of Z is work array, second part data array
!!        real(F(i1,i2))=Z(1,i1,i2,inzee)
!!        imag(F(i1,i2))=Z(2,i1,i2,inzee)
!!            inzee on output is in general different from inzee on input
!!    THE ARRAYELEMENTS Z( , , , ,3-inzee) ARE USED AS WORKING SPACE
!!       On a RISC machine with cache, it is very important to find the optimal
!!       value of NCACHE. NCACHE determines the size of the work array zw, that
!!       has to fit into cache. It has therefore to be chosen to equal roughly
!!    half the size of the physical cache in units of real*8 numbers.
!!       The optimal value of ncache can easily be determined by numerical
!!       experimentation. A too large value of ncache leads to a dramatic
!!       and sudden decrease of performance, a too small value to a to a
!!       slow and less dramatic decrease of performance. If NCACHE is set
!!       to a value so small, that not even a single one dimensional transform
!!       can be done in the workarray zw, the program stops with an error message.
!!    On a vector machine ncache has to be put to 0
!!
subroutine FFT2d(n1,n2,nd1,nd2,z,isign,inzee,zw,ncache)

   implicit none
   !Arguments
   integer, intent(in) :: n1,n2,nd1,nd2,isign,ncache
   integer, intent(inout) :: inzee
   integer, dimension(20) :: after,before,now
   real(kind=8), dimension(2,nd1*nd2,2) :: z
   real(kind=8), dimension(2,ncache/4,2) :: zw
   !Local variables
   integer, parameter :: nmax = 1024
   real(kind=8), dimension(2,nmax) :: trig
   integer :: ntrig,nn,nfft,n,mm,mb,ma,j,m,lot,jp,jj,jb,ja,inzeep
   integer :: ic,i

   if (max(n1,n2).gt.nmax) stop '1024'
   ntrig=nmax
   ! vector computer with memory banks:
   if (ncache.eq.0) then

      ! TRANSFORM ALONG Y AXIS
      call ctrig_sg(n2,ntrig,trig,after,before,now,isign,ic)
      nfft=n1
      mm=nd1
      do i=1,ic-1
         call fftstp_sg(mm,nfft,nd2,mm,nd2,z(1,1,inzee),z(1,1,3-inzee),ntrig,trig,after(i),now(i),before(i),isign)
         inzee=3-inzee
      end do
      i=ic
      call fftrot_sg(mm,nfft,nd2,mm,nd2,z(1,1,inzee),z(1,1,3-inzee),ntrig,trig,after(i),now(i),before(i),isign)
      inzee=3-inzee

      ! TRANSFORM ALONG X AXIS
      if (n1.ne.n2) call ctrig_sg(n1,ntrig,trig,after,before,now,isign,ic)
      nfft=n2
      mm=nd2
      do i=1,ic-1
         call fftstp_sg(mm,nfft,nd1,mm,nd1,z(1,1,inzee),z(1,1,3-inzee),ntrig,trig,after(i),now(i),before(i),isign)
         inzee=3-inzee
      end do
      i=ic
      call fftrot_sg(mm,nfft,nd1,mm,nd1,z(1,1,inzee),z(1,1,3-inzee),ntrig,trig,after(i),now(i),before(i),isign)
      inzee=3-inzee

      ! RISC machine with cache:
   else

      ! TRANSFORM ALONG Y AXIS
      mm=nd1
      m=nd2
      lot=max(1,ncache/(4*n2))
      !    print*,'lot',lot
      nn=lot
      n=n2
      if (2*n*lot*2.gt.ncache) stop 'enlarge ncache :2'
      call ctrig_sg(n2,ntrig,trig,after,before,now,isign,ic)

      ja=1
      jb=n1

      if (ic.eq.1) then
         i=ic
         jj=ja*nd2-nd2+1
         nfft=jb-ja+1
         call fftrot_sg(mm,nfft,m,mm,m,z(1,ja,inzee),z(1,jj,3-inzee),ntrig,trig,after(i),now(i),before(i),isign)

      else

         do j=ja,jb,lot
            ma=j
            mb=min(j+(lot-1),jb)
            nfft=mb-ma+1
            jj=j*nd2-nd2+1

            i=1
            inzeep=2
            call fftstp_sg(mm,nfft,m,nn,n,z(1,j,inzee),zw(1,1,3-inzeep),ntrig,trig,after(i),now(i),before(i),isign)
            inzeep=1

            do i=2,ic-1
               call fftstp_sg(nn,nfft,n,nn,n,zw(1,1,inzeep),zw(1,1,3-inzeep),ntrig,trig,after(i),now(i),before(i),isign)
               inzeep=3-inzeep
            end do

            i=ic
            call fftrot_sg(nn,nfft,n,mm,m,zw(1,1,inzeep),z(1,jj,3-inzee),ntrig,trig,after(i),now(i),before(i),isign)
         end do
      endif
      inzee=3-inzee

      ! TRANSFORM ALONG X AXIS
      mm=nd2
      m=nd1
      lot=max(1,ncache/(4*n1))
      !    print*,'lot',lot
      nn=lot
      n=n1
      if (2*n*lot*2.gt.ncache) stop 'enlarge ncache :1'
      if (n1.ne.n2) call ctrig_sg(n1,ntrig,trig,after,before,now,isign,ic)

      if (ic.eq.1) then
         i=ic
         j=1
         jp=nd2
         jj=j*nd1-nd1+1
         nfft=jp-j+1
         call fftrot_sg(mm,nfft,m,mm,m,z(1,j,inzee),z(1,jj,3-inzee),ntrig,trig,after(i),now(i),before(i),isign)

      else

         do j=1,n2,lot
            ma=j
            mb=min(j+(lot-1),n2)
            nfft=mb-ma+1
            jj=j*nd1-nd1+1

            i=1
            inzeep=2
            call fftstp_sg(mm,nfft,m,nn,n,z(1,j,inzee),zw(1,1,3-inzeep),ntrig,trig,after(i),now(i),before(i),isign)
            inzeep=1

            do i=2,ic-1
               call fftstp_sg(nn,nfft,n,nn,n,zw(1,1,inzeep),zw(1,1,3-inzeep),ntrig,trig,after(i),now(i),before(i),isign)
               inzeep=3-inzeep
            end do
            i=ic
            call fftrot_sg(nn,nfft,n,mm,m,zw(1,1,inzeep),z(1,jj,3-inzee),ntrig,trig,after(i),now(i),before(i),isign)
         end do
      endif
      inzee=3-inzee

   endif

END SUBROUTINE fft2d

!!$!>two dimensional full convolution with a kernel given in the reciprocal space
!!$subroutine 2D_fullconv
!!$!$omp do schedule(static)
!!$do j3 = 1, maxIter
!!$   !this condition ensures that we manage only the interesting part for the FFT
!!$   !if (iproc*(nd3/nproc)+j3 <= n3/2+1) then
!!$   Jp2stb=1
!!$   J2stb=1
!!$   Jp2stf=1
!!$   J2stf=1
!!$   ! transform along x axis
!!$   lot=ncache/(4*n1)
!!$   if (lot < 1)  stop 'n1'
!!$
!!$   do j=1,n2dimp/n3pr1,lot
!!$      nfft=min(j+(lot-1), n2dimp/n3pr1) -j +1
!!$
!!$      !input: I1,J2,j3,Jp2,(jp3)
!!$      if (nproc > 1) then
!!$         call fft_parallel_block(n1,md2/(n3pr1*n3pr2),n3pr2,&
!!$              n1dim,md2/(n3pr1*n3pr2),nd3/n3pr2,&
!!$              nfft,lot,lzt/n3pr1,J2stb,Jp2stb,j3,&
!!$              ntrig,btrig1,after1,now1,before1,ic1,1,&
!!$              zmpi1,zw(1,1,1,ithread),zt(1,j,1,ithread))
!!$      else
!!$         call fft_parallel_block(n1,md2,1,&
!!$              n1dim,md2,nd3,&
!!$              nfft,lot,lzt/n3pr1,J2stb,Jp2stb,j3,&
!!$              ntrig,btrig1,after1,now1,before1,ic1,1,&
!!$              zmpi2,zw(1,1,1,ithread),zt(1,j,1,ithread))
!!$      end if
!!$      !output: I2,i1,j3,(jp3)
!!$   end do
!!$
!!$   !transform along y axis
!!$   lot=ncache/(4*n2)
!!$   if (lot < 1) stop 'n2'
!!$
!!$   do j=1,n1p/n3pr1,lot
!!$      i_one=1
!!$      nfft=min(j+(lot-1),n1p/n3pr1)-j+1
!!$      !reverse ordering
!!$      !input: I2,i1,j3,(jp3)
!!$      call fft_parallel_block(n2,n1,1,&
!!$           n2dim,n1,1,&
!!$           nfft,lot,lot,one,one,1,&
!!$           ntrig,btrig2,after2,now2,before2,ic2,1,&
!!$           zt(1,1,j,ithread),zw(1,1,1,ithread),zw(1,1,1,ithread),inzee)
!!$      we should insert inzee and eliminate zout in this case
!!$      !output: i1,i2,j3,(jp3)
!!$      !Multiply with kernel in fourier space
!!$      i3=mod(iproc,n3pr2)*(nd3/n3pr2)+j3
!!$
!!$      j1start=0
!!$      if (n3pr1>1) j1start=(n1p/n3pr1)*iproc_inplane
!!$
!!$      if (geocode == 'P') then
!!$         call P_multkernel(nd1,nd2,n1,n2,n3,lot,nfft,j+j1start,pot(1,1,j3),zw(1,1,inzee,ithread),&
!!$              i3,hx,hy,hz,offset,scal,strten_omp)
!!$      else
!!$         !write(*,*) 'pot(1,1,j3) = ', pot(1,1,j3)
!!$         call multkernel(nd1,nd2,n1,n2,lot,nfft,j+j1start,pot(1,1,j3),zw(1,1,inzee,ithread))
!!$      end if
!!$
!!$      !TRANSFORM BACK IN REAL SPACE
!!$      !transform along y axis
!!$      !input: i1,i2,j3,(jp3)
!!$      do i=1,ic2
!!$         call fftstp_sg(lot,nfft,n2,lot,n2,zw(1,1,inzee,ithread),zw(1,1,3-inzee,ithread),&
!!$              ntrig,ftrig2,after2(i),now2(i),before2(i),-1)
!!$         !zw(:,:,3-inzee)=zw(:,:,inzee)
!!$         inzee=3-inzee
!!$      end do
!!$
!!$      !reverse ordering
!!$      !input: i1,I2,j3,(jp3)
!!$      if (n3pr1 == 1) then
!!$         call G_unswitch_downcorn(nfft,n2,n2dim,lot,n1,lzt, &
!!$              zw(1,1,inzee,ithread),zt(1,1,j,ithread))
!!$      else
!!$         call G_unswitch_downcorn2(nfft,n2,n2dim,lot,n1p,lzt, &
!!$              zw(1,1,inzee,ithread),zt_t(1,1,1),n3pr1,j)
!!$      endif
!!$      !output: I2,i1,j3,(jp3)
!!$   end do
!!$   !transform along x axis
!!$   !input: I2,i1,j3,(jp3)
!!$
!!$   !LG: this MPI_ALLTOALL is inside a loop. I think that it will rapidly become unoptimal
!!$   if (n3pr1 > 1 .and. inplane_comm/=MPI_COMM_NULL) then
!!$      call f_timing(TCAT_PSOLV_COMPUT,'OF')
!!$      call f_timing(TCAT_PSOLV_COMMUN,'ON')
!!$
!!$      call MPI_ALLTOALL(zt_t,2*(n1p/n3pr1)*(lzt/n3pr1),MPI_double_precision,&
!!$           zt(1,1,1,ithread),2*(n1p/n3pr1)*(lzt/n3pr1), &
!!$           MPI_double_precision,inplane_comm,ierr)
!!$
!!$      call f_timing(TCAT_PSOLV_COMMUN,'OF')
!!$      call f_timing(TCAT_PSOLV_COMPUT,'ON')
!!$   endif
!!$
!!$   lot=ncache/(4*n1)
!!$   do j=1,n2dimp/n3pr1,lot
!!$      nfft=min(j+(lot-1),n2dimp/n3pr1)-j+1
!!$
!!$      !performing FFT
!!$      i=1
!!$      if (n3pr1 > 1) then
!!$         call fftstp_sg(lzt/n3pr1,nfft,n1,lot,n1,zt(1,j,1,ithread),zw(1,1,1,ithread),&
!!$              ntrig,ftrig1,after1(i),now1(i),before1(i),-1)
!!$      else
!!$         call fftstp_sg(lzt,nfft,n1,lot,n1,zt(1,j,1,ithread),zw(1,1,1,ithread),&
!!$              ntrig,ftrig1,after1(i),now1(i),before1(i),-1)
!!$      endif
!!$      inzee=1
!!$      do i=2,ic1
!!$         call fftstp_sg(lot,nfft,n1,lot,n1,zw(1,1,inzee,ithread),zw(1,1,3-inzee,ithread),&
!!$              ntrig,ftrig1,after1(i),now1(i),before1(i),-1)
!!$         inzee=3-inzee
!!$      enddo
!!$
!!$      !output: I2,I1,j3,(jp3)
!!$      !reverse ordering
!!$      !input: J2,Jp2,I1,j3,(jp3)
!!$      if (nproc == 1) then
!!$         call G_unmpiswitch_downcorn(j3,nfft,Jp2stf,J2stf,lot,n1,&
!!$              n1dim,md2,nd3,nproc,zw(1,1,inzee,ithread),zmpi2)
!!$      else
!!$         call G_unmpiswitch_downcorn2(j3,nfft,Jp2stf,J2stf,lot,n1,&
!!$              n1dim,md2,nd3,n3pr1,n3pr2,zw(1,1,inzee,ithread),zmpi1)
!!$      endif
!!$      ! output: I1,J2,j3,Jp2,(jp3)
!!$   end do
!!$   !endif
!!$
!!$   !END OF TRANSFORM FOR X AND Z
!!$
!!$end do
!!$!$omp end do

subroutine fill_second_part(i1s,ld1,n1,npad,zw,zin)
  use f_precisions, only: dp => f_double
  implicit none
  integer, intent(in) :: ld1,n1,i1s,npad
  real(dp),intent(inout) ::  zw(2,ld1,*)
  real(dp), intent(in) :: zin(2,*)
  !local variables
  integer :: I

  do I=1,n1
     zw(1,i1s,I+npad)=zin(1,I)
     zw(2,i1s,I+npad)=zin(2,I)
  end do

end subroutine fill_second_part

subroutine get_first_part(i1s,ld1,n1,zw,zout)
  use f_precisions, only: dp => f_double
  implicit none
  integer, intent(in) :: ld1,n1,i1s
  real(dp),intent(in) ::  zw(2,ld1,*)
  real(dp), intent(inout) :: zout(2,*)
  !local variables
  integer :: i

  do i=1,n1
     zout(1,i)=zw(1,i1s,i)
     zout(2,i)=zw(2,i1s,i)
  end do

end subroutine get_first_part

subroutine transpose_output(ndims,lds,n1dim,lot,nfft,j3,&
     i2s,i2ps,zw,zout)
  use f_precisions, only: dp => f_double
  implicit none
  integer, intent(in) :: n1dim,lot,nfft,j3
  integer, dimension(3), intent(in) :: ndims,lds
  real(dp), dimension(*), intent(in) ::  zw
  real(dp), dimension(2,lds(1),lds(2),lds(3),*), intent(inout) ::  zout
  integer, intent(inout) :: i2s,i2ps
  !local variables
  integer :: j2,mfft,jp2,n1,n2,n2p

  n1=ndims(1)
  n2=ndims(2)
  n2p=ndims(3)

  mfft=0
  do Jp2=i2ps,n2p
     do J2=i2s,n2
        mfft=mfft+1
        if (mfft > nfft) then
           i2ps=Jp2
           i2s=J2
           return
        end if
        call get_first_part(mfft,lot,n1dim,zw,zout(1,1,J2,j3,Jp2))
     end do
     i2s=1
  end do

end subroutine transpose_output

subroutine transpose_and_pad_input(ndims,lds,n1dim,lot,nfft,j3,&
     i2s,i2ps,zin,zw)
  use f_precisions, only: dp => f_double
  implicit none
  integer, intent(in) :: n1dim,lot,nfft,j3
  integer, dimension(3), intent(in) :: ndims,lds
  real(dp), dimension(2,lds(1),lds(2),lds(3),*), intent(in) ::  zin
  integer, intent(inout) :: i2s,i2ps
  real(dp), dimension(2,lot,*), intent(inout) ::  zw
  !local variables
  integer :: j2,mfft,jp2,ish,i1,imfft,n1,n2,n2p

  n1=ndims(1)
  n2=ndims(2)
  n2p=ndims(3)

  ish=n1-n1dim
  mfft=0
  loop_Jp2: do Jp2=i2ps,n2p
     do J2=i2s,n2
        mfft=mfft+1
        if (mfft > nfft) then
           i2ps=Jp2
           i2s=J2
           exit loop_Jp2
        end if
        call fill_second_part(mfft,lot,n1dim,ish,&
             zw,zin(1,1,J2,j3,Jp2))
     end do
     i2s=1
  end do loop_Jp2

  !then zero the rest of the cache array if the padding is required
  do i1=1,ish
     do imfft=1,mfft-1
        zw(1,imfft,i1)=0.0_dp
        zw(2,imfft,i1)=0.0_dp
     end do
  end do

end subroutine transpose_and_pad_input

!!$subroutine fft_1d_cache(ndat_in,ld_in,ndat_out,ld_out,nfft,ninout,n,&
!!$     ncache,ntrig,trig,after,now,before,ic,&
!!$     i_sign,inzee,transpose,iam,nthread,z,zw)
!!$  use f_precisions
!!$  use module_fft_sg, only: n_factors
!!$  implicit none
!!$  logical, intent(in) :: transpose
!!$  integer, intent(in) :: ndat_in,ld_in,ndat_out,ld_out,nfft,ninout,n
!!$  integer, intent(in) :: ncache,ntrig,ic
!!$  integer, intent(in) :: i_sign,iam,nthread
!!$  integer, intent(inout) :: inzee
!!$  integer, dimension(n_factors) :: after,now,before
!!$  real(f_double), dimension(2,ntrig), intent(in) :: trig
!!$  real(f_double), dimension(2,ninout,2), intent(inout) :: z
!!$  real(f_double), dimension(2,ncache/4,2), intent(inout) :: zw
!!$  !local variables
!!$  logical :: real_input = .false.
!!$  integer :: i,lotomp,ma,mb,nfft_th,j,jj,jompa,jompb,inzeep,inzet,lot,nn
!!$
!!$  !set of fft to be treated by the present thread
!!$  lotomp=nfft/nthread+1
!!$  jompa=iam*lotomp+1
!!$  jompb=min((iam+1)*lotomp,nfft)
!!$
!!$  !here we might put the treatment for the input and the output
!!$  !in case they are necessary
!!$
!!$  inzet=inzee
!!$  if (ic == 1 .or. ncache == 0) then
!!$     i=ic
!!$     ma=jompa
!!$     mb=jompb
!!$     nfft_th=mb-ma+1
!!$     j=ma
!!$     if (transpose) then
!!$        jj=j*ld_out-ld_out+1
!!$        !input z(2,ndat_in,ld_in,inzet)
!!$        !output z(2,ld_out,ndat_out,3-inzet)
!!$        call fftrot_sg(ndat_in,nfft_th,ld_in,ndat_out,ld_out,&
!!$             z(1,j,inzet),z(1,jj,3-inzet), &
!!$             ntrig,trig,after(i),now(i),before(i),i_sign)
!!$        if (real_input) then !only works when ld_out==n
!!$           inzet=3-inzet
!!$           call unpack_rfft(ndat_out,n,&
!!$                z(1,jj,inzet),z(1,jj,3-inzet))
!!$        end if
!!$     else
!!$        !input z(2,ndat_in,ld_in,inzet)
!!$        !output z(2,ndat_out,ld_out,3-inzet)
!!$        call fftstp_sg(ndat_in,nfft_th,ld_in,ndat_out,ld_out,&
!!$             z(1,j,inzet),z(1,j,3-inzet), &
!!$             ntrig,trig,after(i),now(i),before(i),i_sign)
!!$        if (real_input) then !only works when ld_out==n
!!$           inzet=3-inzet
!!$           !here maybe the ndat_out has to be rethought
!!$           call unpack_rfft_t((ndat_out-1)/2+1,ndat_out,n,&
!!$                z(1,j,inzet),z(1,j,3-inzet))
!!$        end if
!!$     end if
!!$  else
!!$     lot=max(1,ncache/(4*n))
!!$     nn=lot
!!$     do j=jompa,jompb,lot
!!$        ma=j
!!$        mb=min(j+(lot-1),jompb)
!!$        nfft_th=mb-ma+1
!!$        i=1
!!$        inzeep=2
!!$        call fftstp_sg(ndat_in,nfft_th,ld_in,nn,n,&
!!$             z(1,j,inzet),zw(1,1,3-inzeep), &
!!$             ntrig,trig,after(i),now(i),before(i),i_sign)
!!$        inzeep=1
!!$        do i=2,ic-1
!!$           call fftstp_sg(nn,nfft_th,n,nn,n,&
!!$                zw(1,1,inzeep),zw(1,1,3-inzeep), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           inzeep=3-inzeep
!!$        end do
!!$        i=ic
!!$        if (transpose) then
!!$           jj=j*ld_out-ld_out+1
!!$           call fftrot_sg(nn,nfft_th,n,ndat_out,ld_out,&
!!$                zw(1,1,inzeep),z(1,jj,3-inzet), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           if (real_input) then !only works when ld_out==n
!!$              inzet=3-inzet
!!$              call unpack_rfft(ndat_out,n,&
!!$                   z(1,jj,inzet),z(1,jj,3-inzet))
!!$           end if
!!$        else
!!$           call fftstp_sg(nn,nfft_th,n,ndat_out,ld_out,&
!!$                zw(1,1,inzeep),z(1,j,3-inzet), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           if (real_input) then !only works when ld_out==n
!!$              inzet=3-inzet
!!$              call unpack_rfft_t((ndat_out-1)/2+1,ndat_out,n,&
!!$                   z(1,j,inzet),z(1,j,3-inzet))
!!$           end if
!!$        end if
!!$     end do
!!$  end if
!!$  inzet=3-inzet
!!$  if (iam==0) inzee=inzet !as it is a shared variable
!!$
!!$end subroutine fft_1d_base
!!$
!!$!> base wrapper for FFT, not to be called in public routines
!!$!! @warning: for transa='t' nwork must be > 0
!!$subroutine fft_1d_base(transa,transb,ndat_in,ld_in,ndat_out,ld_out,ninout,nfft,n,&
!!$     ntrig,trig,after,now,before,ic,i_sign,&
!!$     input_provided,za,&
!!$     output_provided,zb,&
!!$     inout_provided,zab,inzee,&
!!$     nwork,zw,iam,nthread)
!!$  use f_precisions
!!$  use module_fft_sg, only: n_factors
!!$  implicit none
!!$  integer, intent(in) :: ndat_in,ld_in,ndat_out,ld_out,nfft,ninout,n
!!$  integer, intent(in) :: nwork,ntrig,ic
!!$  integer, intent(in) :: i_sign,iam,nthread
!!$  character(len=1), intent(in) :: transa,transb
!!$  integer, intent(inout) :: inzee
!!$  integer, dimension(n_factors) :: after,now,before
!!$  real(f_double), dimension(2,ntrig), intent(in) :: trig
!!$  real(f_double), dimension(2,ndat_in*ld_in), intent(in) :: za
!!$  real(f_double), dimension(2,ndata_out*ld_out), intent(inout) :: zb
!!$  real(f_double), dimension(2,ninout,2), intent(inout) :: zab
!!$  real(f_double), dimension(2,nwork/4,2), intent(inout) :: zw
!!$  !local variables
!!$  logical :: real_input = .false.,transpose_output,transpose_input,nowork
!!$  integer :: i,lotomp,ma,mb,nfft_th,j,jj,jompa,jompb,inzeep,inzet,lot,nn
!!$
!!$  !set of fft to be treated by the present thread
!!$  transpose_input=transa=='T' .or. transa=='t'
!!$  transpose_output=transb=='T' .or. transb=='t'
!!$
!!$  !check on arguments
!!$  call f_assert(input_provided .and. (output_provided .or. nwork>0),id='Error on input work')
!!$  call f_assert(transpose_input .and. input_provided .and. nwork >0,id='Transposition needs workarray')
!!$
!!$
!!$  lotomp=nfft/nthread+1
!!$  jompa=iam*lotomp+1
!!$  jompb=min((iam+1)*lotomp,nfft)
!!$  nowork= ic == 1 .or. nwork == 0 !work array cannot be used for only one step
!!$  if (nowork) then
!!$     lot=jompb-jompa
!!$  else
!!$     lot=max(1,nwork/(4*n))
!!$  end if
!!$
!!$  !here we might put the treatment for the input and the output
!!$  !in case they are necessary
!!$
!!$  inzet=inzee
!!$  loop_on_lines: do j=jompa,jompb,lot !this loop is internal to the threads, it might be further parallelised
!!$     ma=j
!!$     mb=min(j+(lot-1),jompb)
!!$     nfft_th=mb-ma+1
!!$
!!$     !then it follows the treatments for the different cases
!!$     if (nowork) then
!!$        if (
!!$        !we need that z is large enough to contain max(ndat_in*ld_in,ndat_out*ld_out)
!!$        do i=1,ic-1
!!$           call fftstp_sg(ndat_in,nfft_th,ld_in,ndat_out,ld_out,&
!!$                zab(1,j,inzet),zab(1,j,3-inzet), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           inzet=3-inzet
!!$        end do
!!$        i=ic
!!$        if (transpose_output) then
!!$           jj=j*ld_out-ld_out+1
!!$           !input z(2,ndat_in,ld_in,inzet)
!!$           !output z(2,ld_out,ndat_out,3-inzet)
!!$           call fftrot_sg(ndat_in,nfft_th,ld_in,ndat_out,ld_out,&
!!$                zab(1,j,inzet),zab(1,jj,3-inzet), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           if (real_input) then !only works when ld_out==n
!!$              inzet=3-inzet
!!$              call unpack_rfft(ndat_out,n,&
!!$                   z(1,jj,inzet),z(1,jj,3-inzet))
!!$           end if
!!$        else
!!$           !input z(2,ndat_in,ld_in,inzet)
!!$           !output z(2,ndat_out,ld_out,3-inzet)
!!$           call fftstp_sg(ndat_in,nfft_th,ld_in,ndat_out,ld_out,&
!!$                zab(1,j,inzet),zab(1,j,3-inzet), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           if (real_input) then !only works when ld_out==n
!!$              inzet=3-inzet
!!$              !here maybe the ndat_out has to be rethought
!!$              call unpack_rfft_t((ndat_out-1)/2+1,ndat_out,n,&
!!$                   z(1,j,inzet),z(1,j,3-inzet))
!!$           end if
!!$        end if
!!$     else
!!$        nn=lot
!!$        i=1
!!$        inzeep=2
!!$        if (input_provided) then
!!$           call fftstp_sg(ndat_in,nfft_th,ld_in,nn,n,&
!!$                za,zw(1,1,3-inzeep), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$        else if (inout_provided) then
!!$           call fftstp_sg(ndat_in,nfft_th,ld_in,nn,n,&
!!$                zab(1,j,inzet),zw(1,1,3-inzeep), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$        end if
!!$        inzeep=1
!!$        do i=2,ic-1
!!$           call fftstp_sg(nn,nfft_th,n,nn,n,&
!!$                zw(1,1,inzeep),zw(1,1,3-inzeep), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           inzeep=3-inzeep
!!$        end do
!!$        i=ic
!!$        if (transpose_output) then
!!$           jj=j*ld_out-ld_out+1
!!$           call fftrot_sg(nn,nfft_th,n,ndat_out,ld_out,&
!!$                zw(1,1,inzeep),zab(1,jj,3-inzet), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           if (real_input) then !only works when ld_out==n
!!$              inzet=3-inzet
!!$              call unpack_rfft(ndat_out,n,&
!!$                   z(1,jj,inzet),z(1,jj,3-inzet))
!!$           end if
!!$        else
!!$           call fftstp_sg(nn,nfft_th,n,ndat_out,ld_out,&
!!$                zw(1,1,inzeep),zab(1,j,3-inzet), &
!!$                ntrig,trig,after(i),now(i),before(i),i_sign)
!!$           if (real_input) then !only works when ld_out==n
!!$              inzet=3-inzet
!!$              call unpack_rfft_t((ndat_out-1)/2+1,ndat_out,n,&
!!$                   z(1,j,inzet),z(1,j,3-inzet))
!!$           end if
!!$        end if
!!$     end if
!!$  end do loop_on_lines
!!$  inzet=3-inzet
!!$  if (iam==0) inzee=inzet !as it is a shared variable
!!$
!!$end subroutine fft_1d_base


      subroutine Iplus_calc(ntau,tau,source,Iplus,optthin)
*
* Calculates Iplus (outwards directed intensities) along
* a ray at all depths in a model atmosphere, by integration of
* the source function. Input: tau is the optical depth along
* the ray, source is the source function ,
* both provided at ntau depth points.  BPz 19/07-2001
* Revised by BPz 27/06-2012:
* Revised by BPz on 09/04-2020 following test comparison with analytical solutions
* (S=polynomial(tau))
*
* Numbering of layers goes from 1 at the observer inwards.
* The tau-scale increases away from the observer.
*
* Opthin means that the ray goes throughthe atmosphere.
* THIS VERSION ASSUMES SPHERICAL SYMMETRY
*
      implicit none
      logical optthin
      integer i,ntau
      real Iplus(*),source(*),tau(*),ex(ntau+1),mex,dtau

      do i=2,ntau
!          ex(i)=exp(tau(i-1)-tau(i))    ! dtau is >0, we need exp(-dtau)
        dtau=tau(i)-tau(i-1)
        if (dtau.lt.0.01) then
          mex=dtau*(1.-dtau/2.*(1.-dtau/3.*(1.-dtau/4.)))
          ex(i)=1.-mex
        else
          ex(i)=exp(-dtau)
        endif
      enddo
      if (optthin) then
* optically thin: assumes no incoming radiation above taumax
* BPlez 26/03-2020 replaces this by calcultaing intensity generated between tau=0 and first depth point.
!        Iplus(ntau)=0.
!        Iplus(ntau)=source(ntau)*(1.-exp(-tau(ntau)))
        mex=tau(1)*(1.-tau(1)/2.*(1.-tau(1)/3.*(1.-tau(1)/4.)))
        if (tau(1).gt.0.01) mex=1.-exp(-tau(1))
        Iplus(ntau)=source(ntau)*mex             ! CHECK THIS ! should compute a tau above the last point in the back
      else
* optically thick: diffusion approximation
        Iplus(ntau)=source(ntau)+
     &     (source(ntau)-source(ntau-1))/(tau(ntau)-tau(ntau-1))
      endif
      do i=ntau-1,1,-1
!        Iplus(i)=Iplus(i+1)*ex(i+1)+
!     &           (source(i+1)+source(i))*0.5*
!     &                  (1.-ex(i+1))
! adopt Trapezoidal rule within each dtau interval (i.e. assumes Sexp(-tau) is affine function
         Iplus(i)=Iplus(i+1)*ex(i+1)+
     &            (source(i+1)*ex(i+1)+source(i))*0.5*(tau(i+1)-tau(i))
      enddo

cc! Iplus(1) corrected for emission/absorption from layer between
cc! tau=tau(1) and tau=0.

cc      Iplus(1)=Iplus(1)*exp(-tau(1))+
cc     &         source(1)*(1.-exp(-tau(1)))

! BPlez: 25/03-2020. Iplus(1) is intensity at tau(1).
! It must be corrected to tau=0 to get intensity at observer.

      return
      end


c
c Last update: Jul 07, 2007
c
************************************************************************
* Subroutine to compute the jacobian corresponding to the phase space
*  mapping:
*          [1t]+[1t]+[4(rm1[a,b]) --> 3(rm11[a,b])+p4
*                                      --> 2(rm111[a,b])+p3+p4]
* where rm1[a,b],rm11[a,b],rm111[a,b] are the masses of the intermediate
*  resonances.
* rm1, rm11 and rm111 can resonate at two different masses (e.g. Z and
*  H) with rm1a < rm1b, rm11a < rm11b and rm111a < rm111b.
* Two exponents (expa,2) are available for mapping the two t-channel
*  propagators.
* rmta, rmtb are the corresponding t-channel masses.
* These mapping are applied for 1.d0>cos(theta)>cos_sep. cos_sep is a
*  PARAMETER
* Outside this range the cosine is generated flat.
* xmasq,xmbsq are the masses squared of the two incoming particles
*     (a --> 5, b --> 6 in the t's)
* pa=sqrt(shat)/2*(1,0,0,1) pb=sqrt(shat)/2*(1,0,0,-1) in the massless
*   limit.
*
* INPUT:
*  shat          : total energy squared
*  xm(8),xmsq(8) : masses and masses squared of the particles
*                        in the order used in the main program
*  rmN,gamN      : N=1,11,111 mass and width for intermediate
*                   resonance N
*  spmNlo,spmNhi : N=111,11,1 low and high limit of resonance mapping.
*                        outside this range the mapping is flat
*  pt5min_in(3)  : array of possible minimum pt's of particle 5,
*                  depending on its flavour. The first element refers
*                  to jets, the second one to charged leptons and the
*                  third one to neutrinos.
*  pt6min_in(3)  : array of possible minimum pt's of particle 6,
*                  depending on its flavour. The first element refers
*                  to jets, the second one to charged leptons and the
*                  third one to neutrinos.
*  id5           : idp(iorder(7)) used to determine which minimum pt
*                  must be considered in the t-channel.
*  id6           : idp(iorder(8)) used to determine which minimum pt
*                  must be considered in the t-channel.
*  iorder(6)     : specifies the permutation that relates internal and
*                        external momenta: p = p(i,iorder(1))
*  xmasq,xmbsq   : masses squared of the two incoming particles
*                        (a --> 5, b --> 6 in the t's)
*  p(0:3,8)      : momenta in the hard scattering CM.
*                        Includes initial momenta.
* OUTPUT:
*  xjac          : full jacobian
************************************************************************

        subroutine phsp1_1_31_multi_cjac(shat,
     &                    rm1a,gam1a,spm1alo,spm1ahi,
     &                    rm1b,gam1b,spm1blo,spm1bhi,
     &                    rm11a,gam11a,spm11alo,spm11ahi,
     &                    rm11b,gam11b,spm11blo,spm11bhi,
     &                    rm111a,gam111a,spm111alo,spm111ahi,
     &                    rm111b,gam111b,spm111blo,spm111bhi,
     &                    rmta,expa,spta,rmtb,expb,sptb,
c giuseppe 07/07/2007
c     &                    pt5min,pt6min,
     &                    pt5min_in,pt6min_in,
     &                    id5,id6,
c end giuseppe 07/07/2007
     &                    xmasq,xmbsq,
     &                    xm,xmsq,iorder,
     &                    p,xjac)

      IMPLICIT NONE
      REAL*8 shat,
     &       rm1a,gam1a,spm1alo,spm1ahi,
     &       rm1b,gam1b,spm1blo,spm1bhi,
     &       rm11a,gam11a,spm11alo,spm11ahi,
     &       rm11b,gam11b,spm11blo,spm11bhi,
     &       rm111a,gam111a,spm111alo,spm111ahi,
     &       rm111b,gam111b,spm111blo,spm111bhi,
     &       rmta,expa,rmtb,expb,
c giuseppe 07/07/2007
     &       pt5min_in(3),pt6min_in(3),
c end giuseppe 07/07/2007
     &       pt5min,pt6min,
     &       xmasq,xmbsq,
     &       xm(8),xmsq(8),
     &       p(0:3,8),xjac
c giuseppe 07/07/2007
      INTEGER iorder(6),id5,id6
c end giuseppe 07/07/2007

* Local variables
      REAL*8 e_sup,e_inf,x_max,x_min,app,
     &       s111,s11,s1,pmod_15,
     &       e111,e11,e1,p111mod,p11mod,p1mod,pmod,
     &       saux,eaux,t_inf,t_sup,t1,t2,pauxmod,pbauxmod,p0mod,
     &       t_inf0,t_sup0,
     &       pi,twopi,fourpi,
     &       rmtasq,rmtbsq,t1aux

      INTEGER io1,io2,io3,io4,io5,io6,i

      PARAMETER (pi=3.141592653589793238462643d0,twopi=2.d0*pi,
     &           fourpi=4.d0*pi)

* Mutimapping BW
      REAL*8 alfa1,alfa2,auxalfa2,
     &       alfa3,auxalfa3,alfa4,auxalfa4,
     &       xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max,
     &       xw2_min,xw2_max,xf3_min,xf3_max

* Mutimapping t
      REAL*8 alfa,xt1_min,xt1_max,xt2_min,xt2_max,spta,sptb
      REAL*8 cos_sep
      PARAMETER(cos_sep=0.4d0)

      io1=iorder(1)
      io2=iorder(2)
      io3=iorder(3)
      io4=iorder(4)
      io5=iorder(5)
      io6=iorder(6)

      xjac=1.d0

c giuseppe 07/07/2007
c Determine which minimum pt must be assigned to t-channel, depending
c on final-state fermion's flavour
      IF(abs(id5).LE.5 .OR. id5.EQ.21)THEN
        pt5min=pt5min_in(1)
      ELSEIF(abs(id5).EQ.11)THEN
        pt5min=pt5min_in(2)
      ELSEIF(abs(id5).EQ.12)THEN
        pt5min=pt5min_in(3)
      ELSE
        print*,'***phsp1_1_31_jac ERROR'
        STOP
      ENDIF
      IF(abs(id6).LE.5 .OR. id6.EQ.21)THEN
        pt6min=pt6min_in(1)
      ELSEIF(abs(id6).EQ.11)THEN
        pt6min=pt6min_in(2)
      ELSEIF(abs(id6).EQ.12)THEN
        pt6min=pt6min_in(3)
      ELSE
        print*,'***phsp1_1_31_jac ERROR'
        STOP
      ENDIF
c end giuseppe 07/07/2007

C All these invariant masses should be computed once and for all and
C stored in some array.

      s111=(p(0,io1)+p(0,io2))**2
      s11=(p(0,io1)+p(0,io2)+p(0,io3))**2
      s1=(p(0,io1)+p(0,io2)+p(0,io3)+p(0,io4))**2
      saux=(p(0,io1)+p(0,io2)+p(0,io3)+p(0,io4)+p(0,io6))**2
      DO i=1,3
        s111=s111-(p(i,io1)+p(i,io2))**2
        s11=s11-(p(i,io1)+p(i,io2)+p(i,io3))**2
        s1=s1-(p(i,io1)+p(i,io2)+p(i,io3)+p(i,io4))**2
        saux=saux-(p(i,io1)+p(i,io2)+p(i,io3)+p(i,io4)+p(i,io6))**2
      ENDDO
      IF( s111.LE.1.d-8 .OR. s11.LE.1.d-8 .OR. s1.LE.1.d-8
     &             .OR. saux.LE.1.d-8 )THEN
        xjac=0.d0
        RETURN
      ENDIF

* saux = (q1+p6)**2 = qaux**2: flat
      e_sup = sqrt(shat)-xm(io5)
      e_inf = xm(io1)+xm(io2)+xm(io3)+xm(io4)+xm(io6)
c      e_inf = sqrt(s1)+xm(io6)
      eaux=sqrt(saux)
      xjac=xjac*(e_sup-e_inf)*2.d0*eaux
      xjac=xjac/twopi

* pa+pb --> p5 + qaux; (pa-p5)**2 = (pb-qaux)**2 = t1
      rmtasq=rmta*rmta
      rmtbsq=rmtb*rmtb
      app=((shat-saux-xmsq(io5))**2-4.d0*saux*xmsq(io5))/(4.d0*shat)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      pmod=sqrt(app)
      app=((shat-xmasq-xmbsq)**2-4.d0*xmasq*xmbsq)/(4.d0*shat)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p0mod=sqrt(app)

      IF(pt5min.LE.0.d0)THEN
        t_inf =xmbsq+saux-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                                    (shat+saux-xmsq(io5))
     &               -2.d0*p0mod*pmod
        app=(xmbsq-saux)*(xmasq-xmsq(io5))
     &           +(saux-xmbsq+xmasq-xmsq(io5))*
     &                        (saux*xmasq-xmbsq*xmsq(io5))/shat
        t_sup=app/t_inf
      ELSE
        IF(pt5min.GE.pmod)THEN
          xjac=0.d0
          RETURN
        ENDIF
        app=sqrt((pmod-pt5min)*(pmod+pt5min))
        t_inf =xmbsq+saux-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                                    (shat+saux-xmsq(io5))
     &               -2.d0*p0mod*app
        t_sup =xmbsq+saux-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                                    (shat+saux-xmsq(io5))
     &               +2.d0*p0mod*app
      ENDIF

      spta=xmbsq+saux-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                                    (shat+saux-xmsq(io5))
     &               +2.d0*p0mod*pmod*cos_sep

* From now on we use t1 = (pa-p5)**2 - rmtasq = (pb-qaux)**2 - rmtasq
      t_inf=t_inf-rmtasq
      t_sup=t_sup-rmtasq
      spta=spta-rmtasq
      CALL T_INI(t_inf,t_sup,spta,expa,
     &                 alfa,xt1_min,xt1_max,xt2_min,xt2_max)
      t1=xmasq+xmsq(io5)-0.5d0/shat*(shat+xmasq-xmbsq)*
     &                                    (shat+xmsq(io5)-saux)
     &               +2.d0*p0mod*p(3,io5)-rmtasq
      IF(t1.LE.spta)THEN
        xjac=xjac*0.5d0*(xt1_max-xt1_min)/p0mod/pmod
        xjac=xjac/alfa
      ELSE
        IF(expa.eq.0.d0)THEN
          xjac=xjac*0.5d0*(xt2_max-xt2_min)/p0mod/pmod
        ELSEIF(expa.ne.1.d0)THEN
          xjac=xjac*0.5d0*(xt2_max-xt2_min)
     &       /(1.d0-expa)/p0mod/pmod*(-t1)**expa
        ELSE
          xjac=xjac*0.5d0*(xt2_max-xt2_min)/p0mod/pmod*(-t1)
        ENDIF
          xjac=xjac/(1.d0-alfa)
      ENDIF

* qaux and p5
* beta/2/(4*pi)**2
      xjac=xjac*pmod/sqrt(shat)/(fourpi)**2
      xjac=xjac*twopi

* s1 = (q11+q12)**2 = q1**2
c      e_sup = sqrt(shat)-xm(io5)-xm(io6)
c      e_inf = sqrt(s11)+ sqrt(s12)
      e_sup = sqrt(saux)-xm(io6)
      e_inf = xm(io1)+xm(io2)+xm(io3)+xm(io4)
      CALL PHSP_INI5(e_sup,e_inf,
     &               spm1alo,spm1ahi,rm1a,gam1a,
     &               spm1blo,spm1bhi,rm1b,gam1b,
     &               alfa1,alfa2,auxalfa2,
     &               alfa3,auxalfa3,alfa4,auxalfa4,
     &               xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max,
     &               xw2_min,xw2_max,xf3_min,xf3_max)
      e1=sqrt(s1)
      IF(e1.LE.spm1alo .AND. alfa1.GT.0.d0)THEN
        xjac=xjac*(xf1_max-xf1_min)*2.d0*e1/alfa1
      ELSEIF(e1.LE.spm1ahi .AND. alfa2.GT.0.d0)THEN
        x_max=xw1_max
        x_min=xw1_min
        xjac=xjac*(x_max-x_min)*((s1-rm1a**2)**2+
     &       (gam1a*rm1a)**2)/(gam1a*rm1a)/(1.d0-alfa1)/auxalfa2
      ELSEIF(e1.LE.spm1blo .AND. alfa3.GT.0.d0)THEN
        xjac=xjac*(xf2_max-xf2_min)*2.d0*e1
     &       /(1.d0-alfa1)/(1.d0-auxalfa2)/auxalfa3
      ELSEIF(e1.LE.spm1bhi .AND. alfa4.GT.0.d0)THEN
        x_max=xw2_max
        x_min=xw2_min
        xjac=xjac*(x_max-x_min)*((s1-rm1b**2)**2+
     &       (gam1b*rm1b)**2)/(gam1b*rm1b)/(1.d0-alfa1)
     &       /(1.d0-auxalfa2)/(1.d0-auxalfa3)/auxalfa4
      ELSE
        xjac=xjac*(xf3_max-xf3_min)*2.d0*e1
     &       /(1.d0-alfa1)/(1.d0-auxalfa2)
     &       /(1.d0-auxalfa3)/(1.d0-auxalfa4)
      ENDIF
      xjac=xjac/twopi

* q1 and q6
      app=((saux-s1-xmsq(io6))**2-4.d0*s1*xmsq(io6))/(4.d0*saux)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      pauxmod=sqrt(app)
      t1aux=t1+rmtasq
      app=((saux-t1aux-xmbsq)**2-4.d0*t1aux*xmbsq)/(4.d0*saux)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      pbauxmod=sqrt(app)

*Define theta, phi as a function of t2 = (pb-p6)**2 = ((pa-p5)-q1)**2
      t_inf=t1aux+s1-0.5d0/saux*(saux+t1aux-xmbsq)*(saux+s1-xmsq(io6))
     &        -2.d0*pauxmod*pbauxmod
      app=(t1aux-s1)*(xmbsq-xmsq(io6))
     &     +(s1-t1aux+xmbsq-xmsq(io6))*(s1*xmbsq-t1aux*xmsq(io6))/saux
      t_sup=app/t_inf
* In order to get the exact limit of integration for t2 from pt6min we
* should know the invariant mass (q1+p5)**2.
* However (q1+p5)**2 > s1+xmsq(io5)**2. Hence we can establish bounds
* on t2 which are looser than the exact ones.
      IF(pt6min.gt.0.d0)THEN
        app=((shat-(s1+xmsq(io5))-xmsq(io6))**2
     &                   -4.d0*(s1+xmsq(io5))*xmsq(io6))/(4.d0*shat)
        IF(app.LE.pt6min*pt6min)THEN
          xjac=0.d0
          RETURN
        ENDIF
        pmod_15=sqrt(app)
        app=sqrt((pmod_15-pt6min)*(pmod_15+pt6min))

        t_inf0 =xmbsq+xmsq(io6)-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                             (shat+xmsq(io6)-(s1+xmsq(io5)))
     &               -2.d0*p0mod*app
        t_sup0 =xmbsq+xmsq(io6)-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                             (shat+xmsq(io6)-(s1+xmsq(io5)))
     &               +2.d0*p0mod*app
        t_inf=max(t_inf,t_inf0)
        t_sup=min(t_sup,t_sup0)
      ENDIF

      sptb=t1aux+s1-0.5d0/saux*(saux+t1aux-xmbsq)*(saux+s1-xmsq(io6))
     &        +2.d0*pauxmod*pbauxmod*cos_sep

* From now on we use t2 = (pb-p6)**2 - rmtbsq = ((pa-p5)-q1)**2 - rmtbsq
      t_inf=t_inf-rmtbsq
      t_sup=t_sup-rmtbsq
      sptb=sptb-rmtbsq
      CALL T_INI(t_inf,t_sup,sptb,expb,
     &                 alfa,xt1_min,xt1_max,xt2_min,xt2_max)
      t2=xmbsq+xmsq(io6)
     &      -2.d0*sqrt(p0mod*p0mod+xmbsq)*p(0,io6)
     &                    -2.d0*p0mod*p(3,io6)-rmtbsq
      IF(t2.LE.sptb)THEN
        xjac=xjac*0.5d0*(xt1_max-xt1_min)/pauxmod/pbauxmod
        xjac=xjac/alfa
      ELSE
        IF(expb.eq.0.d0)THEN
          xjac=xjac*0.5d0*(xt2_max-xt2_min)/pauxmod/pbauxmod
        ELSEIF(expb.ne.1.d0)THEN
          xjac=xjac*0.5d0*(xt2_max-xt2_min)
     &       /(1.d0-expb)/pauxmod/pbauxmod*(-t2)**expb
        ELSE
          xjac=xjac*0.5d0*(xt2_max-xt2_min)/pauxmod/pbauxmod*(-t2)
        ENDIF
        xjac=xjac/(1.d0-alfa)
      ENDIF
* beta/2/(4*pi)**2
      xjac=xjac*pauxmod/sqrt(saux)/(fourpi)**2
      xjac=xjac*twopi

* s11 = (p1+p2+p3)**2 = (q111+p3)**2 = q11**2
      e_sup = sqrt(s1)-xm(io4)
      e_inf = xm(io1)+xm(io2)+xm(io3)
      IF(e_sup.LE.e_inf)THEN
        xjac=0.d0
        RETURN
      ENDIF
      CALL PHSP_INI5(e_sup,e_inf,
     &               spm11alo,spm11ahi,rm11a,gam11a,
     &               spm11blo,spm11bhi,rm11b,gam11b,
     &               alfa1,alfa2,auxalfa2,
     &               alfa3,auxalfa3,alfa4,auxalfa4,
     &               xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max,
     &               xw2_min,xw2_max,xf3_min,xf3_max)
      e11=sqrt(s11)
      IF(e11.LE.spm11alo .AND. alfa1.GT.0.d0)THEN
        xjac=xjac*(xf1_max-xf1_min)*2.d0*e11/alfa1
      ELSEIF(e11.LE.spm11ahi .AND. alfa2.GT.0.d0)THEN
        x_max=xw1_max
        x_min=xw1_min
        xjac=xjac*(x_max-x_min)*((s11-rm11a**2)**2+
     &       (gam11a*rm11a)**2)/(gam11a*rm11a)/(1.d0-alfa1)/auxalfa2
      ELSEIF(e11.LE.spm11blo .AND. alfa3.GT.0.d0)THEN
        xjac=xjac*(xf2_max-xf2_min)*2.d0*e11
     &       /(1.d0-alfa1)/(1.d0-auxalfa2)/auxalfa3
      ELSEIF(e11.LE.spm11bhi .AND. alfa4.GT.0.d0)THEN
        x_max=xw2_max
        x_min=xw2_min
        xjac=xjac*(x_max-x_min)*((s11-rm11b**2)**2+
     &       (gam11b*rm11b)**2)/(gam11b*rm11b)/(1.d0-alfa1)
     &       /(1.d0-auxalfa2)/(1.d0-auxalfa3)/auxalfa4
      ELSE
        xjac=xjac*(xf3_max-xf3_min)*2.d0*e11
     &       /(1.d0-alfa1)/(1.d0-auxalfa2)
     &       /(1.d0-auxalfa3)/(1.d0-auxalfa4)
      ENDIF
      xjac=xjac/twopi

* s111 = (p1+p2)**2 = q111**2
      e_sup = sqrt(s11)-xm(io3)
      e_inf = xm(io1)+xm(io2)
      IF(e_sup.LE.e_inf)THEN
        xjac=0.d0
        RETURN
      ENDIF
      CALL PHSP_INI5(e_sup,e_inf,
     &               spm111alo,spm111ahi,rm111a,gam111a,
     &               spm111blo,spm111bhi,rm111b,gam111b,
     &               alfa1,alfa2,auxalfa2,
     &               alfa3,auxalfa3,alfa4,auxalfa4,
     &               xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max,
     &               xw2_min,xw2_max,xf3_min,xf3_max)
      e111=sqrt(s111)
      IF(e111.LE.spm111alo .AND. alfa1.GT.0.d0)THEN
        xjac=xjac*(xf1_max-xf1_min)*2.d0*e111/alfa1
      ELSEIF(e111.LE.spm111ahi .AND. alfa2.GT.0.d0)THEN
        x_max=xw1_max
        x_min=xw1_min
        xjac=xjac*(x_max-x_min)*((s111-rm111a**2)**2+
     &       (gam111a*rm111a)**2)/(gam111a*rm111a)/(1.d0-alfa1)/auxalfa2
      ELSEIF(e111.LE.spm111blo .AND. alfa3.GT.0.d0)THEN
        xjac=xjac*(xf2_max-xf2_min)*2.d0*e111
     &       /(1.d0-alfa1)/(1.d0-auxalfa2)/auxalfa3
      ELSEIF(e111.LE.spm111bhi .AND. alfa4.GT.0.d0)THEN
        x_max=xw2_max
        x_min=xw2_min
        xjac=xjac*(x_max-x_min)*((s111-rm111b**2)**2+
     &       (gam111b*rm111b)**2)/(gam111b*rm111b)/(1.d0-alfa1)
     &       /(1.d0-auxalfa2)/(1.d0-auxalfa3)/auxalfa4
      ELSE
        xjac=xjac*(xf3_max-xf3_min)*2.d0*e111
     &       /(1.d0-alfa1)/(1.d0-auxalfa2)
     &       /(1.d0-auxalfa3)/(1.d0-auxalfa4)
      ENDIF
      xjac=xjac/twopi

* q11 and p4
      app=((s1-s11-xmsq(io4))**2-4.d0*s11*xmsq(io4))/(4.d0*s1)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p1mod=sqrt(app)
* beta/2/(4*pi)**2
      xjac=xjac*p1mod/sqrt(s1)/(fourpi)**2
      xjac=xjac*fourpi

* q11 --> p111 and p3
      app=((s11-s111-xmsq(io3))**2
     &              -4.d0*s111*xmsq(io3))/(4.d0*s11)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p11mod=sqrt(app)
* beta/2/(4*pi)**2
      xjac=xjac*p11mod/sqrt(s11)/(fourpi)**2
      xjac=xjac*fourpi

* q111 --> p1 and p2
      app=((s111-xmsq(io1)-xmsq(io2))**2
     &              -4.d0*xmsq(io1)*xmsq(io2))/(4.d0*s111)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p111mod=sqrt(app)
* beta/2/(4*pi)**2
      xjac=xjac*p111mod/sqrt(s111)/(fourpi)**2
      xjac=xjac*fourpi

      RETURN
      END