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

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


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
    psScalar x;                         // Value at pixel, for p_psVectorBinDisect
    x.type.type = PS_TYPE_F32;
    int numPixels = 0;                  // Number of pixels outside the range

    for (int i = 0; i < image->numRows; i++) {
        for (int j = 0; j < image->numCols; j++) {
            x.data.F32 = image->data.F32[i][j];
            int binNum = p_psVectorBinDisect((psVector *)inFlux, &x); // Bin below the value

            if (binNum == -2) {
                // We get here if x is below the table lookup range.
                image->data.F32[i][j] = outFlux->data.F32[0];
                numPixels++;
            } else if (binNum == -1) {
                // We get here if x is above the table lookup range.
                image->data.F32[i][j] = outFlux->data.F32[tableSize-1];
                numPixels++;
            } else if (binNum < -2) {
                // We get here if there was some other problem.
                psError(PS_ERR_UNKNOWN, true,
                        "pmNonLinearityLookup(): Could not perform p_psVectorBinDisect().  "
                        "Returning inputReadout image.");
                return inputReadout;
                numPixels++;
            } else {
                // Perform linear interpolation.
                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] = outFlux->data.F32[binNum] +
                                        (x.data.F32 - inFlux->data.F32[binNum]) * slope;
            }
        }
    }
    if (numPixels > 0) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "WARNING: pmNonLinear.c: pmNonLinearityLookup(): %d pixels outside table.", numPixels);
    }
    return inputReadout;
}
