include <math/iminterp.h>
include "sa_defines.h"

#---------------------------------------------------------------------------
.help sa_shiftadd Dec96 source
.ih
NAME
sa_shiftadd -- Shift and co-add the data to create the initial guess using the chosen
shifting method.
.ih
USAGE
call sa_shiftadd (data, shift, sdata, weight, out_data, o_err, n_shifts, len, slen, olen, rsfunc)
.ih
ARGUMENTS
.ls data (I: double[len,n_shifts])
The arrays to be shifted and added.
.le
.ls shift (I: double[n_shifts])
The shifts for each line of data.
.le
.ls sdata (O: double[olen,n_shifts])
The resampled data array.
.le
.ls weight (O: double[olen,n_shifts])
The weights for the resampled data array.
.le
.ls out_data (O: double[olen])
The resampled/co-added/averaged combination of the input array.
.le
.ls o_err (O: double[olen])
The standard deviation for each point in the output, out_data.
.le
.ls n_shifts (I: int)
The number of shifts present.
.le
.ls len (I: int)
The length of each line of data.
.le
.ls slen (O: int[n_shifts])
Array to hold the actual lengths of the shifted data in each row in sdata.
.le
.ls olen (I: int)
Length of the output arrays.
.le
.ls rsfunc (I: int)
Function to be used for the resampling method.
.ih
DESCRIPTION
This routine shifts each row of the data array by the amount specified in the 
"shift" array, using the chosen method and creating a new version of the data
array called sdata.  The 2D sdata array is compressed into a 1D array by 
co-adding all the individual rows on a pixel-by-pixel basis.  This output 
array is then averaged and the standard deviation is calculated for each 
point.  The standard deviation is computed with the corrected two-pass 
algorithm to minimize roundoff error.  Reference: Press, Teukolsky, Vetterling,
and Flannery, "Numerical Recipes," Second Edition, 1992.
.endhelp
#---------------------------------------------------------------------------
procedure sa_shiftadd (data, shift, sdata, weight, out_data, o_err, n_shifts, len, slen, olen, rsfunc)

# Passed parameters.
double	data[len,n_shifts]	# I:  The spectral data to be put together.
double  shift[n_shifts]		# I:  The shifts of each data line.
double  sdata[olen,n_shifts]    # O:  The shifted version of the data array.
double  weight[olen,n_shifts]   # O:  Weights for the shifted data array.
double	out_data[olen]		# O:  Shift/add/average of the input.
double	o_err[olen]		# O:  Standard deviation of out_data.
int	n_shifts		# I:  The number of shifts.
int	len			# I:  Length of each data element.
int     slen[n_shifts]          # O:  Actual length of shifted row in sdata.
int	olen			# I:  Length of result.
int     rsfunc                  # I:  Resampling function.

# Local variables.
int	s			# Current shift operating on.
int     mode                    # Resampling mode.
int     index                   # Insertion index for shift data matrix.
int     ishift                  # Integer shifts of each data line.
double  frac1, frac2            # Fractional portion of pixel for shift.
pointer n			# Number of elements per pixel added.
pointer wn                      # Weighted number of elements per pixel added.
pointer sp			# Stack pointer.
pointer sum, sumsq		# Sums.
 
# Functions.
double  sas_neg()               # What to do with negative square roots.
extern	sas_neg()

begin
	call smark (sp)
	call salloc (n, olen, TY_DOUBLE)
	call salloc (wn, olen, TY_DOUBLE)
	call salloc (sum, olen, TY_DOUBLE)
	call salloc (sumsq, olen, TY_DOUBLE)

        # Determine the resampling mode.
        mode = 0
        switch (rsfunc) {
        case RS_LINEAR:
            mode = II_LINEAR
        case RS_SPLINE3:
            mode = II_SPLINE3
        case RS_POLY3:
            mode = II_POLY3
        case RS_POLY5:
            mode = II_POLY5
        }

        # Shift the individual rows of the input data array and insert
        # into the proper x-position in the sdata array.  Note all the input 
        # shifts have been forced to be positive.
        call aclrd (sdata, olen * n_shifts)
        call aclrd (weight, olen * n_shifts)
	call aclrd (Memd[n], olen)
	call aclri (slen, n_shifts)
        do s = 1, n_shifts {
            ishift = int(shift[s])
            frac1  = (shift[s] - ishift)
            frac2  = 1.0d0 - frac1

            # If the shift is less than threshold, copy the input to the output.
            # Set the weighting to 1.0 for all the output pixels.
            # This is invoked for RS_NONE by default.
            if ((frac1 <= RS_MINDIST) || (rsfunc == RS_NONE)) {
                index = ishift + 1
                call amovd  (data[1,s], sdata[index,s], len)
                call aaddkd (weight[index,s], 1.0d0, weight[index,s], len)
                call aaddkd (Memd[n+ishift], 1.0d0, Memd[n+ishift], len)
                slen[s] = len
            }
            
            # Else call the resampling routine.
            else {
                slen[s] = len + 1
                call sa_resample (data, sdata, weight, frac1, n_shifts, 
                                  len, olen, ishift, s, mode)

                # Accumulate the number of pixels contributing to the 
                # output array.
                call aaddkd (Memd[n+ishift], 1.0d0, Memd[n+ishift], len + 1)
            }
        }

	# Accumulate the spectral and weight data.
	call aclrd (out_data, olen)
	call aclrd (Memd[wn], olen)
	do s = 1, n_shifts {
	    call aaddd (sdata[1,s], out_data, out_data, olen)
	    call aaddd (weight[1,s], Memd[wn], Memd[wn], olen)
	}

	# Compute a weighted average.
	call adivd (out_data, Memd[wn], out_data, olen)

        # Compute the standard deviation using the corrected two-pass
        # algorithm to minimize roundoff error.  First, determine the 
        # summation of the deviation from the mean and the deviation 
        # from the mean squared.
	call aclrd (Memd[sum], olen)
	call aclrd (Memd[sumsq], olen)
	do s = 1, n_shifts {
            ishift = int(shift[s])
            index  = ishift + 1

            # Take the reciprocal of the weight array to scale the sdata array
            # properly for further computation.
            call arcpd (1.0d0, weight[index,s], weight[index,s], slen[s])

            # Weight the individual data values before subtracting the mean.
            call amuld (sdata[index,s], weight[index,s], o_err, slen[s])
	    call asubd (o_err, out_data[index], o_err, slen[s])

            # Accumulate the statistics.
	    call aaddd (o_err, Memd[sum+ishift], Memd[sum+ishift], slen[s])
	    call amuld (o_err, o_err, o_err, slen[s])
	    call aaddd (o_err, Memd[sumsq+ishift], Memd[sumsq+ishift], slen[s])
	}

	# Second, compute the second term of the two-pass equation.
	call amuld (Memd[sum], Memd[sum], Memd[sum], olen)
	call adivd (Memd[sum], Memd[n], Memd[sum], olen)
	
	# Finally, compute the standard deviation.
	call asubd  (Memd[sumsq], Memd[sum], o_err, olen)
	call asubkd (Memd[n], 1.d0, Memd[n], olen)
	call arltd  (Memd[n], olen, 1.d0, 1.d0)
	call adivd  (o_err, Memd[n], o_err, olen)
	call asqrd  (o_err, o_err, olen, sas_neg)

	# Clean up memory allocation.
	call sfree (sp)
end
#---------------------------------------------------------------------------
# End of sa_shiftadd.
#---------------------------------------------------------------------------