# Copyright(c) 1986 Association of Universities for Research in Astronomy Inc.

include <error.h>
include <math.h>
include <math/gsurfit.h>
include <geomap.h>
include "geogmap.h"

define	GHELPFILE 	"images$lib/geomap.key"
define	CHELPFILE 	"images$lib/coomap.key"

$for (rd)

# GEO_MGFIT -- Fit the surface using interactive graphics.

procedure geo_mgfit$t (gd, fit, sx1, sy1, sx2, sy2, xref, yref, xin,
        yin, wts, npts, xerrmsg, yerrmsg, maxch)

pointer	gd		#I graphics file descriptor
pointer	fit		#I pointer to the fit structure
pointer	sx1		#I pointer to the linear x surface fit
pointer	sy1		#I pointer to the linear y surface fit
pointer	sx2		#I pointer to higher order x surface fit
pointer	sy2		#I pointer to higher order y surface fit
PIXEL	xref[npts]	#I the x reference coordinates
PIXEL	yref[npts]	#I the y reference coordinates
PIXEL	xin[npts]	#I input x coordinates
PIXEL	yin[npts]	#I input y coordinates
PIXEL	wts[npts]	#I array of weights
int	npts		#I number of data points
char	xerrmsg[ARB]	#O the output x fit error message 
char	yerrmsg[ARB]	#O the output x fit error message 
int	maxch		#I the size of the error messages

char	errstr[SZ_LINE]
int	newgraph, delete, wcs, key, errcode
pointer	sp, w, gfit, xresid, yresid, cmd
pointer	gt1, gt2, gt3, gt4, gt5
real	wx, wy
PIXEL	xshift, yshift, xscale, yscale, thetax, thetay

int	clgcur(), errget()
pointer	gt_init()

errchk	geo_fxy$t(), geo_mreject$t(), geo_ftheta$t()
errchk	geo_fmagnify$t(), geo_flinear$t()

