include <math/gsurfit.h>
include <foc.h>

define	LEFT	$1[1]		# extend left border?
define	RIGHT	$1[2]		# extend right border?
define	TOP	$1[3]		# extend top border?
define	BOTTOM	$1[4]		# extend bottom border?

# rextrap -- extrapolate reseau grid
# This task extrapolates a reseau grid, extending it by one row or column
# on any or all sides.  The "tracking" is updated with the letter E.
#
# Phil Hodge,  5-Nov-1990  Task created
# Phil Hodge, 31-Mar-1992  Open input table read-only instead of read-write.
# Phil Hodge, 17-Nov-1993  Initialize EXTEND to false.

procedure rextrap()

int	ilist			# for list of input tables
char	entry[SZ_FNAME]		# entry name
int	olist			# for list of output tables
char	sides[SZ_FNAME]		# extrapolate which sides?  (r, l, t, b)
bool	verbose			# print table names?
#--
pointer sp
pointer irp, orp		# pointers to reseau table structs
pointer rlist			# list of reseau entries
pointer ient, oent		# reseau entry pointers
char	inres[SZ_FNAME]		# name of an input reseau table
char	outres[SZ_FNAME]	# name of an output reseau table
int	ent_num			# entry number in input
int	inx, iny		# size of input reseau grid
int	onx, ony		# size of output reseau grid
int	junk
int	k
bool	extend[4]		# extend which sides?
char	lchar, rchar, tchar, bchar	# 'l', 'r', 't', 'b'
int	clpopns(), clgfil(), clplen()
int	strlen()
bool	clgetb()
pointer rs_open(), rs_template()
int	rs_readr(), rs_list()
define	next_table_	91

begin
	ilist = clpopns ("inres")
	call clgstr ("entry", entry, SZ_FNAME)
	olist = clpopns ("outres")
	call clgstr ("sides", sides, SZ_FNAME)
	verbose = clgetb ("verbose")

	if (clplen (ilist) != clplen (olist))
	    call error (1, "input & output list not the same length")

	call strlwr (sides)
	lchar = 'l'
	rchar = 'r'
	tchar = 't'
	bchar = 'b'
	LEFT(extend) = false		# initial values
	RIGHT(extend) = false
	TOP(extend) = false
	BOTTOM(extend) = false
	do k = 1, strlen (sides) {
	    if (sides[k] == lchar)
		LEFT(extend) = true
	    else if (sides[k] == rchar)
		RIGHT(extend) = true
	    else if (sides[k] == tchar)
		TOP(extend) = true
	    else if (sides[k] == bchar)
		BOTTOM(extend) = true
	    else
		call error (1, "unrecognized character in sides parameter")
	}

	# Loop over each table in list.
	while (clgfil (ilist, inres, SZ_FNAME) != EOF) {
	    junk = clgfil (olist, outres, SZ_FNAME)

	    call smark (sp)

	    # Open input reseau table.
	    iferr {
		irp = rs_open (inres, READ_ONLY, NULL)
	    } then {
		call eprintf ("%s is not a reseau table\n")
		    call pargstr (inres)
		goto next_table_
	    }

	    if (verbose) {
		call printf ("%s --> %s\n")
		    call pargstr (inres)
		    call pargstr (outres)
		call flush (STDOUT)
	    }

	    # Allocate space for an input entry.
	    call rs_alloc (irp, TY_REAL, ient)

	    # Get the sizes of the input & output grids.
	    inx = RES_NCOLS(irp)
	    iny = RES_NROWS(irp)
	    onx = inx
	    ony = iny
	    if (LEFT(extend))
		onx = onx + 1
	    if (RIGHT(extend))
		onx = onx + 1
	    if (TOP(extend))
		ony = ony + 1
	    if (BOTTOM(extend))
		ony = ony + 1

	    # Open output reseau table.
	    orp = rs_open (outres, NEW_FILE, NULL)
	    # Specify size of output, and create it.
	    call rs_new (1, 1, ony, onx, TY_REAL, orp)

	    # Allocate space for an output entry.
	    call rs_alloc (orp, TY_REAL, oent)

	    rlist = rs_template (irp, entry)		# process these entries

	    # Do for each reseau entry in current table.
	    ent_num = rs_list (rlist)
	    while (rs_readr (irp, ient, "", ent_num) != RES_F_NONEXTENTRY) {

		# Fill in values for info columns.
		call rext_upd (orp, ient, oent)

		# Copy the input grid into the interior of the output grid.
		call rext_copy (Memr[RES_XPT(ient)], Memr[RES_YPT(ient)],
			Memb[RES_FPT(ient)],
			Memr[RES_XPT(oent)], Memr[RES_YPT(oent)],
			Memb[RES_FPT(oent)], inx, iny, onx, ony, extend)

		# Extrapolate the output grid.
		if (inx < 3 || iny < 3) {
		    call rext_small (Memr[RES_XPT(oent)], Memr[RES_YPT(oent)],
			Memb[RES_FPT(oent)], onx, ony, extend)
		} else {
		    call rext_big (Memr[RES_XPT(oent)], Memr[RES_YPT(oent)],
			Memb[RES_FPT(oent)], onx, ony, extend)
		}

		# Save entry in output reseau table.
		call rs_write (orp, oent, 0)

		ent_num = rs_list (rlist)		# get next number
	    }

	    call rs_close (orp)
	    call rs_close (irp)

next_table_
	    call sfree (sp)
	}

	call clpcls (olist)
	call clpcls (ilist)
