# lintrans -- Get a linear transformation between two sets of positions
#
# R.L. Williamson II, 29-Apr-92   Created from old SDAS routine
#				  LINTRANS_SX
# R. Williamson, 18-Sep-92        Changed calls to ludcmp and ldbksb to
#                                 xtools calls ludcmd and ldbksd.
# R. Williamson, 30-Mar-95	  Fixed transciption bug in ifit=3 and ifit=4
#
#   Description:
#   ------------
#    From the STARLINK documentation:
#    ...
#    S: PURPOSE
#    S:     TO OBTAIN A LINEAR TRANSFORMATION BETWEEN 2 SETS OF X,Y
#    S:     POSITIONS WITH LEAST SQUARED ERROR.
#    S:
#    S: METHOD
#    S:     SET UP THE NORMAL EQUATIONS CORRESPONDING TO THE TYPE OF FIT
#    S:     REQUIRED. SOLVE NORMAL EQUATIONS TO GIVE THE TRANSFORMATION.
#    S:     IF SUCCESSFUL, EXIT. OTHERWISE REDUCE THE NUMBER OF DEGREES
#    S:     OF FREEDOM IN THE FIT AND REPEAT.

procedure lintrans (xa, ya, xb, yb, valid, nfeat, coeff, ifit)

int nfeat, i				# number of features, generic counter
int ifit				# fitting option (1-4)
int origfit				# original fitting option
int npts, itry				# number of points, number of tries
int ifail				# failure flag for ludcmp
int indx[4]				# output to ludcmp
double d				# output to ludcmp
double xa[nfeat]			# input secondary 1st coord array
double ya[nfeat]			# input secondary 2nd coord array
double xb[nfeat]			# input reference 1st coord array
double yb[nfeat]			# input reference 2nd coord array
double coeff[6] 			# fit coefficients
real valid[nfeat]		      	# array indicating which points
					# were rejected.
double a[4, 4], b[4,2], b1[4,2], b2[4,2] # input, output arrays for ludcmp
double w, sw, wx, wy, swxy		 # local stuff
double swx, swx2, swxd, swxxd, swxyd	 # summation holders
double swy, swy2, swyd, swyyd, swyxd
double x0, y0, xd0, yd0
double swxxd0, swyyd0, swxyd0, swyxd0
double top, bot
real half, theta, zero, one
string notenough "Insufficient number of features!"
string toomany "Too many features rejected!"
string notofit "Transformation had to be reduced in complexity.\n"

#
data half/0.5/, zero/0.0/, one/1.0/

