include <fset.h>
include <error.h>
include <mach.h>
include <math/gsurfit.h>
include	<foc.h>
include	"fitgmap.h"
include	"fitmap.h"

define	GM_REAL		1	# computation type is real
define	GM_DOUBLE	2	# computation type is double

# T_RFITX -- Calculates the transformation required to transform
# the coordinate system of a reference image to the coordinate system of
# an input image. The transformation is of the following form.
#
#		xin = f (xref, yref)
#		yin = g (xref, yref)
# D. Giaretta, 01-Jun-87	Original code
# D. Giaretta, 15-Apr-88	Correct xmin etc usage

procedure t_rfitx ()


char	input1[SZ_FNAME]		# input res file
char	input2[SZ_FNAME]		# optional ref reseau file
char	output[SZ_FNAME]		# output file
char	entry[SZ_R_PATTERN]		# reseau entries to use
char	refentry[SZ_R_PATTERN]		# reference reseau entry
real	xmin, xmax, ymin, ymax		# minimum and maximum reference values
int	function			# fitting function
int	xxorder, xyorder, xxterms	# x fit fitting parameters
int	yxorder, yyorder, yxterms	# y fit fitting parameters
real	reject				# number of sigma of rejection
bool	interactive			# interactive graphics
char	device[SZ_FNAME]		# graphics device
#--

pointer	in, out, fit, gd
pointer	rs_open, rs_template(), rlist
pointer	rp,  respt
pointer	rpref, refpt
pointer	dtmap(), gopen()
real	clgetr()
int	clgeti(), btoi(), clgwrd(), nowhite()
int	rs_readr(), rs_list()
bool	rs_check(), clgetb()
char	str[SZ_LINE]

errchk	rs_open, rs_template, rs_readr, rs_list

begin

	rp   = NULL
	call salloc(in, 2, TY_STRUCT)

	# get infile1 and check for blanks
	call clgstr( "input1", input1, SZ_FNAME)
	if ( nowhite( input1,  input1, SZ_FNAME) == 0 )
	    call error( 0, "input file must be given")
	else
	    # open the files, first infile1
	    rp = rs_open( input1, READ_ONLY, 0) 

	# input2 if not blank - otherwise get from infile1
	call clgstr( "input2", input2, SZ_FNAME)
	if ( nowhite( input2,  input2, SZ_FNAME) != 0) {
	    rpref = rs_open( input2, READ_ONLY, 0) 
	    # check if grid sizes the same
	    if (!rs_check(rpref, RES_NROWS(rp), RES_NCOLS(rp)) )
		call error (0, "infile1 and input2 grid sizes must match")
	} else {
	# if no input2, set ref. pointer to infile1
	    rpref = rp
	}
	
	# get entry defs
	call clgstr( "entry",       entry, SZ_R_PATTERN)
	call clgstr( "refentry", refentry, SZ_R_PATTERN)

	# open database output file
	call clgstr ("output", output, SZ_FNAME)
	out = dtmap (output, APPEND)

	# minimum and maximum reference values
	xmin = clgetr ("xmin")
	xmax = clgetr ("xmax")
	ymin = clgetr ("ymin")
	ymax = clgetr ("ymax")

	# surface fitting parameters
	function = clgwrd ("function", str, SZ_LINE,
	    ",chebyshev,legendre,polynomial,")
	xxorder = clgeti ("xxorder")
	xyorder = clgeti ("xyorder")
	xxterms = btoi (clgetb ("xxterms"))
	yxorder = clgeti ("yxorder")
	yyorder = clgeti ("yyorder")
	yxterms = btoi (clgetb ("yxterms"))
	reject = clgetr ("reject")
