//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// XXX WARNING: I have completely replaced this file with an OLD VERSION (that works) instead of the
// one that was being worked on.
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

/** @file  pmSubtractBias.c
 *
 *  This file will contain a module which will subtract the detector bias
 *  in place from an input image.
 *
 *  @author GLG, MHPCC
 *
 *  @version $Revision: 1.9.4.1 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2006-02-17 17:13:42 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 *
 */

#if HAVE_CONFIG_H
#include <config.h>
#endif

#include <assert.h>
#include "pmSubtractBias.h"

#define PM_SUBTRACT_BIAS_POLYNOMIAL_ORDER 2
#define PM_SUBTRACT_BIAS_SPLINE_ORDER 3


#define MAX(a,b) ((a) > (b) ? (a) : (b))
#define MIN(a,b) ((a) < (b) ? (a) : (b))


// XXX: put these in psConstants.h
void PS_POLY1D_PRINT(psPolynomial1D *poly)
{
    printf("-------------- PS_POLY1D_PRINT() --------------\n");
    printf("poly->nX is %d\n", poly->nX);
    for (psS32 i = 0 ; i < (1 + poly->nX) ; i++) {
        printf("poly->coeff[%d] is %f\n", i, poly->coeff[i]);
    }
}

void PS_PRINT_SPLINE(psSpline1D *mySpline)
{
    printf("-------------- PS_PRINT_SPLINE() --------------\n");
    printf("mySpline->n is %d\n", mySpline->n);
    for (psS32 i = 0 ; i < mySpline->n ; i++) {
        PS_POLY1D_PRINT(mySpline->spline[i]);
    }
    PS_VECTOR_PRINT_F32(mySpline->knots);
}

#define PS_IMAGE_PRINT_F32_HIDEF(NAME) \
printf("======== printing %s ========\n", #NAME); \
for (int i = 0 ; i < (NAME)->numRows ; i++) { \
    for (int j = 0 ; j < (NAME)->numCols ; j++) { \
        printf("%.5f ", (NAME)->data.F32[i][j]); \
    } \
    printf("\n"); \
}\


void overscanOptionsFree(pmOverscanOptions *options)
{
    psFree(options->stat);
    psFree(options->poly);
    psFree(options->spline);
}

pmOverscanOptions *pmOverscanOptionsAlloc(bool single, pmFit fitType, unsigned int order, psStats *stat)
{
    pmOverscanOptions *opts = psAlloc(sizeof(pmOverscanOptions));
    psMemSetDeallocator(opts, (psFreeFunc)overscanOptionsFree);

    // Inputs
    opts->single = single;
    opts->fitType = fitType;
    opts->order = order;
    opts->stat = psMemIncrRefCounter(stat);

    // Outputs
    opts->poly = NULL;
    opts->spline = NULL;

    return opts;
}


/******************************************************************************
psSubtractFrame(): this routine will take as input a readout for the input
image and a readout for the bias image.  The bias image is subtracted in
place from the input image.
*****************************************************************************/
static bool SubtractFrame(pmReadout *in,// Input readout
                          const pmReadout *sub, // Readout to be subtracted from input
                          float scale   // Scale to apply before subtracting
                         )
{
    assert(in);
    assert(sub);

    psImage *inImage  = in->image;      // The input image
    psImage *inMask   = in->mask;       // The input mask
    psImage *subImage = sub->image;     // The image to be subtracted
    psImage *subMask  = sub->mask;      // The mask for the subtraction image

    // Offsets of the cells
    int x0in = psMetadataLookupS32(NULL, in->parent->concepts, "CELL.X0");
    int y0in = psMetadataLookupS32(NULL, in->parent->concepts, "CELL.Y0");
    int x0sub = psMetadataLookupS32(NULL, sub->parent->concepts, "CELL.X0");
    int y0sub = psMetadataLookupS32(NULL, sub->parent->concepts, "CELL.Y0");

    if ((inImage->numCols + x0in - x0sub) > subImage->numCols) {
        psError(PS_ERR_UNKNOWN, true, "Image does not have enough columns for subtraction.\n");
        return false;
    }
    if ((inImage->numRows + y0in - y0sub) > subImage->numRows) {
        psError(PS_ERR_UNKNOWN, true, "Image does not have enough rows for subtraction.\n");
        return false;
    }

    if (scale == 1.0) {
        for (int i = 0; i < inImage->numRows; i++) {
            for (int j = 0; j < inImage->numCols; j++) {
                inImage->data.F32[i][j] -= subImage->data.F32[i+y0in-y0sub][j+x0in-x0sub];
                if (inMask && subMask) {
                    inMask->data.U8[i][j] |= subMask->data.U8[i+y0in-y0sub][j+x0in-x0sub];
                }
            }
        }
    } else {
        for (int i = 0; i < inImage->numRows; i++) {
            for (int j = 0; j < inImage->numCols; j++) {
                inImage->data.F32[i][j] -= subImage->data.F32[i+y0in-y0sub][j+x0in-x0sub] * scale;
                if (inMask && subMask) {
                    inMask->data.U8[i][j] |= subMask->data.U8[i+y0in-y0sub][j+x0in-x0sub];
                }
            }
        }
    }

    return true;
}


#if 0
/******************************************************************************
ImageSubtractScalar(): subtract a scalar from the input image.
 
XXX: Use a psLib function for this.
 
XXX: This should
 *****************************************************************************/
static psImage *ImageSubtractScalar(psImage *image,
                                    psF32 scalar)
{
    for (psS32 i=0;i<image->numRows;i++) {
        for (psS32 j=0;j<image->numCols;j++) {
            image->data.F32[i][j]-= scalar;
        }
    }
    return(image);
}
#endif

/******************************************************************************
GenNewStatOptions(): this routine will take as input the options member of the
stat data structure, determine if multiple options have been specified, issue
a warning message if so, and return the highest priority option (according to
the order of the if-statements in this code).  The higher priority options are
listed lower in the code.
 *****************************************************************************/
static psStatsOptions GenNewStatOptions(const psStats *stat)
{
    psS32 numOptions = 0;
    psStatsOptions opt = 0;

    if (stat->options & PS_STAT_ROBUST_MEDIAN) {
        if (numOptions == 0) {
            opt = PS_STAT_ROBUST_MEDIAN;
        }
        numOptions++;
    }

    if (stat->options & PS_STAT_CLIPPED_MEAN) {
        if (numOptions == 0) {
            opt = PS_STAT_CLIPPED_MEAN;
        }
        numOptions++;
    }

    if (stat->options & PS_STAT_SAMPLE_MEDIAN) {
        if (numOptions == 0) {
            opt = PS_STAT_SAMPLE_MEDIAN;
        }
        numOptions++;
    }

    if (stat->options & PS_STAT_SAMPLE_MEAN) {
        numOptions++;
        opt = PS_STAT_SAMPLE_MEAN;
    }


    if (numOptions == 0) {
        psError(PS_ERR_UNKNOWN,true, "No statistics options have been specified.\n");
    }
    if (numOptions != 1) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "WARNING: pmSubtractBias.c: GenNewStatOptions(): Too many statistics options have been specified\n");
    }
    return(opt);
}