begin
	# Initialize gfit structure and working space.
	call smark (sp)
	call salloc (gfit, LEN_GEOGRAPH, TY_STRUCT)
	call salloc (xresid, npts, TY_PIXEL)
	call salloc (yresid, npts, TY_PIXEL)
	call salloc (w, npts, TY_PIXEL)
	call salloc (cmd, SZ_LINE, TY_CHAR)

	# Do initial fit.
	iferr {
	    switch (GM_FIT(fit)) {
	    case GM_ROTATE:
	        call geo_ftheta$t (fit, sx1, sy1, xref, yref, xin, yin, wts,
	            Mem$t[xresid], Mem$t[yresid], npts, xerrmsg, maxch,
		    yerrmsg, maxch)
		    sx2 = NULL
		    sy2 = NULL
	    case GM_RSCALE:
	        call geo_fmagnify$t (fit, sx1, sy1, xref, yref, xin, yin, wts,
	            Mem$t[xresid], Mem$t[yresid], npts, xerrmsg, maxch,
		    yerrmsg, maxch)
		    sx2 = NULL
		    sy2 = NULL
	    case GM_RXYSCALE:
	        call geo_flinear$t (fit, sx1, sy1, xref, yref, xin, yin, wts,
	            Mem$t[xresid], Mem$t[yresid], npts, xerrmsg, maxch,
		    yerrmsg, maxch)
		    sx2 = NULL
		    sy2 = NULL
	    default:
	        call geo_fxy$t (fit, sx1, sx2, xref, yref, xin, wts,
		    Mem$t[xresid], npts, YES, xerrmsg, maxch)
	        call geo_fxy$t (fit, sy1, sy2, xref, yref, yin, wts,
		    Mem$t[yresid], npts, NO, yerrmsg, maxch)
	    }
	    if (GM_MAXITER(fit) <= 0 || IS_INDEFD(GM_REJECT(fit)))
	        GM_NREJECT(fit) = 0
	    else
	        call geo_mreject$t (fit, sx1, sy1, sx2, sy2, xref, yref, xin,
		    yin, wts, Mem$t[xresid], Mem$t[yresid], npts, xerrmsg,
		    maxch, yerrmsg, maxch)
	} then {
	    call sfree (sp)
	    if (GM_PROJECTION(fit) == GM_NONE)
	        call error (2, "Too few points for X and Y fits.")
	    else
	        call error (2, "Too few points for XI and ETA fits.")
	}

	GG_NEWFUNCTION(gfit) = NO
	GG_FITERROR(gfit) = NO
	errcode = OK

	# Set up plotting defaults.
	GG_PLOTTYPE(gfit) = FIT
	GG_OVERPLOT(gfit) = NO
	GG_CONSTXY(gfit) = YES
	newgraph = NO

	# Allocate graphics tools.
	gt1 = gt_init ()
	gt2 = gt_init ()
	gt3 = gt_init ()
	gt4 = gt_init ()
	gt5 = gt_init ()

	# Set the plot title and x and y axis labels.
	call geo_gtset (FIT, gt1, fit)
	call geo_gtset (XXRESID, gt2, fit)
	call geo_gtset (XYRESID, gt3, fit)
	call geo_gtset (YXRESID, gt4, fit)
	call geo_gtset (YYRESID, gt5, fit)

	# Make the first plot.
	call gclear (gd)
	call geo_label (FIT, gt1, fit)
	call geo_1graph$t (gd, gt1, fit, gfit, xref, yref, xin, yin, wts,
	    npts)
	if (GG_CONSTXY(gfit) == YES)
	    call geo_conxy$t (gd, fit, sx1, sy1, sx2, sy2)
	call printf ("%s  %s\n")
	    call pargstr (xerrmsg)
	    call pargstr (yerrmsg)

	# Read the cursor commands.
	call amov$t (wts, Mem$t[w], npts)
	while (clgcur ("cursor", wx, wy, wcs, key, Memc[cmd], SZ_LINE) != EOF) {

	    switch (key) {

	    case 'q':
		call amov$t (Mem$t[w], wts, npts)
		break

	    case '?':
		if (GM_PROJECTION(fit) == GM_NONE)
		    call gpagefile (gd, GHELPFILE, "")
		else
		    call gpagefile (gd, CHELPFILE, "")

	    case ':':
		call geo_colon (gd, fit, gfit, Memc[cmd], newgraph)
		switch (GG_PLOTTYPE(gfit)) {
		case FIT:
		    call gt_colon (Memc[cmd], gd, gt1, newgraph)
		case XXRESID:
		    call gt_colon (Memc[cmd], gd, gt2, newgraph)
		case XYRESID:
		    call gt_colon (Memc[cmd], gd, gt3, newgraph)
		case YXRESID:
		    call gt_colon (Memc[cmd], gd, gt4, newgraph)
		case YYRESID:
		    call gt_colon (Memc[cmd], gd, gt5, newgraph)
		}

	    case 'l':
		if (GG_FITERROR(gfit) == NO) {
		    call geo_lcoeff$t (sx1, sy1, xshift, yshift, xscale, yscale,
		        thetax, thetay)
		    call printf ("xshift: %.2f yshift: %.2f ")
		        call parg$t (xshift)
		        call parg$t (yshift)
		    call printf ("xmag: %0.3g ymag: %0.3g ")
		        call parg$t (xscale)
		        call parg$t (yscale)
		    call printf ("xrot: %.2f yrot: %.2f\n")
		        call parg$t (thetax)
		        call parg$t (thetay)
		}

	    case 't':
		if (GG_FITERROR(gfit) == NO && GG_PLOTTYPE(gfit) == FIT)
		    call geo_lxy$t (gd, fit, sx1, sy1, sx2, sy2, xref, yref,
		        xin, yin, npts, wx, wy)

	    case 'c':
		if (GG_CONSTXY(gfit) == YES)
		    GG_CONSTXY(gfit) = NO
		else if (GG_CONSTXY(gfit) == NO)
		    GG_CONSTXY(gfit) = YES

	    case 'd', 'u':
		if (key == 'd')
		    delete = YES
		else
		    delete = NO

		switch (GG_PLOTTYPE(gfit)) {
		case FIT:
		    call geo_1delete$t (gd, xin, yin, Mem$t[w], wts, npts, wx,
		        wy, delete)
		case XXRESID:
		    call geo_2delete$t (gd, xref, Mem$t[xresid], Mem$t[w], wts,
		        npts, wx, wy, delete)
		case XYRESID:
		    call geo_2delete$t (gd, yref, Mem$t[xresid], Mem$t[w], wts,
		        npts, wx, wy, delete)
		case YXRESID:
		    call geo_2delete$t (gd, xref, Mem$t[yresid], Mem$t[w], wts,
		        npts, wx, wy, delete)
		case YYRESID:
		    call geo_2delete$t (gd, yref, Mem$t[yresid], Mem$t[w], wts,
		        npts, wx, wy, delete)
		}

		GG_NEWFUNCTION(gfit) = YES

	    case 'g':
		if (GG_PLOTTYPE(gfit) != FIT)
		    newgraph = YES
		GG_PLOTTYPE(gfit) = FIT

	    case 'x':
		if (GG_PLOTTYPE(gfit) != XXRESID)
		    newgraph = YES
		GG_PLOTTYPE(gfit) = XXRESID

	    case 'r':
		if (GG_PLOTTYPE(gfit) != XYRESID)
		    newgraph = YES
		GG_PLOTTYPE(gfit) = XYRESID

	    case 'y':
		if (GG_PLOTTYPE(gfit) != YXRESID)
		    newgraph = YES
		GG_PLOTTYPE(gfit) = YXRESID

	    case 's':
		if (GG_PLOTTYPE(gfit) != YYRESID)
		    newgraph = YES
		GG_PLOTTYPE(gfit) = YYRESID

	    case 'f':
		# do fit
		if (GG_NEWFUNCTION(gfit) == YES) {
		    iferr {
			switch (GM_FIT(fit)) {
			case GM_ROTATE:
		            call geo_ftheta$t (fit, sx1, sy1, xref, yref, xin,
			        yin, Mem$t[w], Mem$t[xresid], Mem$t[yresid],
				npts, xerrmsg, maxch, yerrmsg, maxch) 
				sx2 = NULL
				sy2 = NULL
			case GM_RSCALE:
		            call geo_fmagnify$t (fit, sx1, sy1, xref, yref, xin,
			        yin, Mem$t[w], Mem$t[xresid], Mem$t[yresid],
				npts, xerrmsg, maxch, yerrmsg, maxch) 
				sx2 = NULL
				sy2 = NULL
			case GM_RXYSCALE:
		            call geo_flinear$t (fit, sx1, sy1, xref, yref, xin,
			        yin, Mem$t[w], Mem$t[xresid], Mem$t[yresid],
				npts, xerrmsg, maxch, yerrmsg, maxch) 
				sx2 = NULL
				sy2 = NULL
			default:
		            call geo_fxy$t (fit, sx1, sx2, xref, yref, xin,
			        Mem$t[w], Mem$t[xresid], npts, YES,
				xerrmsg, maxch) 
		            call geo_fxy$t (fit, sy1, sy2, xref, yref, yin,
			        Mem$t[w], Mem$t[yresid], npts, NO,
				yerrmsg, maxch) 
			}
		        if (GM_MAXITER(fit) <= 0 || IS_INDEFD(GM_REJECT(fit)))
			    GM_NREJECT(fit) = 0
		        else
			    call geo_mreject$t (fit, sx1, sy1, sx2, sy2, xref,
			        yref, xin, yin, Mem$t[w], Mem$t[xresid],
				Mem$t[yresid], npts, xerrmsg, maxch,
				yerrmsg, maxch)
			GG_NEWFUNCTION(gfit) = NO
			GG_FITERROR(gfit) = NO
			errcode = OK
		    } then {
	    		errcode = errget (errstr, SZ_LINE)
	    		call printf ("%s\n")
			    call pargstr (errstr)
			GG_FITERROR(gfit) = YES
		    }
		}

		# plot new graph
		if (GG_FITERROR(gfit) == YES)
		    newgraph = NO
		else
		    newgraph = YES

	    case 'o':
		GG_OVERPLOT(gfit) = YES

	    default:
		call printf ("\07")

	    }

	    if (newgraph == YES) {
		switch (GG_PLOTTYPE(gfit)) {
		case FIT:
		    call geo_label (FIT, gt1, fit)
	    	    call geo_1graph$t (gd, gt1, fit, gfit, xref, yref, xin, yin,
		        Mem$t[w], npts)
		    if (GG_CONSTXY(gfit) == YES)
			call geo_conxy$t (gd, fit, sx1, sy1, sx2, sy2)
		case XXRESID:
		    call geo_label (XXRESID, gt2, fit)
	    	    call geo_2graph$t (gd, gt2, fit, gfit, xref, Mem$t[xresid],
		        Mem$t[w], npts)
		case XYRESID:
		    call geo_label (XYRESID, gt3, fit)
	    	    call geo_2graph$t (gd, gt3, fit, gfit, yref, Mem$t[xresid],
		         Mem$t[w], npts)
		case YXRESID:
		    call geo_label (YXRESID, gt4, fit)
	    	    call geo_2graph$t (gd, gt4, fit, gfit, xref, Mem$t[yresid],
		         Mem$t[w], npts)
		case YYRESID:
		    call geo_label (YYRESID, gt5, fit)
	    	    call geo_2graph$t (gd, gt5, fit, gfit, yref, Mem$t[yresid],
		         Mem$t[w], npts)
		}
	        call printf ("%s  %s\n")
	    	    call pargstr (xerrmsg)
	    	    call pargstr (yerrmsg)
		newgraph = NO
	    }
	}

	# Free space.
	call gt_free (gt1)
	call gt_free (gt2)
	call gt_free (gt3)
	call gt_free (gt4)
	call gt_free (gt5)
	call sfree (sp)

	# Call an error if appropriate.
	if (errcode > 0)
	    call error (2, errstr)
