# File rvsao/Emvel/emgauss.x
# July 13, 1995
# By Doug Mink, Harvard-Smithsonian Center for Astrophysics
# After Bill Wyatt

# Copyright(c) 1995 Smithsonian Astrophysical Observatory
# You may do anything you like with this file except remove this copyright.
# The Smithsonian Astrophysical Observatory makes no representations about
# the suitability of this software for any purpose.  It is provided "as is"
# without express or implied warranty.

#--- Gaussian program interfaces to EMMIN

procedure minifit (fdata, npix, ngauss, gcenters, gheights, gwidths,
	 	   nparams, pixleft, pixright, debug, retvec, covar, ierr)

real	fdata[ARB]	# input spectrum 
int	npix		# size of input spectrum 
double	gcenters[ARB]	# pixel centers to fit 
double	gheights[ARB]	# emission line heights to fit
double	gwidths[ARB]	# emission line widths to fit
int	ngauss		# number of gaussians to fit (3 MAX) 
int	nparams		# total number of parameters (15 MAX) 
int	pixleft		# fitting limits 
int	pixright	# fitting limits 
bool	debug		# print diagnostic messages if TRUE
double	retvec[ARB]	# RETURNED vector with minimized parameters 
double	covar[ARB]	# RETURNED diagonal of covariance matrix 
int	ierr		# RETURNED error flag 

int	i, j

int	npoly		# dimension of continuum fit 
double	conv		# converts FWHM to sigma 
double	pcoeff[5]	# polynomial coefficients
double	rmin[15],rmax[15]

bool	gmsperr
int	section[4]
int	nsect
double	ht, wd, htmax, mln2

include "emg.com"

begin
	mln2 = 0.69314718055994530942d0
	npoly = nparams - ngauss * 3

	conv = sqrt (8.d0 * mln2)

# save in common for communication to CHI**2 routine 

	pixl = pixleft
	pixr = pixright
	npar = nparams 
	ngau = ngauss
	call aclrd (retvec,nparams)

# compute initial guess vector 

# Continuum polynomial 

	nsect = 2
	section[1] = pixleft
	section[2] = pixleft + npoly
	section[3] = pixright - npoly
	section[4] = pixright

	if (npoly > 1)
	    call gmspfit (fdata, nsect, section, npoly, pcoeff, gmsperr)
	else if (npoly > 0) {
	    pcoeff[1] = 0.d0
	    gmsperr = FALSE
	    }
	if (debug) {
	    if (gmsperr)
		call printf ("MINIFIT: %d-term continuum fit error\n")
	    else
		call printf ("MINIFIT: %d-term continuum fit used\n")
		call pargi (npoly)
	    do i = 1, npoly {
		call printf (" c(%d)= %10.6g ")
		    call pargi (i)
		    call pargd (pcoeff[i])
		}
	    call printf ("\n")
	    }

	# if gmspfit doesn't work, guess another way 
	if (gmsperr) {

	# estimate slope & offset from end pixels
	    if (npoly >= 2) {
		retvec[npoly-2] = (fdata[pixright] - fdata[pixleft]) / npix
		retvec[npoly-1] = fdata[pixleft] - retvec[npoly-2] * pixleft
		}
	# constant guess from mean of left and right pixels
	    if (npoly == 1)
		retvec[npoly-1] = (fdata[pixleft] + fdata[pixright]) / 2.0
	    }

# normal - transfer coefficients 
	else if (npoly > 0) {
	    do i = 1, npoly {
		retvec[npoly-i+1] = pcoeff[i]
		}
	    }

# Gaussians 
	htmax = 0.d0
	do i = 1, ngauss {

	# Evaluate continuum at center of gaussian
	    ht = gheights[i]
	    wd = gwidths[i] / conv
	    if (ht > htmax) htmax = ht

	# Set initial values for this gaussian
	    J = npoly + 1 + (i-1) * 3
	    retvec[j]   = gcenters[i]		# center 
	    retvec[j+1] = ht			# height 
	    retvec[j+2] = wd			# sigma = FWHM/conv 
	    rmin[j] = pixleft
	    rmax[j] = pixright
	    rmin[j+1] = .0d0
	    rmax[j+1] = 3.d0 * ht
	    rmin[j+2] = 1.d0 / conv
	    rmax[j+2] = 3.d0 * wd

	    }
	if (npoly == 1) {
	    rmin[1] = retvec[1] - htmax
	    rmax[1] = retvec[1] + htmax
	    }
	else {
	    do j = 1, npoly {
		rmin[j] = 0.d0
		rmax[j] = 0.d0
		}
	    }
	if (debug) {
	    call printf ("MINIFIT: %d-term gaussian fit\n")
		call pargi (nparams)
	    call printf (" a(i) = ")
	    do i = 1, nparams {
		call printf ("%10.4g ")
		    call pargd (retvec[i])
        	}
	    call printf ("\n")
            }

