include <math.h>
include <tbset.h>
include "fitxy.h"	# defines NV, XY_SAME, XY_OPPOSITE, etc.

# fitxy -- linear 2-D fit
# This task does a linear fit to V2,V3 coordinates as a function of X,Y
# coordinates.  The scales in x and y are constrained to be the same.
#
#	v2 = sign * cos(a) * x - sin(a) * y + v2_0
#	v3 = sign * sin(a) * x + cos(a) * y + v3_0
#
#	where sign is +1 if parity="same" and -1 if parity="opposite",
# 	and a is the angle of rotation from X,Y to V2,V3.
#
# The fit is evaluated, and the V2,V3 coordinates and residuals of the
# fit (input V2,V3 minus the fit) are printed to the standard output.
#
# Phil Hodge,  8-Sep-1992  Task created.
# Phil Hodge, 26-Jan-1994  Include parity and pdbinfo parameters, and
#			allow same or opposite parity for input coordinates.
# Phil Hodge, 18-Jul-1994  Include "Memc" for xytable & v2v3table in the
#			call to strcpy in case Memc[v2v3table] is blank.

procedure fitxy()

pointer xytable			# independent-variable values (X,Y pixels)
pointer v2v3table		# dependent-variable values (V2,V3 arcsec)
char	xcol[SZ_COLNAME]	# column names
char	ycol[SZ_COLNAME]
char	v2col[SZ_COLNAME]
char	v3col[SZ_COLNAME]
int	parity			# same or opposite parity
pointer sparity			# scratch for "same" or "opposite"
bool	verbose			# print results?
bool	pdbinfo			# also print PDB info?
#--
pointer sp
pointer xy, v2v3		# arrays of pixel, V2-V3 coords
double	coeff[NV]		# results of the fit:  c, s, v2_0, v3_0
double	sig[NV]			# standard deviations of fit coefficients
double	scale			# arcsec per pixel
double	angle			# angle between xy & v2v3
int	n			# number of elements in arrays of coords
int	clgwrd()
bool	clgetb(), isblank()

begin
	call smark (sp)
	call salloc (xytable, SZ_FNAME, TY_CHAR)
	call salloc (v2v3table, SZ_FNAME, TY_CHAR)
	call salloc (sparity, SZ_FNAME, TY_CHAR)

	call clgstr ("xytable", Memc[xytable], SZ_FNAME)	# X,Y
	call clgstr ("v2v3table", Memc[v2v3table], SZ_FNAME)	# V2,V3
	call clgstr ("xcol", xcol, SZ_COLNAME)
	call clgstr ("ycol", ycol, SZ_COLNAME)
	call clgstr ("v2col", v2col, SZ_COLNAME)
	call clgstr ("v3col", v3col, SZ_COLNAME)
	# The order of these values is defined in fitxy.h.
	parity = clgwrd ("parity", Memc[sparity], SZ_FNAME, "|same|opposite")
	verbose = clgetb ("verbose")
	pdbinfo = clgetb ("pdbinfo")

	if (isblank (Memc[v2v3table]))
	    # bug fix:  18-July-1994 PEH
	    call strcpy (Memc[xytable], Memc[v2v3table], SZ_FNAME)

	# Read data from input files.  Pointers v2v3 & xy are allocated here.
	call sr_read (Memc[xytable], Memc[v2v3table],
		xcol, ycol, v2col, v3col, xy, v2v3, n)

	# Solve for coeff.
	call sr_solve (Memd[xy], Memd[v2v3], n, parity,
		coeff, sig, scale, angle)

	# Evaluate & print differences.
	if (verbose)
	    call sr_print (Memd[xy], Memd[v2v3], n, parity, pdbinfo,
		coeff, sig, scale, angle)

	# Save the results in the parameter file.
	call clputd ("scale", scale)
	call clputd ("angle", angle)
	call clputd ("v2_0", coeff[3])
	call clputd ("v3_0", coeff[4])
	call clputd ("s_scale", sig[1])
	call clputd ("s_angle", sig[2])
	call clputd ("s_v2_0", sig[3])
	call clputd ("s_v3_0", sig[4])

	# Save some results in the parameter file for evalxy.
	call clpstr ("evalxy.parity", Memc[sparity])
	call clputd ("evalxy.scale", scale)
	call clputd ("evalxy.angle", angle)
	call clputd ("evalxy.v2_0", coeff[3])
	call clputd ("evalxy.v3_0", coeff[4])

	call mfree (v2v3, TY_DOUBLE)
	call mfree (xy, TY_DOUBLE)

	call sfree (sp)