end

# GEO_LCOEFF -- Print the coefficents of the linear portion of the
# fit, xshift, yshift, xexpansion, yexpansion, x and y rotations.

procedure geo_lcoeff$t (sx, sy, xshift, yshift, xscale, yscale, xrot, yrot)

pointer sx              #I pointer to the x surface fit
pointer sy              #I pointer to the y surface fit
PIXEL   xshift          #O output x shift
PIXEL   yshift          #O output y shift
PIXEL   xscale          #O output x scale
PIXEL   yscale          #O output y scale
PIXEL   xrot            #O rotation of point on x axis
PIXEL   yrot            #O rotation of point on y axis

int     nxxcoeff, nxycoeff, nyxcoeff, nyycoeff
pointer sp, xcoeff, ycoeff
PIXEL   xxrange, xyrange, xxmaxmin, xymaxmin
PIXEL   yxrange, yyrange, yxmaxmin, yymaxmin
PIXEL   a, b, c, d

bool    fp_equal$t()
$if (datatype == r)
int     gsgeti()
real    gsgetr()
$else
int     dgsgeti()
double  dgsgetd()
$endif

begin
        # Allocate working space.
        call smark (sp)
$if (datatype == r)
        call salloc (xcoeff, gsgeti (sx, GSNCOEFF), TY_PIXEL)
        call salloc (ycoeff, gsgeti (sy, GSNCOEFF), TY_PIXEL)