#if 0
/******************************************************************************
ScaleOverscanVector(): this routine takes as input an arbitrary vector,
creates a new vector of length n, and fills the new vector with the
interpolated values of the old vector.  The type of interpolation is:
    PM_FIT_POLYNOMIAL: fit a polynomial to the entire input vector data.
    PM_FIT_SPLINE: fit splines to the input vector data.
XXX: Doesn't it make more sense to do polynomial interpolation on a few
elements of the input vector, rather than fit a polynomial to the entire
vector?
 *****************************************************************************/
static psVector *ScaleOverscanVector(psVector *overscanVector,
                                     psS32 n,
                                     void *fitSpec,
                                     pmFit fit)
{
    psTrace(".psModule.pmSubtracBias.ScaleOverscanVector", 4,
            "---- ScaleOverscanVector() begin (%d -> %d) ----\n", overscanVector->n, n);
    //    PS_VECTOR_PRINT_F32(overscanVector);

    if (NULL == overscanVector) {
        return(overscanVector);
    }

    // Allocate the new vector.
    psVector *newVec = psVectorAlloc(n, PS_TYPE_F32);

    //
    // If the new vector is the same size as the old, simply copy the data.
    //
    if (n == overscanVector->n) {
        for (psS32 i = 0 ; i < n ; i++) {
            newVec->data.F32[i] = overscanVector->data.F32[i];
        }
        return(newVec);
    }
    psPolynomial1D *myPoly;
    psSpline1D *mySpline;
    psF32 x;
    psS32 i;
    if (fit == PM_FIT_POLYNOMIAL) {
        // Fit a polynomial to the old overscan vector.
        myPoly = (psPolynomial1D *) fitSpec;
        PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
        myPoly = psVectorFitPolynomial1D(myPoly, NULL, 0, overscanVector, NULL, NULL);
        if (myPoly == NULL) {
            psError(PS_ERR_UNKNOWN, false, "ScaleOverscanVector()(1): Could not fit a polynomial to the psVector.\n");
            return(NULL);
        }

        // For each element of the new vector, convert the x-ordinate to that
        // of the old vector, use the fitted polynomial to determine the
        // interpolated value at that point, and set the new vector.
        for (i=0;i<n;i++) {
            x = ((psF32) i) * ((psF32) overscanVector->n) / ((psF32) n);
            newVec->data.F32[i] = psPolynomial1DEval(myPoly, x);
        }
    } else if (fit == PM_FIT_SPLINE) {
        psS32 mustFreeSpline = 0;
        // Fit a spline to the old overscan vector.
        mySpline = (psSpline1D *) fitSpec;
        // XXX: Does it make any sense to have a psSpline argument?
        if (mySpline == NULL) {
            mustFreeSpline = 1;
        }

        //
        // NOTE: Since the X arg in the psVectorFitSpline1D() function is NULL,
        // splines endpoints will be from 0.0 to overscanVector->n-1.  Must scale
        // properly when doing the spline eval.
        //
        //        mySpline = psVectorFitSpline1D(mySpline, NULL, overscanVector, NULL);
        mySpline = psVectorFitSpline1D(NULL, overscanVector);
        if (mySpline == NULL) {
            psError(PS_ERR_UNKNOWN, false, "ScaleOverscanVector()(2): Could not fit a spline to the psVector.\n");
            return(NULL);
        }
        //        PS_PRINT_SPLINE(mySpline);

        // For each element of the new vector, convert the x-ordinate to that
        // of the old vector, use the fitted polynomial to determine the
        // interpolated value at that point, and set the new vector.
        for (i=0;i<n;i++) {
            // Scale to [0 : overscanVector->n - 1]
            x = ((psF32) i) * ((psF32) (overscanVector->n-1)) / ((psF32) n);
            newVec->data.F32[i] = psSpline1DEval(mySpline, x);
        }
        if (mustFreeSpline ==1) {
            psFree(mySpline);
        }
        //        PS_VECTOR_PRINT_F32(newVec);


    } else {
        psError(PS_ERR_UNKNOWN, true, "unknown fit type.  Returning NULL.\n");
        psFree(newVec);
        return(NULL);
    }

    psTrace(".psModule.pmSubtracBias.ScaleOverscanVector", 4,
            "---- ScaleOverscanVector() exit ----\n");
    return(newVec);
}