end

# rext_copy -- copy to middle of output
# This routine copies the input grid to the interior of the output grid.
# The array ext specifies which are the extra sides of the output grid.
# The values in the extra sides are not assigned.

procedure rext_copy (xin, yin, fin, xout, yout, fout,
		inx, iny, onx, ony, extend)

real	xin[inx,iny]		# i: x coordinates of input reseau marks
real	yin[inx,iny]		# i: y coordinates of input reseau marks
bool	fin[inx,iny]		# i: flag -- is reseau indef?
real	xout[onx,ony]		# o: x coordinates of output reseau marks
real	yout[onx,ony]		# o: y coordinates of output reseau marks
bool	fout[onx,ony]		# o: flag -- is reseau indef?
int	inx, iny		# i: size of input reseau grid
int	onx, ony		# i: size of output reseau grid
bool	extend[ARB]		# i: extend which sides?
#--
int	xs, ys			# starting indexes for x & y in output
int	i, j			# indexes in input arrays
int	io, jo			# indexes in output arrays

begin
	# Initialize.
	do j = 1, ony {
	    do i = 1, onx {
		xout[i,j] = INDEF
		yout[i,j] = INDEF
		fout[i,j] = true		# yes, it's INDEF
	    }
	}

	# Set values for starting loop indexes.
	if (LEFT(extend))
	    xs = 2
	else
	    xs = 1

	if (BOTTOM(extend))
	    ys = 2
	else
	    ys = 1

	jo = ys
	do j = 1, iny {
	    io = xs
	    do i = 1, inx {
		xout[io,jo] = xin[i,j]
		yout[io,jo] = yin[i,j]
		fout[io,jo] = fin[i,j]
		io = io + 1
	    }
	    jo = jo + 1
	}
end

# rext_big -- extrapolate the output grid
# This routine fits a plane to a small set of reseau and evaluates the
# fit in order to extrapolate each point along each edge.  The corners
# are filled in the same way.  The X and Y coordinates are the integer
# values 1, 2, or 3.

procedure rext_big (xout, yout, fout, onx, ony, extend)

real	xout[onx,ony]		# io: x coordinates of output reseau marks
real	yout[onx,ony]		# io: y coordinates
bool	fout[onx,ony]		# o: flag -- is reseau indef?
int	onx, ony		# i: size of output reseau grid
bool	extend[ARB]		# i: extend which sides?
#--
pointer sfx, sfy		# surface fit structures for x & y
real	x, y, w			# arguments for gsaccum
int	xs, xe, ys, ye		# start & end indexes for x & y
int	i, j
int	row, col		# loop indexes
int	ierx, iery		# error return codes
real	gseval()

