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

#include <stdio.h>
#include <pslib.h>

#include "pmHDU.h"
#include "pmFPA.h"
#include "pmNonLinear.h"

pmReadout *pmNonLinearityPolynomial(pmReadout *inputReadout, const psPolynomial1D *input1DPoly)
{
    PS_ASSERT_PTR_NON_NULL(inputReadout, NULL);
    PS_ASSERT_PTR_NON_NULL(inputReadout->image, NULL);
    PS_ASSERT_IMAGE_TYPE(inputReadout->image, PS_TYPE_F32, NULL);
    PS_ASSERT_PTR_NON_NULL(input1DPoly, NULL);

    psImage *image = inputReadout->image; // Image to correct
    for (int i = 0; i < image->numRows; i++) {
        for (int j = 0; j < image->numCols; j++) {
            image->data.F32[i][j] = psPolynomial1DEval(input1DPoly, image->data.F32[i][j]);
        }
    }
    return inputReadout;
}

// set the bin closest to the corresponding value.  
#define PS_BIN_FOR_VALUE(RESULT, VECTOR, VALUE) { \
       	psVectorBinaryDisectResult result; \
       	psScalar tmpScalar; \
       	tmpScalar.type.type = PS_TYPE_F32; \
	tmpScalar.data.F32 = (VALUE); \
	RESULT = psVectorBinaryDisect (&result, VECTOR, &tmpScalar); \
	switch (result) { \
	  case PS_BINARY_DISECT_PASS: \
            break; \
	  case PS_BINARY_DISECT_OUTSIDE_RANGE: \
            numPixels ++; \
	    break; \
	  case PS_BINARY_DISECT_INVALID_INPUT: \
	  case PS_BINARY_DISECT_INVALID_TYPE: \
	    psAbort ("programming error"); \
	    break; \
        } }

pmReadout *pmNonLinearityLookup(pmReadout *inputReadout, const psVector *inFlux, const psVector *outFlux)
{
    PS_ASSERT_PTR_NON_NULL(inputReadout, NULL);
    PS_ASSERT_PTR_NON_NULL(inputReadout->image, NULL);
    PS_ASSERT_IMAGE_TYPE(inputReadout->image, PS_TYPE_F32, NULL);
    PS_ASSERT_PTR_NON_NULL(inFlux, NULL);
    if (inFlux->n < 2) {
        psError(PS_ERR_UNKNOWN, true,
                "pmNonLinearityLookup(): input vector less than 2 elements.  Returning inputReadout image.");
        return(inputReadout);
    }
    PS_ASSERT_PTR_NON_NULL(outFlux,NULL);
    psS32 tableSize = inFlux->n;
    if (inFlux->n != outFlux->n) {
        tableSize = PS_MIN(inFlux->n, outFlux->n);
        psLogMsg(__func__, PS_LOG_WARN,
                 "WARNING: pmNonLinear.c: pmNonLinearityLookup(): "
                 "input vectors have different sizes (%ld, %ld)\n",
                 inFlux->n, outFlux->n);
    }
    PS_ASSERT_VECTOR_TYPE(inFlux, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_TYPE(outFlux, PS_TYPE_F32, NULL);

    psImage *image = inputReadout->image; // Input image
    int numPixels = 0;                  // Number of pixels outside the range
    int binNum;

    for (int i = 0; i < image->numRows; i++) {
        for (int j = 0; j < image->numCols; j++) {
	    float value = image->data.F32[i][j];
            PS_BIN_FOR_VALUE(binNum, inFlux, value);

	    // Perform linear interpolation.
	    // XXX this will result in non-sensical results if inFlux contains equal-value
	    // bins.  either enforce d(inFlux)/d(binNum) > 0 or see psStats.c PS_BIN_INTERPOLATE
	    float slope = 
		(outFlux->data.F32[binNum + 1] - outFlux->data.F32[binNum]) /
		(inFlux->data.F32[binNum + 1] - inFlux->data.F32[binNum]);
	    image->data.F32[i][j] = slope*(value - inFlux->data.F32[binNum]) + outFlux->data.F32[binNum];
        }
    }
    if (numPixels > 0) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "WARNING: pmNonLinear.c: pmNonLinearityLookup(): %d pixels outside table.", numPixels);
    }
    return inputReadout;
}