####	calctype = clgwrd ("calctype", str, SZ_LINE, ",real,double,")

	# graphics parameters
	interactive = clgetb ("interactive")
	call clgstr ("device", device, SZ_FNAME)

	# flush standard output on newline
	call fseti (STDOUT, F_FLUSHNL, YES)

	# initialize the fit structure
	call fitminit (fit, function, xxorder, xyorder, xxterms,
	    yxorder, yyorder, yxterms, reject)

	# open graphics stream
	if (interactive)
	    gd = gopen (device, NEW_FILE, STDGRAPH)
	else
	    gd = NULL

	# allocate space for reference 
	call rs_alloc( rpref, TY_REAL, refpt)
	# allocate space for reseau
	call rs_alloc( rp,    TY_REAL, respt)

	# read reference
	rlist = rs_template( rpref, refentry)
	if ( rs_readr ( rpref, refpt, EOS, rs_list( rlist ) ) == 
							RES_F_NONEXTENTRY )
	    call error ( 0, " cannot read reference entry ")

	# expand reseau template
	rlist = rs_template( rp, entry)

	# process until we reach the end of list
	while ( rs_readr( rp, respt, EOS, rs_list( rlist) ) != 
							RES_F_NONEXTENTRY ) {

	    RES_DATA(in) = respt
	    REF_DATA(in) = refpt

	    # set entry name in structure
	    call strcpy (RES_ENT_ENTRY(respt), GM_NAME(fit), SZ_FNAME)

	    iferr { 
	        call rfitx (in, out, fit, gd, xmin, xmax, ymin, ymax)
	    } then {
		call eprintf ("Cannot process coordinate list: %s\n")
		    call pargstr ( RES_ENT_ENTRY(respt) )
		call erract (EA_WARN)
	    }

	}

	# close up
	call fitfree (fit)
	if (gd != NULL)
	    call gclose (gd)
	call dtunmap (out)
	call rs_close ( rp )
	call rs_close ( rpref )

end

# RFITX -- calculate the coordinate transformations

procedure rfitx (in, out, fit, gd, xmin, xmax, ymin, ymax)

pointer	in			# pointer to input data
pointer	out			# pointer to output file
pointer	fit			# pointer to fit parameters
pointer	gd			# graphics stream pointer
real	xmin, xmax		# max and min xref values
real	ymin, ymax		# max and min yref values
#--

int	npts, i
pointer	sp, xref, yref, xin, yin, wts
pointer	sx1, sy1, sx2, sy2

int	fit_npts(), fit_getpts()
real	asumr(), tmin, tmax