#  Fit 1-3 gaussians
	call emmin (fdata, nparams, retvec, rmin, rmax, debug, covar, ierr)

	if (debug) {
	    call printf (" a(i) = ")
	    do i = 1, nparams {
		call printf ("%10.7g ")
		    call pargd (retvec[i])
        	}
	    call printf ("\n")
	    call printf ("da(i) = ")
	    do i = 1, nparams {
		call printf ("%10.7g ")
		    call pargd (covar[i])
        	}
	    call printf ("\n")
            }
#	do i = 1, nparams {
#	    if (retvec[i] < rmin[i])
#		ierr = 3
#	    else if (retvec[i] > rmax[i])
#		ierr = 3
#	    }

	return
end


# Calculate chi**2, weighted by the data itself 
# Called by EMMIN
# Note the Chi**2 function is :
#
#    chi^2 = SUM { ( [ Xi - f(Xi) ] **2 ) / weight }
# 
# This routine uses the data itself as the weight.
 
procedure gauss (spectrum,a,debug,vchi)

real	spectrum[ARB]	# Spectrum
double	a[ARB]		# Polynomial continuum and gaussian parameters
bool	debug		# True to print chi^2
double	vchi		# RETURNED chi^2

int	i
double	valobs, valcal, valdif, pix

double	emfit_eval()
include "emg.com"

begin
	vchi = 0.d0
	do i = pixl, pixr {
	    if (abs (spectrum[i]) > 1.e-36) {
		pix = double (i)
		valobs = double (spectrum[i])
		valcal = emfit_eval (npar, ngau, a, pix)
		valdif = valobs - valcal

	    # data is the weight 
#		vchi = vchi + (valdif * valdif) / abs (valobs)
		vchi = vchi + (valdif * valdif)
		}
#	    call printf ("GAUSS: pixel %d: obs: %10.5f calc: %10.5f diff: %10.5f\n")
		call pargi (i)
		call pargd (valobs)
		call pargd (valcal)
		call pargd (valdif)
	    }
#	if (debug) {
#	    call printf ("GAUSS:  chi2 is %f\n")
#		call pargd (vchi)
#	    }

	return
end


# Generate 1st derivatives of chi**2 based on parameter vector `a', and
# return them in vector `dvec'.
# Called by EMMIN
#  Note: The first partial derivative of the Chi**2 function is:
#
#    d ( chi**2)                               d f(Xi)
#    ----------  = -2 * SUM { [ ( Xi - f(Xi) ) ------- ] / weight }
#       d A                                      d A
#
# This routine uses the data itself as the weight.

procedure dgauss (spectrum, a, dvec)

real	spectrum[ARB]
double	a[ARB]
double	dvec[ARB]

int	i, j
int	npoly
int	npix
pointer	drbuf
double	pix, gauss, dc, dh, ds
double	fact1, fact2
double	emfit_eval(), gauss_eval()
double	speci, specc, dspec

include "emg.com"

begin

# zero the return vector 
	call aclrd (dvec,npar)
	npoly = npar - ngau * 3
	npix   = pixr - pixl + 1