end

procedure sr_read (xytable, v2v3table,
		xcol, ycol, v2col, v3col, xy, v2v3, n)

char	xytable[ARB]		# i: name of table of X,Y positions
char	v2v3table[ARB]		# i: name of table of V2,V3 positions
char	xcol[ARB]		# i: column name for X coordinates
char	ycol[ARB]		# i: column name for Y coordinates
char	v2col[ARB]		# i: column name for V2 coordinates
char	v3col[ARB]		# i: column name for V3 coordinates
pointer xy			# o: pointer to X,Y data
pointer v2v3			# o: pointer to V2,V3 data
int	n			# o: number of elements in arrays xy, v2v3
#--
pointer tp1, tp2		# pointers to table descriptors
pointer cpx, cpy		# column descriptors for X & Y
pointer cpv2, cpv3		# column descriptors for V2 & V3
double	x, y			# values read from input table
double	v2, v3
int	nrows			# number of rows in tables
int	row			# loop index
pointer tbtopn()
int	tbpsta()

begin
	tp1 = tbtopn (xytable, READ_ONLY, NULL)
	tp2 = tbtopn (v2v3table, READ_ONLY, NULL)

	nrows = min (tbpsta (tp1, TBL_NROWS), tbpsta (tp1, TBL_NROWS))

	call malloc (xy, 2*nrows, TY_DOUBLE)
	call malloc (v2v3, 2*nrows, TY_DOUBLE)

	# Find the columns.
	call tbcfnd (tp1, xcol, cpx, 1)
	call tbcfnd (tp1, ycol, cpy, 1)
	call tbcfnd (tp2, v2col, cpv2, 1)
	call tbcfnd (tp2, v3col, cpv3, 1)
	if (cpx == NULL || cpy == NULL)
	    call error (1, "column not found in xytable")
	if (cpv2 == NULL || cpv3 == NULL)
	    call error (1, "column not found in v2v3table")

	n = 0					# incremented in loop
	do row = 1, nrows {

	    call tbegtd (tp1, cpx, row, x)
	    call tbegtd (tp1, cpy, row, y)
	    call tbegtd (tp2, cpv2, row, v2)
	    call tbegtd (tp2, cpv3, row, v3)

	    if (IS_INDEFD(x) || IS_INDEFD(y) || IS_INDEFD(v2) || IS_INDEFD(v3))
		next

	    n = n + 1

	    Memd[xy + (n-1)*2] = x
	    Memd[xy + (n-1)*2 + 1] = y
	    Memd[v2v3 + (n-1)*2] = v2
	    Memd[v2v3 + (n-1)*2 + 1] = v3
	}

	call tbtclo (tp2)
	call tbtclo (tp1)

	if (n < NV/2) {
	    if (n < 1) {
		call eprintf ("no data at all\n")
	    } else {
		call eprintf ("only %d positions; you need at least %d\n")
		    call pargi (n)
		    call pargi (NV/2)
	    }
	    call error (1, "")
	}
end

# sr_solve -- solve the set of equations

procedure sr_solve (xy, v2v3, n, parity,
		coeff, sig, scale, angle)