begin
	# if no data proceed to next file
	npts = fit_npts (in)
	if (npts == 0)
	    return

	# allocate space for data
	call smark (sp)
	call salloc (xref, npts, TY_REAL)
	call salloc (yref, npts, TY_REAL)
	call salloc (xin, npts, TY_REAL)
	call salloc (yin, npts, TY_REAL)
	call salloc (wts, npts, TY_REAL)

	# read in data and check that data is in range
	npts = fit_getpts (in, Memr[xref], Memr[yref], Memr[xin], Memr[yin],
	    xmin, xmax, ymin, ymax)
	if (npts == 0) {
	    call sfree (sp)
	    return
	}

	# prepare data for fitting by subtracting a zero point
	GM_XOREF(fit) 	= asumr (Memr[xref], npts) / npts
	GM_YOREF(fit) 	= asumr (Memr[yref], npts) / npts
	GM_XOIN(fit) 	= asumr (Memr[xin], npts) / npts
	GM_YOIN(fit) 	= asumr (Memr[yin], npts) / npts
	call amovkr (1., Memr[wts], npts)

	# determine x max and min
	if (IS_INDEFR(xmin) ) {
	    tmin = INDEFR
	    do i = 1, npts
		if ( IS_INDEFR(tmin) || 
		     ( !IS_INDEFR(Memr[xref+i-1]) && tmin > Memr[xref+i-1] ) )
		    tmin = Memr[xref+i-1]

	    GM_XMIN(fit) = tmin
	} else {
	    GM_XMIN(fit) = xmin
	}

	if (IS_INDEFR(xmax) ) {
	    tmax = INDEFR
	    do i = 1, npts
		if ( IS_INDEFR(tmax) || 
		     ( !IS_INDEFR(Memr[xref+i-1]) && tmax < Memr[xref+i-1] ) )
		    tmax = Memr[xref+i-1]

	    GM_XMAX(fit) = tmax
	} else {
	    GM_XMAX(fit) = xmax
	}

	# determine y max and min
	if (IS_INDEFR(ymin) ) {
	    tmin = INDEFR
	    do i = 1, npts
		if ( IS_INDEFR(tmin) || 
		     ( !IS_INDEFR(Memr[yref+i-1]) && tmin > Memr[yref+i-1] ) )
		    tmin = Memr[yref+i-1]

	    GM_YMIN(fit) = tmin
	} else {
	    GM_YMIN(fit) = ymin
	}

	if (IS_INDEFR(ymax) ) {
	    tmax = INDEFR
	    do i = 1, npts
		if ( IS_INDEFR(tmax) || 
		     ( !IS_INDEFR(Memr[yref+i-1]) && tmax < Memr[yref+i-1] ) )
		    tmax = Memr[yref+i-1]

	    GM_YMAX(fit) = tmax
	} else {
	    GM_YMAX(fit) = ymax
	}


	# initalize surface pointers
	sx1 = NULL
	sy1 = NULL
	sx2 = NULL
	sy2 = NULL

	# fit the data
	if (gd != NULL)
	    call fitmgfit (gd, fit, sx1, sy1, sx2, sy2, Memr[xref],
	        Memr[yref], Memr[xin], Memr[yin], Memr[wts], npts)
	else
	    call fitfit (fit, sx1, sy1, sx2, sy2, Memr[xref], Memr[yref],
		Memr[xin], Memr[yin], Memr[wts], npts)

	# output data
	call fitmout (fit, out, sx1, sy1, sx2, sy2)

	# free space and close files
	call fitmfree (sx1, sy1, sx2, sy2)
	call sfree (sp)
end

# FIT_NPTS -- find number of lines in file

int procedure fit_npts (fd)

pointer	fd

int	npts, i, num_data
pointer	resflag, refflag

begin
	npts = 0

	resflag = RES_FPT( RES_DATA(fd) )
	refflag = RES_FPT( REF_DATA(fd) )
	num_data = RES_ENT_NROWS(RES_DATA(fd))*RES_ENT_NCOLS(RES_DATA(fd))
	for ( i=1; i<= num_data; i=i+1) 
	    if ( !Memb[resflag + i-1] && !Memb[refflag +i-1] )
		npts = npts + 1
	
	return (npts)
end

# FIT_GETPTS -- fetch data points

int procedure fit_getpts (fd, xref, yref, xin, yin, xmin, xmax, ymin, ymax)

pointer	fd			# file descriptor
real	xref[ARB]		# x reference coordinates
real	yref[ARB]		# y reference coordinates
real	xin[ARB]		# x coordinates
real	yin[ARB]		# ycoordinates
real	xmin, xmax		# range of x coords
real	ymin, ymax		# range of y coords
#--
int	npts, num_data, i
pointer	res, ref
begin

	npts 		= 0
	res 		= RES_DATA(fd) 
	ref 		= REF_DATA(fd)
	num_data 	= RES_ENT_NROWS(res)*RES_ENT_NCOLS(res)
	
	for ( i=1; i<= num_data; i=i+1) {
	    if ( !Memb[RES_FPT(res) + i-1] && !Memb[RES_FPT(ref) +i-1] ) {
	
		xref[npts+1] = Memr[RES_XPT(ref) + i-1]
		yref[npts+1] = Memr[RES_YPT(ref) + i-1]
		xin[npts+1]  = Memr[RES_XPT(res) + i-1]
		yin[npts+1]  = Memr[RES_YPT(res) + i-1]
		
		if ( !IS_INDEFR(xmin) )
	            if (Memr[RES_XPT(ref)+i-1] < xmin )
		    	next

		if ( !IS_INDEFR(xmax) )
	            if (Memr[RES_XPT(ref)+i-1] > xmax )
		    	next

		if ( !IS_INDEFR(ymin) )
	            if (Memr[RES_YPT(ref)+i-1] < ymin )
		    	next

		if ( !IS_INDEFR(ymax) )
	            if (Memr[RES_YPT(ref)+i-1] > ymax )
		    	next
		
		npts = npts + 1

	    }

	}

	return (npts)