$else
        call salloc (xcoeff, dgsgeti (sx, GSNCOEFF), TY_PIXEL)
        call salloc (ycoeff, dgsgeti (sy, GSNCOEFF), TY_PIXEL)
$endif

        # Get coefficients and numbers of coefficients.
$if (datatype == r)
        call gscoeff (sx, Mem$t[xcoeff], nxxcoeff)
        call gscoeff (sy, Mem$t[ycoeff], nyycoeff)
        nxxcoeff = gsgeti (sx, GSNXCOEFF)
        nxycoeff = gsgeti (sx, GSNYCOEFF)
        nyxcoeff = gsgeti (sy, GSNXCOEFF)
        nyycoeff = gsgeti (sy, GSNYCOEFF)
$else
        call dgscoeff (sx, Mem$t[xcoeff], nxxcoeff)
        call dgscoeff (sy, Mem$t[ycoeff], nyycoeff)
        nxxcoeff = dgsgeti (sx, GSNXCOEFF)
        nxycoeff = dgsgeti (sx, GSNYCOEFF)
        nyxcoeff = dgsgeti (sy, GSNXCOEFF)
        nyycoeff = dgsgeti (sy, GSNYCOEFF)
$endif

        # Get the data range.
$if (datatype == r)
        if (gsgeti (sx, GSTYPE) != GS_POLYNOMIAL) {
            xxrange = (gsgetr (sx, GSXMAX) - gsgetr (sx, GSXMIN)) / 2.0
            xxmaxmin = - (gsgetr (sx, GSXMAX) + gsgetr (sx, GSXMIN)) / 2.0
            xyrange = (gsgetr (sx, GSYMAX) - gsgetr (sx, GSYMIN)) / 2.0
            xymaxmin = - (gsgetr (sx, GSYMAX) + gsgetr (sx, GSYMIN)) / 2.0
$else
        if (dgsgeti (sx, GSTYPE) != GS_POLYNOMIAL) {
            xxrange = (dgsgetd (sx, GSXMAX) - dgsgetd (sx, GSXMIN)) / 2.0d0
            xxmaxmin = - (dgsgetd (sx, GSXMAX) + dgsgetd (sx, GSXMIN)) / 2.0d0
            xyrange = (dgsgetd (sx, GSYMAX) - dgsgetd (sx, GSYMIN)) / 2.0d0
            xymaxmin = - (dgsgetd (sx, GSYMAX) + dgsgetd (sx, GSYMIN)) / 2.0d0
$endif
        } else {
            xxrange = PIXEL(1.0)
            xxmaxmin = PIXEL(0.0)
            xyrange = PIXEL(1.0)
            xymaxmin = PIXEL(0.0)
        }

$if (datatype == r)
        if (gsgeti (sy, GSTYPE) != GS_POLYNOMIAL) {
            yxrange = (gsgetr (sy, GSXMAX) - gsgetr (sy, GSXMIN)) / 2.0
            yxmaxmin = - (gsgetr (sy, GSXMAX) + gsgetr (sy, GSXMIN)) / 2.0
            yyrange = (gsgetr (sy, GSYMAX) - gsgetr (sy, GSYMIN)) / 2.0
            yymaxmin = - (gsgetr (sy, GSYMAX) + gsgetr (sy, GSYMIN)) / 2.0
$else
        if (dgsgeti (sy, GSTYPE) != GS_POLYNOMIAL) {
            yxrange = (dgsgetd (sy, GSXMAX) - dgsgetd (sy, GSXMIN)) / 2.0d0
            yxmaxmin = - (dgsgetd (sy, GSXMAX) + dgsgetd (sy, GSXMIN)) / 2.0d0
            yyrange = (dgsgetd (sy, GSYMAX) - dgsgetd (sy, GSYMIN)) / 2.0d0
            yymaxmin = - (dgsgetd (sy, GSYMAX) + dgsgetd (sy, GSYMIN)) / 2.0d0
$endif
        } else {
            yxrange = PIXEL(1.0)
            yxmaxmin = PIXEL(0.0)
            yyrange = PIXEL(1.0)
            yymaxmin = PIXEL(0.0)
        }

        # Get the shifts.
        xshift = Mem$t[xcoeff] + Mem$t[xcoeff+1] * xxmaxmin / xxrange +
            Mem$t[xcoeff+2] * xymaxmin / xyrange
        yshift = Mem$t[ycoeff] + Mem$t[ycoeff+1] * yxmaxmin / yxrange +
            Mem$t[ycoeff+2] * yymaxmin / yyrange

        # Get the rotation and scaling parameters and correct for normalization.
        if (nxxcoeff > 1)
            a = Mem$t[xcoeff+1] / xxrange
        else
            a = PIXEL(0.0)
        if (nxycoeff > 1)
            b = Mem$t[xcoeff+nxxcoeff] / xyrange
        else
            b = PIXEL(0.0)
        if (nyxcoeff > 1)
            c = Mem$t[ycoeff+1] / yxrange
        else
            c = PIXEL(0.0)
        if (nyycoeff > 1)
            d = Mem$t[ycoeff+nyxcoeff] / yyrange
        else
            d = PIXEL(0.0)

        # Get the magnification factors.
        xscale = sqrt (a * a + c * c)
        yscale = sqrt (b * b + d * d)

        # Get the x and y axes rotation factors.
        if (fp_equal$t (a, PIXEL(0.0)) && fp_equal$t (c, PIXEL(0.0)))
            xrot = PIXEL(0.0)
        else
            xrot = RADTODEG (atan2 (-c, a))
        if (xrot < PIXEL(0.0))
            xrot = xrot + PIXEL(360.0)

        if (fp_equal$t (b, PIXEL(0.0)) && fp_equal$t (d, PIXEL(0.0)))
            yrot = PIXEL(0.0)
        else
            yrot = RADTODEG (atan2 (b, d))
        if (yrot < PIXEL(0.0))
            yrot = yrot + PIXEL(360.0)

        call sfree (sp)
end

$endfor