double	v2v3[2,n]	# i: V2,V3 coordinates (1 is V2, 2 is V3)
double	xy[2,n]		# i: pixel coordinates (1 is X, 2 is Y)
int	n		# i: number of points (pairs of coordinates)
int	parity		# i: same or opposite
double	coeff[NV]	# o: results of the fit
double	sig[NV]		# o: standard deviations of the fit coefficients
double	scale		# o: arcsec per pixel
double	angle		# o: angle between xy & v2v3
#--
double	sx, sy, sx2, sy2
double	sv2, sv3
double	sxv2, sxv3, syv2, syv3
double	a[NV,NV], b[NV]		# matrix and vector
double	ai[NV,NV]		# inverse of A
int	indx[NV]		# row permutations by ludcmd
double	d			# returned by ludcmd
double	x, y			# a particular X,Y pair
double	v2, v3			# a particular V2,V3 pair
double	x_a, y_a		# average values of X,Y
double	v2_a, v3_a		# average values of V2,V3
double	rv2, rv3		# residuals:  observed - fit
double	sr2			# sum of squared residuals
double	sigs, siga		# temp for std dev of scale & angle
double	psign			# plus or minus one, for parity
int	istat			# error flag for ludcmd
int	j, k			# loop indexes

begin
	# Get average values of X,Y and V2,V3.
	sx   = 0.d0
	sy   = 0.d0
	sv2  = 0.d0
	sv3  = 0.d0
	do k = 1, n {
	    sx   = sx   + xy[1,k]
	    sy   = sy   + xy[2,k]
	    sv2  = sv2  + v2v3[1,k]
	    sv3  = sv3  + v2v3[2,k]
	}
	x_a = sx / n
	y_a = sy / n
	v2_a = sv2 / n
	v3_a = sv3 / n

	# Initialize.
	sx   = 0.d0		# sum of X
	sy   = 0.d0		# sum of Y
	sx2  = 0.d0		# sum of X**2
	sy2  = 0.d0		# sum of Y**2
	sv2  = 0.d0		# sum of V2
	sv3  = 0.d0		# sum of V3
	sxv2 = 0.d0		# sum of X * V2
	sxv3 = 0.d0		# sum of X * V3
	syv2 = 0.d0		# sum of Y * V2
	syv3 = 0.d0		# sum of Y * V3

	# Accumulate sums.
	do k = 1, n {
	    x = xy[1,k] - x_a
	    y = xy[2,k] - y_a
	    v2 = v2v3[1,k] - v2_a
	    v3 = v2v3[2,k] - v3_a
	    sx   = sx   + x
	    sy   = sy   + y
	    sx2  = sx2  + x**2
	    sy2  = sy2  + y**2
	    sv2  = sv2  + v2
	    sv3  = sv3  + v3
	    sxv2 = sxv2 + x * v2
	    sxv3 = sxv3 + x * v3
	    syv2 = syv2 + y * v2
	    syv3 = syv3 + y * v3
	}

	# Define normal matrix.
	if (parity == XY_SAME)
	    psign = 1.d0
	else if (parity == XY_OPPOSITE)
	    psign = -1.d0

	a[1,1] = sx2 + sy2
	a[1,2] = 0.d0
	a[1,3] = sx * psign
	a[1,4] = sy

	a[2,1] = 0.d0
	a[2,2] = sx2 + sy2
	a[2,3] = -sy
	a[2,4] = sx * psign

	a[3,1] = sx * psign
	a[3,2] = -sy
	a[3,3] = n
	a[3,4] = 0.d0

	a[4,1] = sy
	a[4,2] = sx * psign
	a[4,3] = 0.d0
	a[4,4] = n

	b[1] = sxv2 * psign + syv3
	b[2] = sxv3 * psign - syv2
	b[3] = sv2
	b[4] = sv3

	# Solve.  Use double-precision versions of Numerical Recipes routines.
	call ludcmd (a, NV, NV, indx, d, istat)	# a is overwritten
	if (istat != OK)
	    call error (1, "singular matrix")
	call lubksd (a, NV, NV, indx, b)	# b is the solution

	scale = sqrt (b[1]**2 + b[2]**2)
	angle = atan2 (b[2], b[1]) * RADIAN
	if (angle < 0.d0)
	    angle = angle + 360.d0

	# Evaluate the fit at X,Y = [0,0] in order to shift the zero point
	# offsets so they are relative to that point.  The coefficients in
	# the B array are appropriate for coordinates relative to the mean
	# values, so we use B here.
	call sr_eval (-x_a, -y_a, parity, b, v2, v3)

	# These are the output coefficients.
	COS_ANGLE(coeff) = COS_ANGLE(b)
	SIN_ANGLE(coeff) = SIN_ANGLE(b)
	V2_ZERO(coeff) = v2 + v2_a
	V3_ZERO(coeff) = v3 + v3_a

	# Get the uncertainties in the coefficients.
	if (n <= NV/2) {
	    do k = 1, NV
		sig[k] = 0.d0
	    return			# not enough data to get errors
	}

	# Get the sum of squared deviations from the fit.
	sr2 = 0.d0		# sum of squared residuals of V2 and V3
	do k = 1, n {
	    call sr_eval (xy[1,k], xy[2,k], parity, coeff, v2, v3)
	    rv2 = v2v3[1,k] - v2
	    rv3 = v2v3[2,k] - v3
	    sr2 = sr2 + rv2**2 + rv3**2
	}
	# Divide the sum of squared residuals by the total number of
	# data points (X + Y) minus the number of parameters determined
	# from the fit minus one.
	sr2 = sr2 / (2.d0 * n - NV - 1.d0)

	# Compute the inverse of A.
	do k = 1, NV {
	    do j = 1, NV
		ai[j,k] = 0.d0
	    ai[k,k] = 1.d0
	}
	do k = 1, NV
	    call lubksd (a, NV, NV, indx, ai[1,k])

	# AI is now the inverse of A.  Compute the standard deviations.
	do k = 1, NV
	    sig[k] = sqrt (sr2 * ai[k,k])

	# Convert the standard deviations of the cosine & sine coefficients
	# to scale and angle.
	sigs = sqrt ((COS_ANGLE(coeff) * sig[1])**2 +
		     (SIN_ANGLE(coeff) * sig[2])**2) / scale
	siga = sqrt ((COS_ANGLE(coeff) * sig[2])**2 +
		     (SIN_ANGLE(coeff) * sig[1])**2) / scale**2
	sig[1] = sigs
	sig[2] = RADTODEG(siga)
