!*****************************************
!*  Rasmus Handberg
!*  Department of Physics and Astronomy
!*  University of Aarhus
!*****************************************

module fminsearch
	!private converged
	interface
		subroutine simplex_1D(f, x0, eps, xmin, fmin)
			use kind_defs
			real(dbl), external :: f
			real(dbl), intent(in) :: x0(2), eps
			real(dbl), intent(out) :: xmin, fmin
		end subroutine
	
		subroutine simplex(f, x0, eps, xmin, fmin)
			use kind_defs
			real(dbl), external :: f
			real(dbl), intent(in) :: x0(:,:), eps
			real(dbl), intent(out) :: xmin(size(x0,1)), fmin
		end subroutine
	
		logical function converged(P, eps)
			use kind_defs
			real(dbl), intent(in) :: P(:,:), eps
		end function
	end interface
end module

! For caluclating in 1D.
! Just do it so you can call it directly with reals and not vector-reals.
subroutine simplex_1D(f, x0, eps, xmin, fmin)
	use kind_defs
	use fminsearch, only : simplex
	implicit none
	real(dbl), external :: f
	real(dbl), intent(in) :: x0(2), eps
    real(dbl), intent(out) :: xmin, fmin
    real(dbl) :: A(1,2), vmin(1)
    
    ! Make the input as a 1x2 matrix:
    A(1,1:2) = x0
    ! Call the simplex-algoritm:
    call simplex(f, A, eps, vmin, fmin)
	! And return in the form of a real instead of a 1-length vector:
	xmin = vmin(1)
end subroutine

! Downhill simplex
subroutine simplex(f, x0, eps, xmin, fmin)
	use kind_defs
	use fminsearch, only : converged
	implicit none
	real(dbl), external :: f
	real(dbl), intent(in) :: x0(:,:), eps
	real(dbl), intent(out) :: xmin(size(x0,1)), fmin
	real(dbl), dimension(size(x0,1),size(x0,2)) :: P
	real(dbl) :: fP((size(x0,2))), fptmp
	real(dbl), dimension(size(x0,1)) :: phi, plo, pce, ptmp
	integer :: i, n, m, hi, lo, iter, maxiter

	! Variables: 
	P = x0 ! Nessary so P can be changed
	n = size(P, 1)
	m = size(P, 2)
	maxiter = 10000
	
	! Make vector of function-values:
	do i = 1,m
		fP(i) = f(P(:,i))
	enddo

	! Start the iteration:
	do iter = 1,maxiter
		! Find high, low and centroide:
		hi = maxloc(fP,1); phi = P(:,hi)
		lo = minloc(fP,1); plo = P(:,lo)
		pce = 0.0
		do i = 1,m
			if (i /= hi) then
				pce = pce + P(:,i)
			endif
		enddo
		pce = pce/real(n)
		
		! Try Reflection:
		ptmp = pce + (pce - phi)
		fptmp = f(ptmp)
		
		if (fptmp < fP(hi)) then
			! Accept Reflection:
			!print *, "Reflection"
			P(:,hi) = ptmp
			fP(hi) = fptmp
		elseif (fptmp < fP(lo)) then
			! Expansion:
			!print *, "Expansion"
			plo = pce + 2*(plo - pce)
			P(:,lo) = plo
			fP(lo) = f(plo)
		else
			! Try Contraction:
			ptmp = pce + (phi - pce)
			fptmp = f(ptmp)
			
			if (fptmp < fP(hi)) then
				! Accept Contraction:
				!print *, "Contraction"
				P(:,hi) = ptmp
				fP(hi) = fptmp
			else
				! Reduction:
				!print *, "Reduction"
				do i = 1,m
					if (i /= lo) then
						P(:,i) = 0.5*(P(:,i) + plo)
						fP(i) = f(P(:,i))
					endif
				enddo
			endif
		endif
	
		if (converged(P, eps)) exit
	enddo
	
	! Take the lowest point in simplex as the soluton:
	lo = minloc(fP, 1)
	xmin = P(:,lo)
	fmin = fP(lo)
end subroutine

logical function converged(P, eps)
	use kind_defs
	implicit none
	real(dbl), dimension(:,:), intent(in) :: P
	real(dbl), intent(in) :: eps
	real(dbl), dimension(size(P,2)) :: v
	integer :: i, n, k
	n = size(P, 2)
	
	converged = .true.
	do i = 1,n
		do k = i+1,n
			v = P(:,k)-P(:,i)
			if (sqrt(dot_product(v,v)) >= eps) then
				converged = .false.
				return
			endif
		enddo
	enddo
end function