# grab storage for saving calculations for D2FUNK 
	if (deriv == NULL || diffbuf == NULL ||
	    npar > nderiv || (pixr - pixl + 1) * npar > ndata) {
	  
	# Allocate space for npix * npar derivatives 
	    npix = pixr - pixl + 1
	    ndata = npix * npar
	    if (deriv == NULL)
		call malloc (deriv, ndata, TY_DOUBLE)
	    else
		call realloc (deriv, ndata, TY_DOUBLE)
	    if (deriv == NULL) {
		call fprintf (STDERR," DFUNC Error - can't malloc DERIV ")
		ndata = 0
		return
		}
	  
	# Allocate space for npix differences 
	    nderiv  = npix
	    if (diffbuf == NULL)
		call malloc (diffbuf, nderiv, TY_DOUBLE)
	    else
		call realloc (diffbuf, nderiv, TY_DOUBLE)
	    if (diffbuf == NULL) {
		call fprintf(STDERR," DFUNC Error - can't malloc DIFFBUF? ")
		nderiv = 0
		return
		}
	    }	    

	# Calculate the derivatives and the Chi**2 derivatives over the fit 
	#  and the data points
	 
	do i = pixl, pixr {

	# skip if zero weight
	    if (abs (spectrum[i]) < 1.e-36) next

	# vector of npar partial derivatives for this pixel 
	    drbuf = deriv + (i - pixl) * npar

	    pix = double (i)  
	    speci = double (spectrum[i])
	    specc = emfit_eval (npar,ngau,a,pix)
	    dspec = speci - specc
	    Memd[diffbuf+i-pixl] = speci - specc

	# -2 * (Obs - Fit) / weight 
#	    fact1 = -2.0 * dspec / abs (spectrum[i])
	    fact1 = -2.0 * dspec
#	call printf ("DGAUSS: Pixel %5.0f, obs= %12.7g calc= %12.7g fact1= %12.7g\n")
#	    call pargd (pix)
#	    call pargd (speci)
#	    call pargd (specc)
#	    call pargd (fact1)

	# Polynomial part 
#	call printf ("       Polynomial:")
	    do j = 1, npoly {
		Memd[drbuf] = pix ** (npoly-j-1) 
		dvec[j] = dvec[j] + (fact1 * Memd[drbuf])
		drbuf = drbuf + 1
#	call printf (" %12.7g")
#	    call pargd (dvec[j])
		}
#	call printf ("\n")

	# Up to 3 Gaussian components at this point 
	    do j = npoly+1, npar, 3 {
		gauss = gauss_eval(pix, a[j])
		fact2 = (pix - a[j]) / (a[j+2] * a[j+2]) # (X-C)/sig**2 

	    # Center 
		dc = fact2 * gauss
		dvec[j] = dvec[j] + fact1 * dc
		Memd[drbuf] = dc
		drbuf = drbuf + 1

	    # Height 
		if (a[j+1] != 0)
		    dh = gauss / a[j+1]
		else
		    dh = 0.d0
		dvec[j+1] = dvec[j+1] + fact1 * dh
		Memd[drbuf] = dh
		drbuf = drbuf + 1

	    # Sigma 
		ds = a[j+2] * fact2 * fact2 * gauss
		dvec[j+2] = dvec[j+2] + fact1 * ds
		Memd[drbuf] = ds
		drbuf = drbuf + 1
		}
#	call printf ("    Gaussian= %12.7g,  c= %9.3f  h= %9.4f  s= %9.5f\n")
#	    call pargd (gauss)
#	    call pargd (a[j])
#	    call pargd (a[j+1])
#	    call pargd (a[j+2])
#	call printf ("       fact2= %12.7g, dc= %12.7g dh= %12.7g ds= %12.7g\n")
#	    call pargd (fact2)
#	    call pargd (dc)
#	    call pargd (dh)
#	    call pargd (ds)
#	call printf ("    Gaussian  :  %12.7g %12.7g %12.7g\n")
#	    call pargd (dvec[j])
#	    call pargd (dvec[j+1])
#	    call pargd (dvec[j+2])
	    }
	return
end


# Generate 2nd partial derivatives based on parameter vector `a', and
# return them in array `d2arr'
#
#  Note: The second partial derivative of the Chi**2 function is:
#
#  2                                       2
# d (chi**2)                              d f(Xi)   d f(Xi)  d f(Xi)
# ----------  = -2 SUM { ( [ Xi - f(Xi)] -------- - -------  ------- ) / wt }
#   dA dB                                  dA dB       dA       dB
# 
# where wt = weight, and is in this function the data itself.

procedure d2gaus (spectrum, a, d2arr, debug)

real	spectrum[ARB]
double	a[ARB]
double	d2arr[16,16]
bool	debug

