include "ffit.h"

define	LAMBDA	0.1	# offset for parameters = LAMBDA * range

# fcs_fit -- do the fit
# This routine sets up the parameters for a fit using the amoeba code
# and calls amoeba.  This is repeated with a smaller starting simplex.
# If there are no variable parameters, this routine simply returns.

procedure fcs_fit (fnl, range, ftol, funk)

pointer fnl		# i: pointer to fit struct
real	range[ARB]	# i: a measure of the range of the parameters
real	ftol		# i: fractional tolerance for accepting fit
extern	funk()		# i: the function to be fit
real	funk()
#--
pointer sp
pointer vpp		# scratch for variable parameters
pointer pp		# scratch for p (parameters at each vertex)
pointer yp		# scratch for y (function evaluated at each vertex)
int	nvpar		# number of variable parameters
int	iter		# number of iterations
int	j, k		# loop indexes

begin
	# Find out how many of the parameters are to be varied.
	nvpar = 0
	do k = 1, F_NPAR(fnl) {
	    if (F_VAR(fnl,k))
		nvpar = nvpar + 1
	}
	if (nvpar < 1)
	    return			# nothing to do

	call smark (sp)

	call salloc (vpp, nvpar, TY_REAL)
	call salloc (pp, nvpar*(nvpar+1), TY_REAL)
	call salloc (yp, nvpar+1, TY_REAL)

	# Copy the parameters that are to be varied to a scratch array.
	j = 0
	do k = 1, F_NPAR(fnl) {
	    if (F_VAR(fnl,k)) {
		Memr[vpp+j] = F_PAR(fnl,k)	# j is zero indexed here
		j = j + 1
	    }
	}

	# Fill the vertices of the simplex, and evaluate the function
	# at each vertex.  Memr[pp] and Memr[yp] are output.
	call f_sim_fill (fnl, nvpar, Memr[vpp], LAMBDA, range,
		Memr[pp], Memr[yp], funk)

	# Log the parameter values and function value at each vertex.
	if (F_LOGFILE(fnl) != NULL) {
	    call fprintf (F_LOGFILE(fnl), "Initial values at each vertex:\n")
	    do k = 1, F_NPAR(fnl) {
		if (F_VAR(fnl,k)) {
		    call fprintf (F_LOGFILE(fnl), "        par%d")
			call pargi (k)
		}
	    }
	    call fprintf (F_LOGFILE(fnl), "    function\n")
	    do k = 1, nvpar+1
		call fcs_log (fnl, Memr[pp], nvpar, Memr[yp+k-1], k)
	}

	# Do the fit.
	call famoeb (fnl, Memr[pp], Memr[yp], nvpar, ftol,
		funk, iter)

	if (F_LOGFILE(fnl) != NULL) {
	    call fprintf (F_LOGFILE(fnl), "Number of iterations = %d\n")
		call pargi (iter)
	}

	# Get the average values of the parameters.
	call f_sim_avg (Memr[pp], Memr[vpp], nvpar)

	# Fill the vertices again, starting with the current values
	# (Memr[vpp]) and a smaller offset.
	call f_sim_fill (fnl, nvpar, Memr[vpp], LAMBDA*LAMBDA, range,
		Memr[pp], Memr[yp], funk)

	# Log the parameter values and function value at each vertex.
	if (F_LOGFILE(fnl) != NULL) {
	    call fprintf (F_LOGFILE(fnl), "Intermediate values:\n")
	    do k = 1, nvpar+1
		call fcs_log (fnl, Memr[pp], nvpar, Memr[yp+k-1], k)
	}

	# Redo the fit.
	call famoeb (fnl, Memr[pp], Memr[yp], nvpar, ftol,
		funk, iter)

	if (F_LOGFILE(fnl) != NULL) {
	    call fprintf (F_LOGFILE(fnl), "Number of iterations = %d\n")
		call pargi (iter)
	}

	# Get the averages
	call f_sim_avg (Memr[pp], Memr[vpp], nvpar)

	# Log final fit and function value.
	if (F_LOGFILE(fnl) != NULL) {
	    call fprintf (F_LOGFILE(fnl), "Final values:\n")
	    do k = 1, nvpar {
		call fprintf (F_LOGFILE(fnl), "%12.5g")
		    call pargr (Memr[vpp+k-1])
	    }
	    call fprintf (F_LOGFILE(fnl), "%12.5g\n")
		call pargr (funk (fnl, Memr[vpp]))
	}

	# Replace values in F_PAR.
	j = 0
	do k = 1, F_NPAR(fnl) {
	    if (F_VAR(fnl,k)) {
		F_PAR(fnl,k) = Memr[vpp+j]	# j is zero indexed here
		j = j + 1
	    }
	}

	call sfree (sp)
end

# f_sim_fill -- fill simplex & evaluate function
# The first index of P is the vertex number, and the second index is
# the parameter number.

procedure f_sim_fill (fnl, nvpar, vp, lambda, range,
		p, y, funk)

pointer fnl			# i: pointer to fit struct
int	nvpar			# i: number of variable parameters
real	vp[nvpar]		# i: initial values of variable parameters
real	lambda			# i: scale length
real	range[ARB]		# i: offset = range * lambda
real	p[nvpar+1,nvpar]	# o: initial par values at vertices
real	y[nvpar+1]		# o: function evaluated at vertices
extern	funk()			# i: function to be minimized
real	funk()
#--
real	sp[MPAR]		# copy of par values at a vertex
int	j, k			# loop indexes

begin
	# Initially fill each vertex with the same values.
	do k = 1, nvpar {
	    do j = 1, nvpar+1
		p[j,k] = vp[k]
	}

	# Apply an offset for vertices after the first.
	do k = 1, nvpar
	    p[1+k,k] = vp[k] + range[k] * lambda

	# Evaluate function at each vertex.
	do j = 1, nvpar + 1 {
	    do k = 1, nvpar
		sp[k] = p[j,k]
	    y[j] = funk (fnl, sp)
	}
end

# f_sim_avg -- average the values at the vertices
# This routine takes the average over vertices of the values of each variable
# parameter.

procedure f_sim_avg (p, vp, nvpar)

real	p[nvpar+1,nvpar]	# i: parameter values at vertices
real	vp[nvpar]		# o: averages of variable parameter values
int	nvpar			# i: number of variable parameters
#--
real	sum			# for accumulating sum
int	j, k			# loop indexes

begin
	do k = 1, nvpar {		# do for each variable parameter
	    sum = 0.
	    do j = 1, nvpar+1		# do for each vertex
		sum = sum + p[j,k]
	    vp[k] = sum / (nvpar + 1.)
	}
end

# fcs_log -- write to log file
# This routine writes the values of the variable parameters and the function
# to be minimized.

procedure fcs_log (fnl, p, nvpar, y, vertex)

pointer fnl			# i: pointer to fit struct
real	p[nvpar+1,nvpar]	# i: variable parameters at each vertex
real	y			# i: function value at current vertex
int	nvpar			# i: number of variable parameters
int	vertex			# i: vertex number
#--
int	k

begin
	# Write out the values of the variable parameters.
	do k = 1, nvpar {
	    call fprintf (F_LOGFILE(fnl), "%12.5g")
		call pargr (p[vertex,k])
	}

	# Write the value of the function at the current vertex.
	call fprintf (F_LOGFILE(fnl), "%12.5g\n")
	    call pargr (y)
end