end

# sr_print -- evaluate fit
# This routine evaluates the fit and prints the V2,V3 coordinates and the
# residuals of the fit.  The residuals are the observed minus the fit.

procedure sr_print (xy, v2v3, n, parity, pdbinfo,
		coeff, sig, scale, angle)

double	xy[2,n]		# i: pixel coordinates (1 is x, 2 is y)
double	v2v3[2,n]	# i: V2,V3 coordinates (1 is V2, 2 is V3)
int	n		# i: number of pairs of coordinates
int	parity		# i: same or opposite
bool	pdbinfo		# i: also print PDB info?
double	coeff[NV]	# i: results of the fit
double	sig[NV]		# i: standard deviations of the fit coefficients
double	scale		# i: arcsec per pixel
double	angle		# i: angle between xy & v2v3
#--
double	v2, v3		# the fit evaluated for a given xy pair
double	sumv2s, sumv3s	# for getting RMS of residuals
double	rv2, rv3	# residuals in v2, v3
double	maxv2, maxv3	# max residuals
double	pdb_0[NV]	# zero points for PDB coordinates
int	k		# loop index

begin
	sumv2s = 0.d0
	sumv3s = 0.d0
	maxv2 = 0.d0
	maxv3 = 0.d0
	call printf (
	"#          observed V2 & V3        residual (observed minus fit)\n")
	
	do k = 1, n {
	    call sr_eval (xy[1,k], xy[2,k], parity, coeff, v2, v3)
	    rv2 = v2v3[1,k] - v2
	    rv3 = v2v3[2,k] - v3

	    call printf ("%15.7g %15.7g %15.7g %15.7g\n")
		call pargd (v2v3[1,k])
		call pargd (v2v3[2,k])
		call pargd (rv2)
		call pargd (rv3)

	    sumv2s = sumv2s + rv2**2
	    sumv3s = sumv3s + rv3**2
	    maxv2 = max (maxv2, abs (rv2))
	    maxv3 = max (maxv3, abs (rv3))
	}

	# Evaluate at the four corners also, to define plot limits.
	if (pdbinfo) {
	    call printf ("# the fit evaluated at the four corners:\n")
	    call sr_eval (1.d0, 1.d0, parity, coeff, v2, v3)
	    call printf ("%15.7g %15.7g    0.   0.     # [1,1]\n")
		call pargd (v2)
		call pargd (v3)
	    call sr_eval (1024.d0, 1.d0, parity, coeff, v2, v3)
	    call printf ("%15.7g %15.7g    0.   0.     # [1024,1]\n")
		call pargd (v2)
		call pargd (v3)
	    call sr_eval (1.d0, 1024.d0, parity, coeff, v2, v3)
	    call printf ("%15.7g %15.7g    0.   0.     # [1,1024]\n")
		call pargd (v2)
		call pargd (v3)
	    call sr_eval (1024.d0, 1024.d0, parity, coeff, v2, v3)
	    call printf ("%15.7g %15.7g    0.   0.     # [1024,1024]\n")
		call pargd (v2)
		call pargd (v3)
	}

	call printf ("\n")
	call printf ("# scale =%8.6f +/- %8.6f arcsec/pixel\n")
	    call pargd (scale)
	    call pargd (sig[1])
	call printf ("# angle =%8.3f +/- %8.3f degrees\n")
	    call pargd (angle)
	    call pargd (sig[2])
	call printf ("# v2_0 = %8.3f +/- %8.3f arcsec\n")
	    call pargd (coeff[3])
	    call pargd (sig[3])
	call printf ("# v3_0 = %8.3f +/- %8.3f arcsec\n")
	    call pargd (coeff[4])
	    call pargd (sig[4])

	# Print the matrix coefficients.  These are for X,Y photocathode
	# coordinates, so the signs of the first column of the matrix
	# are changed, and the zero point is the location of image
	# coordinates [X, Y] = [1025, 0].
	if (pdbinfo) {
	    call sr_eval (1025.d0, 0.d0, parity, coeff, pdb_0[1], pdb_0[2])
	    call printf ("\n")
	    call printf (
		"# PDB matrix coefficients:  a  b  v2_0;  c  d  v3_0\n")
	    call printf ("# %15.7g %15.7g %15.7g\n")
		call pargd (coeff[1])
		call pargd (-coeff[2])
		call pargd (pdb_0[1])
	    call printf ("# %15.7g %15.7g %15.7g\n")
		call pargd (coeff[2])
		call pargd (coeff[1])
		call pargd (pdb_0[2])
	}

	# Compute and print the averages.
	sumv2s = sqrt (sumv2s / double(n))
	sumv3s = sqrt (sumv3s / double(n))

	call printf ("\n")
	call printf ("# residuals:       V2              V3\n")
	call printf ("# RMS  %15.7g %15.7g arcsec\n")
	    call pargd (sumv2s)
	    call pargd (sumv3s)
	call printf ("# RMS  %15.7g %15.7g pixels\n")
	    call pargd (sumv2s / scale)
	    call pargd (sumv3s / scale)
	call printf ("# max  %15.7g %15.7g arcsec\n")
	    call pargd (maxv2)
	    call pargd (maxv3)
	call printf ("# max  %15.7g %15.7g pixels\n")
	    call pargd (maxv2 / scale)
	    call pargd (maxv3 / scale)
end

# sr_eval -- evaluate the fit at one point
# This routine simply evaluates the fit for one X,Y point, giving V2,V3.
# It's purpose is to ensure consistency in the use of the coefficients,
# especially in the signs.  This routine is also used by evalxy.

procedure sr_eval (x, y, parity, coeff, v2, v3)

double	x, y		# i: pixel coordinates
int	parity		# i: same or opposite
double	coeff[NV]	# i: coefficients of fit
double	v2, v3		# o: V2, V3 location (arcsec) corresponding to X, Y

begin
	if (parity == XY_SAME) {
	    v2 = coeff[1] * x - coeff[2] * y + coeff[3]
	    v3 = coeff[2] * x + coeff[1] * y + coeff[4]
	} else if (parity == XY_OPPOSITE) {
	    v2 = -coeff[1] * x - coeff[2] * y + coeff[3]
	    v3 = -coeff[2] * x + coeff[1] * y + coeff[4]
	} else {
	    call error (1, "unknown value of 'parity'")
	}
end