begin
	# Set values for starting and ending row & col loop indexes.
	# These are the indexes in the output arrays of the middle reseau
	# of a three-element-wide box that must be entirely within the
	# range of values from the input arrays.
	if (LEFT(extend))
	    xs = 3
	else
	    xs = 2

	if (RIGHT(extend))
	    xe = onx-2
	else
	    xe = onx-1

	if (BOTTOM(extend))
	    ys = 3
	else
	    ys = 2

	if (TOP(extend))
	    ye = ony-2
	else
	    ye = ony-1

	# Extrapolate each side separately.
	if (LEFT(extend)) {

	    do row = ys, ye {

		# Initialize for fitting a plane.
		call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
		call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)

		do j = row-1, row+1 {
		    y = j - row + 2		# so values are 1, 2, 3

		    # Fit to first two columns that already exist.
		    do i = 2, 3 {
			x = i
			if (!fout[i,j]) {
			    call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
			    call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
			}
		    }
		}
		call gssolve (sfx, ierx)
		call gssolve (sfy, iery)
		if (ierx == OK && iery == OK) {
		    xout[1,row] = gseval (sfx, 1., 2.)
		    yout[1,row] = gseval (sfy, 1., 2.)
		    fout[1,row] = false

		    if (row == ys) {
			xout[1,row-1] = gseval (sfx, 1., 1.)
			yout[1,row-1] = gseval (sfy, 1., 1.)
			fout[1,row-1] = false
		    }
		    if (row == ye) {
			xout[1,row+1] = gseval (sfx, 1., 3.)
			yout[1,row+1] = gseval (sfy, 1., 3.)
			fout[1,row+1] = false
		    }
		}

		call gsfree (sfy)
		call gsfree (sfx)
	    }
	}

	if (RIGHT(extend)) {	    	# extend right side

	    do row = ys, ye {

		call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
		call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)

		do j = row-1, row+1 {
		    y = j - row + 2		# so values are 1, 2, 3

		    # Fit to last two columns that already exist.
		    do i = onx-2, onx-1 {
			x = i - onx + 3.	# so values are 1 and 2
			if (!fout[i,j]) {
			    call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
			    call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
			}
		    }
		}
		call gssolve (sfx, ierx)
		call gssolve (sfy, iery)
		if (ierx == OK && iery == OK) {
		    xout[onx,row] = gseval (sfx, 3., 2.)
		    yout[onx,row] = gseval (sfy, 3., 2.)
		    fout[onx,row] = false

		    if (row == ys) {
			xout[onx,row-1] = gseval (sfx, 3., 1.)
			yout[onx,row-1] = gseval (sfy, 3., 1.)
			fout[onx,row-1] = false
		    }
		    if (row == ye) {
			xout[onx,row+1] = gseval (sfx, 3., 3.)
			yout[onx,row+1] = gseval (sfy, 3., 3.)
			fout[onx,row+1] = false
		    }
		}

		call gsfree (sfy)
		call gsfree (sfx)
	    }
	}

	if (TOP(extend)) {	    	# extend top side

	    do col = xs, xe {

		call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
		call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)

		do j = ony-2, ony-1 {
		    y = j - ony + 3		# so values are 1 and 2

		    do i = col-1, col+1 {
			x = i - col + 2		# so values are 1, 2, 3
			if (!fout[i,j]) {
			    call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
			    call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
			}
		    }
		}
		call gssolve (sfx, ierx)
		call gssolve (sfy, iery)
		if (ierx == OK && iery == OK) {
		    xout[col,ony] = gseval (sfx, 2., 3.)
		    yout[col,ony] = gseval (sfy, 2., 3.)
		    fout[col,ony] = false

		    if (col == xs) {
			xout[col-1,ony] = gseval (sfx, 1., 3.)
			yout[col-1,ony] = gseval (sfy, 1., 3.)
			fout[col-1,ony] = false
		    }
		    if (col == xe) {
			xout[col+1,ony] = gseval (sfx, 3., 3.)
			yout[col+1,ony] = gseval (sfy, 3., 3.)
			fout[col+1,ony] = false
		    }
		}

		call gsfree (sfy)
		call gsfree (sfx)
	    }
	}

	if (BOTTOM(extend)) {	    	# extend bottom side

	    do col = xs, xe {

		call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
		call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)

		do j = 2, 3 {
		    y = j

		    do i = col-1, col+1 {
			x = i - col + 2		# so values are 1, 2, 3
			if (!fout[i,j]) {
			    call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
			    call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
			}
		    }
		}
		call gssolve (sfx, ierx)
		call gssolve (sfy, iery)
		if (ierx == OK && iery == OK) {
		    xout[col,1] = gseval (sfx, 2., 1.)
		    yout[col,1] = gseval (sfy, 2., 1.)
		    fout[col,1] = false

		    if (col == xs) {
			xout[col-1,1] = gseval (sfx, 1., 1.)
			yout[col-1,1] = gseval (sfy, 1., 1.)
			fout[col-1,1] = false
		    }
		    if (col == xe) {
			xout[col+1,1] = gseval (sfx, 3., 1.)
			yout[col+1,1] = gseval (sfy, 3., 1.)
			fout[col+1,1] = false
		    }
		}

		call gsfree (sfy)
		call gsfree (sfx)
	    }
	}

	# Now fill in the corners.
	if (LEFT(extend) && BOTTOM(extend)) {
	    # Fit to the 3 x 3 box in the lower left corner.  Only four of
	    # these values are from the input reseau table; four others were
	    # obtained earlier by extrapolation, and the last is the corner.
	    call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
	    call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
	    do j = 1, 3 {
		y = j
		do i = 1, 3 {
		    x = i
		    # We don't want to include [1,1], but the test on fout
		    # should take care of that.
		    if (!fout[i,j]) {
			call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
			call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
		    }
		}
	    }
	    call gssolve (sfx, ierx)
	    call gssolve (sfy, iery)
	    if (ierx == OK && iery == OK) {
		xout[1,1] = gseval (sfx, 1., 1.)
		yout[1,1] = gseval (sfy, 1., 1.)
		fout[1,1] = false
	    }
	    call gsfree (sfy)
	    call gsfree (sfx)
	}

	if (LEFT(extend) && TOP(extend)) {
	    call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
	    call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
	    do j = ony-2, ony {
		y = j - ony + 3
		do i = 1, 3 {
		    x = i
		    if (!fout[i,j]) {
			call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
			call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
		    }
		}
	    }
	    call gssolve (sfx, ierx)
	    call gssolve (sfy, iery)
	    if (ierx == OK && iery == OK) {
		xout[1,ony] = gseval (sfx, 1., 3.)
		yout[1,ony] = gseval (sfy, 1., 3.)
		fout[1,ony] = false
	    }
	    call gsfree (sfy)
	    call gsfree (sfx)
	}

	if (RIGHT(extend) && TOP(extend)) {
	    call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
	    call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
	    do j = ony-2, ony {
		y = j - ony + 3
		do i = onx-2, onx {
		    x = i - onx + 3
		    if (!fout[i,j]) {
			call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
			call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
		    }
		}
	    }
	    call gssolve (sfx, ierx)
	    call gssolve (sfy, iery)
	    if (ierx == OK && iery == OK) {
		xout[onx,ony] = gseval (sfx, 3., 3.)
		yout[onx,ony] = gseval (sfy, 3., 3.)
		fout[onx,ony] = false
	    }
	    call gsfree (sfy)
	    call gsfree (sfx)
	}

	if (RIGHT(extend) && BOTTOM(extend)) {
	    call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
	    call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., 3., 1., 3.)
	    do j = 1, 3 {
		y = j
		do i = onx-2, onx {
		    x = i - onx + 3
		    if (!fout[i,j]) {
			call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
			call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
		    }
		}
	    }
	    call gssolve (sfx, ierx)
	    call gssolve (sfy, iery)
	    if (ierx == OK && iery == OK) {
		xout[onx,1] = gseval (sfx, 3., 1.)
		yout[onx,1] = gseval (sfy, 3., 1.)
		fout[onx,1] = false
	    }
	    call gsfree (sfy)
	    call gsfree (sfx)
	}