int	i, j, k, m, jg
int	npoly
int	npix
double	gauss_eval()

double	pix
double	diff
double	part2nd
int	derindex
double	gauss
int	jmod, np2
 
double dcc(), dcs(), dch()
double dss(), dhs()

include "emg.com"

begin
	npoly = npar - ngau * 3
	npix  = pixr - pixl + 1
	np2 = npar * npar

	call aclrd (d2arr,np2)

	if (deriv == NULL || diffbuf == NULL ||
	    ndata < npix * npar || nderiv < npar) {
	    call fprintf(STDERR, "D2FUNK Error: DFUNK bad allocation?\n")
	    return
	    }

	do i = pixl, pixr {
	    pix = double (i+1)
	    diff = Memd[diffbuf + i - pixl]
	    derindex = (i - pixl) * npar
	    gauss = 0.0

	    if (abs (spectrum[i]) < 1.e-36) next

	    do j = 1, npar {

		if (j > npoly) {
		    jmod =  mod ((j-npoly),3)
		    if (jmod == 0) jmod = 3
		    jmod = jmod - 1
		    if (jmod == 0) 
			gauss = gauss_eval (pix, a[j])
		    }

		do k = j, npar {

	    # If poly terms or different Gaussians, 2nd partials are zero 
	    # poly cross terms are zero 
		    if (j<=npoly || k<=npoly ||
			(j-npoly-1)/3!=(k-npoly-1)/3) {
			if (spectrum[i] != 0) {
			    d2arr[j,k] = d2arr[j,k] + (2.d0 *
					 Memd[deriv+derindex+j-1] *
					 Memd[deriv+derindex+k-1] /
					 spectrum[i])
			    }
			}
	    # Gaussian cross terms 
		    else {
			m = mod (k-npoly, 3)
			jg = j - jmod
			if (m == 0) m = 3
			if (m == 1) {
			    if (jmod == 0)
				part2nd = dcc (pix, a[jg], gauss)
			    else if (jmod == 1)
				part2nd = dch (pix, a[jg], gauss)
			    else if (jmod == 2)
				part2nd = dcs (pix, a[jg], gauss)
			    }
			else if (m == 2) {
			    if (jmod == 0)
				part2nd = dch (pix, a[jg], gauss)
			    else if (jmod == 1)
				part2nd = 0.d0
			    else if (jmod == 2)
				part2nd = dhs (pix, a[jg], gauss)
			    }
			else if (m == 3) {
			    if (jmod == 0)
				part2nd = dcs (pix, a[jg], gauss)
			    else if (jmod == 1)
				part2nd = dhs (pix, a[jg], gauss)
			    else if (jmod == 2)
				part2nd = dss (pix, a[jg], gauss)
			    }
			if (spectrum[i] != 0) {
			    d2arr[j,k] = d2arr[j,k] + (-2.0 / spectrum[i]) * 
				     (diff * part2nd -
				     Memd[deriv+derindex + j - 1] *
				     Memd[deriv+derindex + k - 1])
			    }
			}
		    }
		}
	    }

# Copy the upper diagonal to the lower diagonal 
	do i = 1, npar {
	    do j = i+1, npar {
		d2arr[j,i] = d2arr[i,j]
		}
	    }

	return
end


# 2nd partial derivatives 
#   2                              
#  d G     Gauss(C,H,S)   
# ----- =  ------------ * [ ( (C - X) / S )**2  - 1 ]
#  dCdC        S**2       
 
double procedure dcc (pix, gcoef, gauss)

double	pix		# pixel number
double	gcoef[3]	# center, sigma, height 
double	gauss		# gaussian at this pixel

double	gauss_eval()
double	gau
double	fact, deriv

begin
	if (gauss == 0)
	    gau = gauss_eval (pix, gcoef)
	else
	    gau = gauss

# (Cen - X)/sig 
	if (gcoef[3] != 0) {
	    fact = (gcoef[1] - pix) / gcoef[3]
	    deriv = ((fact * fact - 1.0) * gau) / (gcoef[3] * gcoef[3])
	    }
	else
	    deriv = 0.d0
	return deriv
end