begin

	origfit = ifit

	# Check validity of arguments

	if (nfeat < 1) {
	    call error (1, notenough)
	} else {
	    sw = zero
	    do i = 1, nfeat {
		if (valid[i] > half) {
		    sw = sw + one
		}
	    }
	    if (sw <= zero ) {
		call error (1, toomany)
	    } else {
		ifit = min (max (1, ifit), 4) # set type of fit 1-4

		# check that the fit does not have too many 
		# degrees of freedom for the number of data 
		# points available

		npts = nint (sw)
		if (npts <= 2) ifit = min (ifit, 3)
		if (npts <= 1) ifit = 1

		# Initialize sums for normal equations

		swx   = zero
		swy   = zero
		swxy  = zero
		swx2  = zero
		swy2  = zero
		swxd  = zero
		swyd  = zero
		swxxd = zero
		swyyd = zero
		swxyd = zero
		swyxd = zero


		# Form sums, setting weight to zero for
		# invalid positions

		do i = 1, nfeat {
		    if (valid [i] >  half) {
			w = one
		    } else {
			w = zero
		    }
		    wx	 = w * xa[i]
		    wy	 = w * ya[i]
		    swx  = swx + wx
		    swy  = swy + wy
		    swxd = swxd + w * xb [i]
		    swyd = swyd + w * yb [i]

		    # if fit only requires a shift of origin, further
		    #sums are not needed
		    
		    if (ifit != 1) {
			swxy  = swxy  + wx * ya [i]
			swx2  = swx2  + wx * xa [i]
			swy2  = swy2  + wy * ya [i]
			swxxd = swxxd + wx * xb [i]
			swxyd = swxyd + wx * yb [i]
			swyxd = swyxd + wy * xb [i]
			swyyd = swyyd + wy * yb [i]
		     }
		}

		#Iterate up to 4 times, reducing ifit by 1 each time
		
		ifit = ifit + 1
		itry = 0
		ifail = 1
		while (itry <= 4 && ifail != 0) {
		    itry = itry + 1
		    ifit = ifit - 1

		    # shift of origin only: equations simply solved

		    if (ifit == 1) {
			coeff [1] = (swxd - swx) / sw
			coeff [2] =  one
			coeff [3] =  zero
			coeff [4] = (swyd - swy) / sw
			coeff [5] =  zero
			coeff [6] =  one
			ifail = 0 # Indicates success

		    # shift of origin and rotation
		
		    } else if (ifit == 2) {
			
			# Calculate the centroids of each set of positions
			
			xd0 = swxd / sw
			yd0 = swyd / sw
			x0  = swx  / sw
			y0  = swy  / sw

			# Initialize storage for new sums
			
			swyxd0 = zero
			swxyd0 = zero
			swxxd0 = zero
			swyyd0 = zero


			# Form new sums, using the deviations 
			# from the centroids

		       do i = 1, nfeat {
			    if (valid [i] > half) {
				swyxd0 = swyxd0 + (ya [i] - y0) *
							(xb [i] - xd0)
				swxyd0 = swxyd0 + (xa [i] - x0) *
							(yb [i] - yd0)
				swxxd0 = swxxd0 + (xa [i] - x0) *
							(xb [i] - xd0)
				swyyd0 = swyyd0 + (ya [i] - y0) *
							(yb [i] - yd0)
			    }
			}

			# If rotation angle is not defined, ifail = 1
			# (so try less complex method)

			top = swyxd0 - swxyd0
			bot = swyyd0 + swxxd0
			if (top == zero && bot == zero) {
			    ifail = 1
			} else {
			    
			    # Otherwise calculate the rotation angle 
			    # about the centroids and assign the results 
			    #to the transform coefficients
				
			    theta = atan2 (top, bot)
			    coeff [1] = xd0 - (x0 * cos (theta) +
						y0 * sin (theta))
			    coeff [2] = cos (theta)
			    coeff [3] = sin (theta)
			    coeff [4] = yd0 - ( - x0 * sin (theta) +
							y0 * cos (theta))
			    coeff [5] = - sin (theta)
			    coeff [6] =   cos (theta)
			    ifail = 0 # Success
			}

			# shift, rotation and magnification: set up 
			# normal equations


		    } else if (ifit == 3) {
			a (1, 1) = sw
			a (1, 2) = swx
			a (1, 3) = swy
			a (1, 4) = 0.0d0
			a (2, 1) = swx
			a (2, 2) = swx2 + swy2
			a (2, 3) = 0.0d0
			a (2, 4) = swy
			a (3, 1) = swy
			a (3, 2) = 0.0d0
			a (3, 3) = swx2 + swy2
			a (3, 4) =  - swx
			a (4, 1) = 0.0d0
			a (4, 2) = swy
			a (4, 3) =  - swx
			a (4, 4) = sw
			b (1, 1) = swxd
			b (2, 1) = swxxd + swyyd
			b (3, 1) = swyxd - swxyd
			b (4, 1) = swyd

			# Call Numerical Recipes algorithms LUDCMP and
			# LUBKSB to solve linear normal equations
                        # Now in STSDAS xtools library

			call ludcmd (a, 4, 4, indx, d, ifail)
			call lubksd (a, 4, 4, indx, b)

                        # If successful, assign results to the transformation
                        # coefficients
 
                        if (ifail == 0) {
                            coeff (1) =   b (1,1)
                            coeff (2) =   b (2,1)
                            coeff (3) =   b (3,1)
                            coeff (4) =   b (4,1)
                            coeff (5) = - b (3,1)
                            coeff (6) =   b (2,1)
                        }

#
#  Full Fit required: Set up normal equations
#

		    } else if (ifit == 4) {
			a (1, 1) = sw
			a (1, 2) = swx
			a (1, 3) = swy
			a (2, 1) = swx
			a (2, 2) = swx2
			a (2, 3) = swxy  
			a (3, 1) = swy
			a (3, 2) = swxy
			a (3, 3) = swy2
			b1 (1, 1) = swxd
			b1 (2, 1) = swxxd
			b1 (3, 1) = swyxd
			b2 (1, 1) = swyd
			b2 (2, 1) = swxyd
			b2 (3, 1) = swyyd

			# Call Numerical Recipes algorithms LUDCMP and
			# LUBKSB to solve linear normal equations

			call ludcmd (a, 3, 4, indx, d, ifail)
			call lubksd (a, 3, 4, indx, b1)
			call lubksd (a, 3, 4, indx, b2)

			# If successful, assign results to the transformation
			# coefficients

			if (ifail == 0) {
			    coeff (1) = b1 (1,1)
			    coeff (2) = b1 (2,1)
			    coeff (3) = b1 (3,1)
			    coeff (4) = b2 (1,1)
			    coeff (5) = b2 (2,1)
			    coeff (6) = b2 (3,1)
			}
		    }
		    # If a fit was successfully obtained this time, exit
		    # from iteration loop.  Otherwise, try again with ifit
		    # reduced by 1

		}

		if (ifit != origfit) {
			call eprintf (notofit)
			call eprintf("Final fitting option was %s\n")
			if (ifit == 1) call pargstr("translation")
			if (ifit == 2) call pargstr("rotrans")
			if (ifit == 3) call pargstr("general")
		}
	    }
	}
	
	end