# xyfitrej - Iteratively fits registration coefficients to two sets of positions
#
# R.L. Williamson II, 29-Apr-92         Task created from SDAS routine 
#                                       LINTRANS_SX
#   Description:
#   ------------
#    This module is basically a copy of a STARLINK subroutine supplied to STSDAS
#    by Bill Pence and used in the STSDAS system with permission. It is somewhat
#    modified to make it compatible withST SDAS standards and methods, but the
#    original code (excluding interactive I/O) is essentially the same.
#
#    The original user documentation on XYFITR from STARLINK is shown here.
#
#    S: ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#    S: PURPOSE
#    S:        TO FIND A LEAST-SQUARES LINEAR TRANSFORMATION BETWEEN TWO SETS
#    S:        OF X,Y POSITIONS, REJECTING ERRONEOUS DATA.
#    S:
#    S: METHOD
#    S:        CALL LINTRN TO OBTAIN A LEAST-SQUARES FIT. CALCULATE THE RMS
#    S:        MIS-ALIGNMENT AND THE MOST MIS-ALIGNED DATA POINT. REJECT THE
#    S:        WORST POINT IF IT EXCEEDS A THRESHOLD BASED ON THE RMS ERROR.
#    S:        REPEAT FOR A PRESET NUMBER OF ITERATIONS, OR UNTIL NO FURTHER
#    S:        POINTS ARE REJECTED

procedure xyfitrej (xa, ya, xb, yb, valid, nfeat, maxit, gamma, ifit, coeff)

bool reject                             
int iter,imax                           # iteration counter, maximum iterations
int nfeat, i,                           # number of features, generic counter 
int ifit                                # fitiing option (1-4)          
int maxit                               # maximum number of iterations  
int ngood                               # number of good points
real varsum, var                        # sum of variances, worst variance
real errmax                             # maximum error in given iteration
real valid[nfeat]                       # array indicating which points 
                                        # were rejected.                
real gamma                              # rejection tolerance
real errsq, xd, yd 			# RMS error, derived x,y
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                
string badgamma "All objects have been rejected! Try a smaller GAMMA."

begin
        iter = 0
        reject = true
        while (reject) {
            
            # Call LINTRN to find linear transformation between 2 sets
            
            call lintrans (xa, ya, xb, yb, valid, nfeat, coeff, ifit)
            
            # Quit if MAXIT is exceeded
            if (iter >= maxit) { 
                reject = false
            } else {
                iter = iter + 1
                reject = false
                
                # Initialize counters for finding Standard deviation and
                # Maximum misalignment
                 
                varsum = 0.0
                ngood = 0
                errmax = -1.0
            
                # Scan all points not rejected so far
                do i = 1, nfeat   {
                     if (valid[i] > 0.5) {    #Calculate the misalignment
                        xd = coeff[1] + coeff[2]*xa[i] + coeff[3]*ya[i]
                        yd = coeff[4] + coeff[5]*xa[i] + coeff[6]*ya[i]
                        errsq = (xb[i] -xd)**2 + (yb[i] -yd)**2
                        varsum = varsum + errsq
                        ngood = ngood +1
                        if (errsq > errmax) {  # Find Maximum misalignment
                            errmax = errsq
                            imax = i
                        } # Max error
                    } # valid point check
                } # loop over all points
            
                # Find RMS misalignment  and reject the worst point if it is
                # more than gamma std. devs. out
            
                if (ngood > 0 ) {
                    var = varsum / ngood
                    if ((var > 0) && (errmax > var *(gamma**2))) {
                        valid[imax] = 0.0
                        reject = true
                    } # end variance check
                 } else {  # no more good points!
                     call error(1, badgamma)
                     reject = false
                 } # Good points check
             } 
         }

end