#endif

// Produce an overscan vector from an array of pixels
static psVector *overscanVector(pmOverscanOptions *overscanOpts, // Overscan options
                                const psArray *pixels, // Array of vectors containing the pixel values
                                psStats *myStats // Statistic to use in reducing the overscan
                               )
{
    // Reduce the overscans
    psVector *reduced = psVectorAlloc(pixels->n, PS_TYPE_F32); // Overscan for each row
    psVector *ordinate = psVectorAlloc(pixels->n, PS_TYPE_F32); // Ordinate
    psVector *mask = psVectorAlloc(pixels->n, PS_TYPE_U8); // Mask for fitting
    for (int i = 0; i < pixels->n; i++) {
        psVector *values = pixels->data[i]; // Vector with overscan values
        if (values->n > 0) {
            mask->data.U8[i] = 0;
            ordinate->data.F32[i] = 2.0*(float)i/(float)pixels->n - 1.0; // Scale to [-1,1]
            psVectorStats(myStats, values, NULL, NULL, 0);
            double reducedVal = NAN; // Result of statistics
            if (! p_psGetStatValue(myStats, &reducedVal)) {
                psError(PS_ERR_UNKNOWN, false, "p_psGetStatValue(): could not determine result "
                        "of statistics on row %d.\n", i);
                return NULL;
            }
            reduced->data.F32[i] = reducedVal;
        } else if (overscanOpts->fitType == PM_FIT_NONE) {
            psError(PS_ERR_UNKNOWN, true, "The overscan is not supplied for all points on the "
                    "image, and no fit is requested.\n");
            return NULL;
        } else {
            // We'll fit this one out
            mask->data.U8[i] = 1;
        }
    }

    // Fit the overscan, if required
    switch (overscanOpts->fitType) {
    case PM_FIT_NONE:
        // No fitting --- that's easy.
        break;
    case PM_FIT_POLY_ORD:
        if (overscanOpts->poly && (overscanOpts->poly->nX != overscanOpts->order ||
                                   overscanOpts->poly->type != PS_POLYNOMIAL_ORD)) {
            psFree(overscanOpts->poly);
            overscanOpts->poly = NULL;
        }
        if (! overscanOpts->poly) {
            overscanOpts->poly = psPolynomial1DAlloc(PS_POLYNOMIAL_ORD, overscanOpts->order);
        }
        psVectorFitPolynomial1D(overscanOpts->poly, mask, 1, reduced, NULL, ordinate);
        psFree(reduced);
        reduced = psPolynomial1DEvalVector(overscanOpts->poly, ordinate);
        break;
    case PM_FIT_POLY_CHEBY:
        if (overscanOpts->poly && (overscanOpts->poly->nX != overscanOpts->order ||
                                   overscanOpts->poly->type != PS_POLYNOMIAL_CHEB)) {
            psFree(overscanOpts->poly);
            overscanOpts->poly = NULL;
        }
        if (! overscanOpts->poly) {
            overscanOpts->poly = psPolynomial1DAlloc(PS_POLYNOMIAL_CHEB, overscanOpts->order);
        }
        psVectorFitPolynomial1D(overscanOpts->poly, mask, 1, reduced, NULL, ordinate);
        psFree(reduced);
        reduced = psPolynomial1DEvalVector(overscanOpts->poly, ordinate);
        break;
    case PM_FIT_SPLINE:
        // XXX I don't think psSpline1D is up to scratch yet --- it has no mask, and requires an
        // input spline
        overscanOpts->spline = psVectorFitSpline1D(reduced, ordinate);
        psFree(reduced);
        reduced = psSpline1DEvalVector(overscanOpts->spline, ordinate);
        break;
    default:
        psError(PS_ERR_UNKNOWN, true, "Unknown value for the fitting type: %d\n", overscanOpts->fitType);
        return NULL;
        break;
    }

    psFree(ordinate);
    psFree(mask);

    return reduced;
}