end

# rext_small -- extrapolate the output grid
# This routine fits a plane to all the reseau (a small set) and evaluates
# the fit in order to extrapolate around the edges.

procedure rext_small (xout, yout, fout, onx, ony, extend)

real	xout[onx,ony]		# io: x coordinates of output reseau marks
real	yout[onx,ony]		# io: y coordinates
bool	fout[onx,ony]		# o: flag -- is reseau indef?
int	onx, ony		# i: size of output reseau grid
bool	extend[ARB]		# i: extend which sides?
#--
pointer sfx, sfy		# surface fit structures for x & y
real	rnx, rny		# onx, ony as real numbers
real	x, y, w			# arguments for gsaccum
int	i, j
int	ierx, iery		# error return codes
real	gseval()

begin
	rnx = onx
	rny = ony

	# Initialize for fitting a plane over entire output grid.
	call gsinit (sfx, GS_POLYNOMIAL, 2, 2, NO, 1., rnx, 1., rny)
	call gsinit (sfy, GS_POLYNOMIAL, 2, 2, NO, 1., rnx, 1., rny)

	# Fit the plane.
	do j = 1, ony {
	    y = j
	    do i = 1, onx {
		x = i
		# This flag will keep us from fitting to points that have
		# not been assigned, i.e. ones not in the input grid.
		if (!fout[i,j]) {
		    call gsaccum (sfx, x, y, xout[i,j], w, WTS_UNIFORM)
		    call gsaccum (sfy, x, y, yout[i,j], w, WTS_UNIFORM)
		}
	    }
	}
	call gssolve (sfx, ierx)
	call gssolve (sfy, iery)
	if (ierx == OK && iery == OK) {
	    if (LEFT(extend)) {
		x = 1.
		do j = 1, ony {
		    y = j
		    xout[1,j] = gseval (sfx, x, y)
		    yout[1,j] = gseval (sfy, x, y)
		    fout[1,j] = false
		}
	    }
	    if (RIGHT(extend)) {
		x = onx
		do j = 1, ony {
		    y = j
		    xout[onx,j] = gseval (sfx, x, y)
		    yout[onx,j] = gseval (sfy, x, y)
		    fout[onx,j] = false
		}
	    }
	    if (BOTTOM(extend)) {
		y = 1.
		do i = 1, onx {
		# Corners are evaluated twice, but the answer is the same.
		    x = i
		    xout[i,1] = gseval (sfx, x, y)
		    yout[i,1] = gseval (sfy, x, y)
		    fout[i,1] = false
		}
	    }
	    if (TOP(extend)) {
		y = ony
		do i = 1, onx {
		    x = i
		    xout[i,ony] = gseval (sfx, x, y)
		    yout[i,ony] = gseval (sfy, x, y)
		    fout[i,ony] = false
		}
	    }
	}
	call gsfree (sfy)
	call gsfree (sfx)
end

# rext_upd -- update info columns

procedure rext_upd (orp, ient, oent)

pointer	orp		# i: res pointer for output table
pointer ient		# i: entry pointer for input table
pointer	oent		# i: entry pointer for output table
#--
long	clktime()

begin
	call strcpy (RES_ENT_ENTRY(ient), RES_ENT_ENTRY(oent), SZ_R_ENTRY)

	# Copy tracking info and append an E for extrapolation.
	call strcpy (RES_ENT_TRACKING(ient), RES_ENT_TRACKING(oent),
		SZ_R_TRACKING)
	call strcat ("E", RES_ENT_TRACKING(oent), SZ_R_TRACKING)

	RES_ENT_DATE(oent) = clktime (0)
	RES_ENT_NROWS(oent) = RES_NROWS(orp)
	RES_ENT_NCOLS(oent) = RES_NCOLS(orp)

	RES_ENT_SFIELD1(oent) = RES_ENT_SFIELD1(ient)
	RES_ENT_SFIELD2(oent) = RES_ENT_SFIELD2(ient)
	RES_ENT_SFIELD3(oent) = RES_ENT_SFIELD3(ient)
	RES_ENT_SFIELD4(oent) = RES_ENT_SFIELD4(ient)
end