# magrotran --  Computes general Image translation/rotation/magnification
#
# R.L. Williamson II, 29-Apr-92   Created from SDAS routine MAGROTRAN_SX
#
#   Description:
#   ------------
#   This module computes the parameters needed to translate, rotate and
#   magnify one (secondary) image to enable it to be registered with
#   another (reference) image. It uses a general leastsquares algorithm
#   copied from the STARLINK system called XYFIT; The current code was
#   obtained from Bill Pence at the Space Telescope Science Institute.
#
#   Slightly modified versions of three subroutines from the STARLINK
#   software library have been included here. The correspondence is as
#   follows:
#
#   STARLINK   STSDAS Name   Description of subroutine
#   -------------------------------------------------------------------------
#   XYFIT      ---------     Controlling subroutine; omitted in STSDAS version
#   FITLST     MAGROTRAN     Calls Fitting routine and calculates RMS error
#   XYFITR     XYFITREJ      Iteration loop which can reject worst points
#   LINTRN     LINTRANS      Linear transform between 2 sets of positions
#
#   The original STARLINK software was written by R.F. Warren-Smith.
#
#   Original STARLINK documentation follows: (preceded by a S:)
#
#    S:
#    S:  PURPOSE
#    S:   TO CONTROL THE FITTING OF A LINEAR TRANSFORMATION BETWEEN
#    S:   TWO SETS OF X,Y POSITIONS AND TO DISPLAY THE RESULTS
#    S:
#    S:  METHOD
#    S:   EXTRACT THE X,Y POSITIONS AND IDENTIFIERS FROM THE INPUT LISTS
#    S:   AND MATCH THEM TOGETHER USING XYMTCH. CALL XYFITR TO PERFORM
#    S:   THE FITTING, THEN DISPLAY A TABLE OF RESULTS.
#
#    The code to display the result has been eliminated here.

procedure magrotran(xa, ya, xb, yb, npoints, ifit, maxit, gamma,
                                epsilon, coeff, nreject)

int npoints, i                          # number of features, generic counter
int ifit                                # fitting option (1-4)
int maxit                               # maximum number of iterations
int nreject                             # number of points rejected
real gamma, epsilon                     # rejection tolerance, fit tolerance
real xmag, ymag, jacobian               # magnitude terms, jacobian
real errsq, xd, yd, sigma               # RMS error, derived x,y, sigma
double xa[npoints]                      # input secondary 1st coord array
double ya[npoints]                      # input secondary 2nd coord array
double xb[npoints]                      # input reference 1st coord array
double yb[npoints]                      # input reference 2nd coord array
double coeff[6]                         # fit coefficients
pointer sp
pointer valid                           # array indicating which points 
                                        # were rejected.
pointer errmsg
string badjac "WARNING:Transformation is skewed! Jacobian = %g (Should be 1.0)\n"
string badgamma "All positions were rejected! Try a larger GAMMA.\n"


begin
        call smark (sp)

        # Allocate error message array
        call salloc(errmsg, SZ_LINE, TY_CHAR)

        # Allocate valid array
        call salloc(valid, npoints, TY_REAL)
        # Set all values in valid array to 1.0
        call amovkr(1.0, Memr[valid], npoints)

        # Call XYFITREJ

        call xyfitrej (xa,ya,           # Secondary input table
                       xb,yb,           # Reference input table
                       Memr[valid],     # Array of valid entries
                       npoints,         # Number of entries
                       maxit,           # Max number of iterations 
                       gamma,           # Number of sigmas at which to reject
                       ifit,            # Type of fit returned from XYFITREJ
                       coeff)           # Coefficients



        #       ifit = 1 --> translation only
        #       ifit = 2 --> translation and rotation
        #       ifit = 3 --> translation
        #       ifit = 4 --> full fit; probably has skew too


        xmag   = 1.0
        ymag   = 1.0

        #  Assume there is magnification;  
        #  Find the magnification scale factors.

        if (ifit > 2) {
            xmag = sqrt (coeff[2]**2 + coeff[3]**2)
            ymag = sqrt (coeff[5]**2 + coeff[6]**2)           
            }

         # Check for zero magnification factors to avoid 
         # overflow in Jacobian divide

         if (xmag * ymag == 0.0) {
             call error (1,"Magnification factor is Zero!")
         }

         # Check the Jacobian Determinant; should be 1 !!!

         jacobian = coeff[2] * coeff[6] - coeff[5] * coeff[3]
         jacobian = jacobian / ( xmag * ymag )   # Calculate Determinant

         if ( abs ( jacobian - 1.0 ) > epsilon ) {
             call sprintf(Memc[errmsg], SZ_LINE, badjac)
             call pargr(jacobian)
             call eprintf(Memc[errmsg])
          }

          # Calculate RMS error and count number of positions rejected
          nreject=0
          sigma = 0.0

          do i = 1, npoints {
              xd = coeff[1] + coeff[2] * xa[i] + coeff[3] * ya[i]
              yd = coeff[4] + coeff[5] * xa[i] + coeff[6] * ya[i]
              errsq = (xd - xb[i])**2 + (yd - yb[i])**2

              if (Memr[valid+i-1] > 0.5) {      # Increase sum of errors
                  sigma = sigma + errsq
              } else  {
                  nreject = nreject + 1
              }
          }
             
          # Calculate RMS Error
             
          if (nreject < npoints ) {
             sigma = sqrt (sigma / (npoints - nreject))
          } else {
              sigma = 0.0
              call error(1, badgamma)
          }
        call sfree (sp)
end