#    2
#   d G    - (C - X) 
#  ---- =  --------- * Gauss(C,H,S) * [ ( (C - X) / S )**2 - 2.0 ]
#  dCdS       S**3
 
double procedure dcs (pix, gcoef, gauss)

double	pix		# pixel number
double	gcoef[3]	# center, sigma, height 
double	gauss		# gaussian at this pixel

double gauss_eval()
double gau
double fact, deriv

begin
	if (gauss == 0)
	    gau = gauss_eval (pix, gcoef)
	else
	    gau = gauss

	if (gcoef[3] != 0) {
	    fact = (gcoef[1] - pix) / gcoef[3] # (Cen - X)/sig 
	    deriv = (-(fact * fact - 2.0) * fact * gau) / (gcoef[3] * gcoef[3])
	    }
	else
	    deriv = 0.d0
	return deriv
end

#   2
#  d G     1    dG    -(C - X)
# ----- = --- * -- =  --------- * Gauss(C,H,S)  
#  dCdH    H    dC     H * S**2
 
double procedure dch (pix, gcoef, gauss)

double	pix		# pixel number
double	gcoef[3]	# center, sigma, height 
double	gauss		# gaussian at this pixel

double gauss_eval()
double gau
double fact, deriv

begin
	if (gauss == 0)
	    gau = gauss_eval (pix, gcoef)
	else
	    gau = gauss

	if (gcoef[3] != 0 && gcoef[2] != 0) {
	    fact = (gcoef[1] - pix) / gcoef[3] # (Cen - X)/sig 
	    deriv = (-fact * gau) / (gcoef[2] * gcoef[3])
	    }
	else
	    deriv = 0.d0
	return deriv
end

#   2              2                             2
#  d G      (C - X)                       (C - X)
# ----- = [ ------- ] * Gauss(C,H,S) * [ ---------  -  3.0 ]
#  dSdS       S**4                          S**2
 
double procedure dss (pix, gcoef, gauss)

double	pix		# pixel number
double	gcoef[3]	# center, sigma, height 
double	gauss		# gaussian at this pixel

double	gauss_eval()
double	gau
double	fact, deriv

begin
	if (gauss == 0)
	    gau = gauss_eval (pix, gcoef)
	else
	    gau = gauss

	if (gcoef[3] != 0) {
	    fact = (gcoef[1] - pix) / gcoef[3] # (Cen - X)/sig 
	    fact = fact * fact
	    deriv = ((fact - 3.0) * fact * gau) / (gcoef[3] * gcoef[3])
	    }
	else
	    deriv = 0.d0
	return deriv
end

#   2                              2
#  d G     1    dG    1     (C - X)
# ----- = --- * -- = --- *  ------- * Gauss(C,H,S)
#  dHdS    H    dS    H      S**3
 
double procedure dhs (pix, gcoef, gauss)

double	pix		# pixel number
double	gcoef[3]	# center, sigma, height 
double	gauss		# gaussian at this pixel

double	gauss_eval()
double	gau
double	fact, deriv

begin
	if (gauss == 0)
	    gau = gauss_eval (pix, gcoef)
	else
	    gau = gauss

	if (gcoef[3] != 0 && gcoef[2] != 0) {
	    fact = (gcoef[1] - pix) / gcoef[3] # (Cen - X)/sig 
	    fact = fact * fact
	    deriv = (fact * gau) / (gcoef[2] * gcoef[3])
	    }
	else {
	    deriv = 0.d0
	    }
	return deriv
end


# Evaluate a poly + gaussian 

# order of coefficients in (a), 15 maximum, is
#   pn,    (continuum polynomial), highest-order coefficient 
#   pn-1,  (continuum polynomial)
#    ...etc...
#   p0,    (continuum polynomial)  constant coefficient
#   g1 (center)
#   g1 (sigma)
#   g1 (height)
#   g2 (center)
#   ...etc...
 
double procedure emfit_eval (npar, ngauss, a, pix)

int	npar	# total number of parameters 
int	ngauss	# Number of Gaussians in the fit 
double	a[ARB]	# coefficients 
double	pix	# indep. var. 

int	i
int	npoly
double	val
double	gauss_eval(), poly_eval()

begin
	npoly = npar - ngauss * 3
	val = poly_eval (pix, a, npoly)
	do i = npoly+1, npar, 3  {
	    val = val + gauss_eval(pix, a[i])
	    }

	return val