end


# FITMOUT -- write the output database file

procedure fitmout (fit, out, sx1, sy1, sx2, sy2)

pointer	fit		# pointer to fitting structure
int	out		# pointer to database file
pointer	sx1, sy1	# pointer to linear surfaces
pointer	sx2, sy2	# pointer to distortion surfaces

int	i, ncoeff
pointer	xcoeff, ycoeff
real	xshift, yshift, xscale, yscale, xrot, yrot

int	gsgeti()

begin
	# print title
	call dtptime (out)
	call dtput (out, "begin\t%s\n")
	    call pargstr (GM_NAME(fit))

	# output the geometric parameters
	call lincoeff (fit, sx1, sy1, xshift, yshift, xscale, yscale, xrot, 
	    yrot)
	call dtput (out, "\txrefmean\t%g\n")
	    call pargr (GM_XOREF(fit))
	call dtput (out, "\tyrefmean\t%g\n")
	    call pargr (GM_YOREF(fit))
	call dtput (out, "\txmean\t\t%g\n")
	    call pargr (GM_XOIN(fit))
	call dtput (out, "\tymean\t\t%g\n")
	    call pargr (GM_YOIN(fit))
	call dtput (out, "\txshift\t\t%g\n")
	    call pargr (xshift)
	call dtput (out, "\tyshift\t\t%g\n")
	    call pargr (yshift)
	call dtput (out, "\txscale\t\t%g\n")
	    call pargr (xscale)
	call dtput (out, "\tyscale\t\t%g\n")
	    call pargr (yscale)
	call dtput (out, "\txrotation\t%g\n")
	    call pargr (xrot)
	call dtput (out, "\tyrotation\t%g\n")
	    call pargr (yrot)

	# rebuild the linear coefficients
	call fitmkcof (sx1, sy1, xscale, yscale, xrot, yrot)

	# allocate memory for linear coefficients
	ncoeff = max (gsgeti (sx1, GSNSAVE), gsgeti (sy1, GSNSAVE))
	call calloc (xcoeff, ncoeff, TY_REAL)
	call calloc (ycoeff, ncoeff, TY_REAL)

	# output linear coefficients
	call gssave (sx1, Memr[xcoeff])
	call gssave (sy1, Memr[ycoeff])
	call dtput (out, "\tsurface1\t%d\n")
	    call pargi (ncoeff)
	do i = 1, ncoeff {
	    call dtput (out, "\t\t\t%g\t%g\n")
		call pargr (Memr[xcoeff+i-1])
		call pargr (Memr[ycoeff+i-1])
	}

	call mfree (xcoeff, TY_REAL)
	call mfree (ycoeff, TY_REAL)

	# allocate memory for higer order coefficients
	if (sx2 == NULL)
	    ncoeff = 0
	else
	    ncoeff = gsgeti (sx2, GSNSAVE)
	if (sy2 == NULL)
	    ncoeff = max (0, ncoeff)
	else
	    ncoeff = max (gsgeti (sy2, GSNSAVE), ncoeff)
	call calloc (xcoeff, ncoeff, TY_REAL)
	call calloc (ycoeff, ncoeff, TY_REAL)

	# save coefficients
	call gssave (sx2, Memr[xcoeff])
	call gssave (sy2, Memr[ycoeff])

	# output coefficients
	call dtput (out, "\tsurface2\t%d\n")
	    call pargi (ncoeff)
	do i = 1, ncoeff {
	    call dtput (out, "\t\t\t%g\t%g\n")
		call pargr (Memr[xcoeff+i-1])
		call pargr (Memr[ycoeff+i-1])
	}

	call mfree (xcoeff, TY_REAL)
	call mfree (ycoeff, TY_REAL)
end