//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2006-08-29 21:39:44 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 *
 */

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

#include <stdio.h>
#include <assert.h>
#include <pslib.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

    int xIpar = psMetadataLookupS32(NULL, in->parent->concepts, "CELL.XPARITY");
    int xSpar = psMetadataLookupS32(NULL, sub->parent->concepts, "CELL.XPARITY");
    if (xIpar != xSpar) {
        psError(PS_ERR_UNKNOWN, true, "images for subtraction do not have the same "
                "CELL.XPARITY (%d vs %d).\n    pmSubtractBias must be upgraded to handle this situation\n",
                xIpar, xSpar);
        return false;
    }

    int yIpar = psMetadataLookupS32(NULL, in->parent->concepts, "CELL.YPARITY");
    int ySpar = psMetadataLookupS32(NULL, sub->parent->concepts, "CELL.YPARITY");
    if (yIpar != ySpar) {
        psError(PS_ERR_UNKNOWN, true, "images for subtraction do not have the same "
                "CELL.YPARITY (%d vs %d).\n    pmSubtractBias must be upgraded to handle this situation\n",
                xIpar, xSpar);
        return false;
    }

    // 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;
}

// 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
                               )
{
    assert(overscanOpts);
    assert(pixels);
    assert(myStats);

    psStatsOptions statistic = psStatsSingleOption(myStats->options); // Statistic to use
    assert(statistic != 0);

    // 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
    reduced->n = ordinate->n = mask->n = pixels->n;

    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);
            reduced->data.F32[i] = psStatsGetValue(myStats, statistic);
        } 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.
 *****************************************************************************/
bool pmSubtractBias(pmReadout *in, pmOverscanOptions *overscanOpts,
                    const pmReadout *bias, const pmReadout *dark)
{
    psTrace("psModules.detrend", 4,
            "---- pmSubtractBias() begin ----\n");
    PS_ASSERT_PTR_NON_NULL(in, NULL);
    PS_ASSERT_IMAGE_NON_NULL(in->image, NULL);
    PS_ASSERT_IMAGE_TYPE(in->image, PS_TYPE_F32, NULL);
    if (bias) {
        PS_ASSERT_IMAGE_NON_NULL(bias->image, NULL);
        PS_ASSERT_IMAGE_TYPE(bias->image, PS_TYPE_F32, NULL);
    }
    if (dark) {
        PS_ASSERT_IMAGE_NON_NULL(dark->image, NULL);
        PS_ASSERT_IMAGE_TYPE(dark->image, 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 false;
        }

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

        psStatsOptions statistic = psStatsSingleOption(overscanOpts->stat->options); // Statistic to use
        if (statistic == 0) {
            psError(PS_ERR_BAD_PARAMETER_VALUE, false, "Multiple or no statistics options set: %p\n",
                    overscanOpts->stat);
            return false;
        }
        psStats *stats = psStatsAlloc(statistic); // A new psStats, to avoid clobbering original

        // Reduce all overscan pixels to a single value
        if (overscanOpts->single) {
            psVector *pixels = psVectorAlloc(0, PS_TYPE_F32);
            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(stats, pixels, NULL, NULL, 0);
            double reduced = psStatsGetValue(stats, statistic); // Result of statistics
            (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
                pixels->n = image->numRows;
                for (int i = 0; i < pixels->n; i++) {
                    psVector *values = psVectorAlloc(0, PS_TYPE_F32);
                    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, stats);
                psFree(pixels);
                if (! reduced) {
                    return false;
                }

                // 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
                pixels->n = image->numCols;
                for (int i = 0; i < pixels->n; i++) {
                    psVector *values = psVectorAlloc(0, PS_TYPE_F32);
                    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, stats);
                psFree(pixels);
                if (! reduced) {
                    return false;
                }

                // 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(stats);
    } // End of overscan subtraction

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

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

    return true;
}