end


double procedure gauss_eval (pix, gcoef)

double pix		# pixel value 
double gcoef[3]		# Gaussian Center, Height, Sigma 

double arg, result

begin
	if (gcoef[3] > 0) {
	    arg = (gcoef[1] - pix) / gcoef[3]
	    result = gcoef[2] * exp (-0.5d0 * arg * arg)
	    }
	else
	    result = 0.d0

	return result
end


double procedure poly_eval (pix, pcoeff, n)

double	pix
double	pcoeff[ARB]	# Stored highest order first 
int	n

double	val		# RETURNED polynomial value
int i

begin
	val = 0.d0
	if (n <= 0) return
	do i = 1, n {
	    val = val * pix + pcoeff[i]
	    }

	return (val)
end


# GMSPFIT - fit to multiple sections of an input array 
# Return is true for success, false otherwise 

procedure gmspfit (fx, nsect, sections, ndim, coeff, gmsperr)

real	fx[ARB]		# input data array 
int	nsect		# number of data sections 
int	sections[ARB]	# pairs of (begin,end) points to fit 
int	ndim		# dimension of fit 
double	coeff[ARB]	# polynomial coefficients
bool	gmsperr		# true if fit done, else false

int	i, j
double	chisq
pointer	x, y, sp

int	count
int	nfit

begin
	gmsperr = TRUE
	if (nsect == 0) return

	nfit = 0
	do i = 1, nsect*2, 2 {
	    nfit = nfit + sections[i+1] - sections[i] + 1
	    }

	# allocate d.p. space 
	call smark (sp)
	x = NULL
	call salloc (x, nfit, TY_DOUBLE) 
	if (x == NULL) {
	    call printf (" GMSPFIT:  cannot allocate y\n")
	    call sfree (sp)
	    return
	    }
	y = NULL
	call salloc (y, nfit, TY_DOUBLE) 
	if (y == NULL) {
	    call printf (" GMSPFIT:  cannot allocate y\n")
	    call sfree (sp)
	    return
	    }

	count = 0
	do i = 1, nsect*2, 2  {
	    do j = sections[i], sections[i+1] {
		Memd[x+count] = double (j)
		Memd[y+count] = double (fx[j])
		count = count + 1
		}
	    }

# Call the fitting routine 
	call polfit (Memd[x], Memd[y], nfit, ndim, coeff, chisq)

	call sfree (sp)
	if (chisq > 0.0)
	    gmsperr = FALSE
	return
end


procedure initgaus()

include "emg.com"

begin
	deriv = NULL
	diffbuf = NULL
	ndata = 0
	nderiv = 0
end


procedure clgaus()

include "emg.com"

begin
	if (deriv != NULL)
	    call mfree (deriv, TY_DOUBLE)
	if (diffbuf != NULL)
	    call mfree (diffbuf, TY_DOUBLE)
	return
end
# Nov 20 1991	Free x and y before malloc'ing them
# Dec  4 1991	Eliminate unused npoly1
# Dec  6 1991	Pass initial values as arguments to minifit
#		instead of computing them; print final values if debug

# Nov 19 1992	Allow a polynomial fits from none to 4th order
#		add one more point to each section for initial polynomial fit
# Nov 24 1992	Add errors to debug listing

# Apr 13 1994	Change DCS return line after ftnchek
# Apr 22 1994	Do not pass spectrum length to GAUSS, DGAUSS, and D2GAUS
# Apr 29 1994	Use stack pointer for GMSPFIT work vectors
# Apr 29 1994	Add subroutine to free pointers
# May  2 1994	Comment out DHH as it always returns 0
# May 20 1994	Set initial background constant as mean of end pixels
# May 25 1994	Add parameter limits
# May 27 1994	Increase parameter limits
# Jun  6 1994	Set error flag to 3 if parameter limts are exceeded
# Jun  7 1994	If constant continuum, set initial value to 0
# Jun 15 1994	Clean up code
# Nov 16 1994	Remove data statement for IBM AIX

# Feb 15 1995	Check for divide by zero; return 0 if negative sigma
# Jul 13 1995	Fix array subscripts in 2nd derivative subroutines