/******************************************************************************
XXX: The SDRS does not specify type support.  F32 is implemented here.
 *****************************************************************************/
pmReadout *pmSubtractBias(pmReadout *in, pmOverscanOptions *overscanOpts,
                          const pmReadout *bias, const pmReadout *dark)
{
    psTrace(".psModule.pmSubtracBias.pmSubtractBias", 4,
            "---- pmSubtractBias() begin ----\n");
    PS_ASSERT_READOUT_NON_NULL(in, NULL);
    PS_ASSERT_READOUT_NON_EMPTY(in, NULL);
    PS_ASSERT_READOUT_TYPE(in, PS_TYPE_F32, NULL);

    psImage *image = in->image;         // The input image

    // Overscan processing
    if (overscanOpts) {
        // Check for an unallowable pmFit.
        if (overscanOpts->fitType != PM_FIT_NONE && overscanOpts->fitType != PM_FIT_POLY_ORD &&
                overscanOpts->fitType != PM_FIT_POLY_CHEBY && overscanOpts->fitType != PM_FIT_SPLINE) {
            psError(PS_ERR_UNKNOWN, true, "Invalid fit type (%d).  Returning original image.\n", overscanOpts->fitType);
            return(in);
        }

        psList *overscans = in->bias; // List of the overscan images

        psStats *myStats = psStatsAlloc(PS_STAT_SAMPLE_MEAN); // A new psStats, to avoid clobbering original
        myStats->options = GenNewStatOptions(overscanOpts->stat);

        // Reduce all overscan pixels to a single value
        if (overscanOpts->single) {
            psVector *pixels = psVectorAlloc(0, PS_TYPE_F32);
            pixels->n = 0;
            psListIterator *iter = psListIteratorAlloc(overscans, PS_LIST_HEAD, false); // Iterator
            psImage *overscan = NULL;   // Overscan image from iterator
            while ((overscan = psListGetAndIncrement(iter))) {
                int index = pixels->n;  // Index
                pixels = psVectorRealloc(pixels, pixels->n + overscan->numRows * overscan->numCols);
                // XXX Reimplement with memcpy
                for (int i = 0; i < overscan->numRows; i++) {
                    for (int j = 0; j < overscan->numCols; j++) {
                        pixels->data.F32[index++] = overscan->data.F32[i][j];
                    }
                }

            }
            psFree(iter);

            (void)psVectorStats(myStats, pixels, NULL, NULL, 0);
            double reduced = NAN;     // Result of statistics
            if (! p_psGetStatValue(myStats, &reduced)) {
                psError(PS_ERR_UNKNOWN, false, "p_psGetStatValue(): could not determine result from requested statistical operation.  Returning input image.\n");
                return(in);
            }
            (void)psBinaryOp(image, image, "-", psScalarAlloc((float)reduced, PS_TYPE_F32));
        } else {

            // We do the regular overscan subtraction

            bool readRows = psMetadataLookupBool(NULL, in->parent->concepts, "CELL.READDIR");// Read direction

            if (readRows) {
                // The read direction is rows
                psArray *pixels = psArrayAlloc(image->numRows); // Array of vectors containing pixels
                for (int i = 0; i < pixels->n; i++) {
                    psVector *values = psVectorAlloc(0, PS_TYPE_F32);
                    values->n = 0;
                    pixels->data[i] = values;
                }

                // Pull the pixels out into the vectors
                psListIterator *iter = psListIteratorAlloc(overscans, PS_LIST_HEAD, false); // Iterator
                psImage *overscan = NULL; // Overscan image from iterator
                while ((overscan = psListGetAndIncrement(iter))) {
                    int diff = image->row0 - overscan->row0; // Offset between the two regions
                    for (int i = MAX(0,diff); i < MIN(image->numRows, overscan->numRows + diff); i++) {
                        // i is row on overscan
                        // XXX Reimplement with memcpy
                        psVector *values = pixels->data[i];
                        int index = values->n; // Index in the vector
                        values = psVectorRealloc(values, values->n + overscan->numCols);
                        for (int j = 0; j < overscan->numCols; j++) {
                            values->data.F32[index++] = overscan->data.F32[i][j];
                        }
                        values->n += overscan->numCols;
                        pixels->data[i] = values; // Update the pointer in case it's moved
                    }
                }
                psFree(iter);

                // Reduce the overscans
                psVector *reduced = overscanVector(overscanOpts, pixels, myStats);
                psFree(pixels);
                if (! reduced) {
                    return in;
                }

                // Subtract row by row
                for (int i = 0; i < image->numRows; i++) {
                    for (int j = 0; j < image->numCols; j++) {
                        image->data.F32[i][j] -= reduced->data.F32[i];
                    }
                }
                psFree(reduced);

            } else {
                // The read direction is columns
                psArray *pixels = psArrayAlloc(image->numCols); // Array of vectors containing pixels
                for (int i = 0; i < pixels->n; i++) {
                    psVector *values = psVectorAlloc(0, PS_TYPE_F32);
                    values->n = 0;
                    pixels->data[i] = values;
                }

                // Pull the pixels out into the vectors
                psListIterator *iter = psListIteratorAlloc(overscans, PS_LIST_HEAD, false); // Iterator
                psImage *overscan = NULL; // Overscan image from iterator
                while ((overscan = psListGetAndIncrement(iter))) {
                    int diff = image->col0 - overscan->col0; // Offset between the two regions
                    for (int i = MAX(0,diff); i < MIN(image->numCols, overscan->numCols + diff); i++) {
                        // i is column on overscan
                        // XXX Reimplement with memcpy
                        psVector *values = pixels->data[i];
                        int index = values->n; // Index in the vector
                        values = psVectorRealloc(values, values->n + overscan->numRows);
                        for (int j = 0; j < overscan->numRows; j++) {
                            values->data.F32[index++] = overscan->data.F32[i][j];
                        }
                        values->n += overscan->numRows;
                        pixels->data[i] = values; // Update the pointer in case it's moved
                    }
                }
                psFree(iter);

                // Reduce the overscans
                psVector *reduced = overscanVector(overscanOpts, pixels, myStats);
                psFree(pixels);
                if (! reduced) {
                    return in;
                }

                // Subtract column by column
                for (int i = 0; i < image->numCols; i++) {
                    for (int j = 0; j < image->numRows; j++) {
                        image->data.F32[j][i] -= reduced->data.F32[i];
                    }
                }
                psFree(reduced);
            }
        }
        psFree(myStats);
    } // End of overscan subtraction

    // Bias frame subtraction
    if (bias) {
        SubtractFrame(in, bias, 1.0);
    }

    if (dark) {
        // Get the scaling
        float inTime = psMetadataLookupF32(NULL, in->parent->concepts, "CELL.DARKTIME");
        float darkTime = psMetadataLookupF32(NULL, dark->parent->concepts, "CELL.DARKTIME");
        SubtractFrame(in, dark, inTime/darkTime);
    }

    return in;
}


