#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdarg.h>
#include <float.h>
#include <math.h>
#include "psMemory.h"
#include "psVector.h"
#include "psTrace.h"
#include "psError.h"
#include "psAbort.h"
#include "psStats.h"
#include "psImage.h"
#include "psFunctions.h"
#include "float.h"
#include <math.h>

/// This routine must determine the various statistics for the image.
/*****************************************************************************
    NOTE: verify that image/mask have the correct types, and sizes.
 *****************************************************************************/
psStats *psImageStats(psStats *stats,
                      psImage *in,
                      psImage *mask,
                      int maskVal)
{
    psVector *junkData=NULL;
    psVector *junkMask=NULL;
    int ptr = 0;
    int i = 0;
    int j = 0;

    // NOTE: Verify this action.
    if ((stats == NULL) ||
            (in == NULL)) {
        return(NULL);
    }
    // NOTE: Verify this action.
    if (stats->options == 0) {
        return(stats);
    }

    junkData = psVectorAlloc(in->numRows * in->numCols, in->type.type);
    junkData->n = junkData->nalloc;
    ptr=0;
    for (i=0;i<in->numRows;i++) {
        for (j=0;j<in->numCols;j++) {
            junkData->data.F32[ptr++] = in->data.F32[i][j];
        }
    }

    if (mask != NULL) {
        // NOTE: verify that mask data is PS_TYPE_U8.
        // NOTE: figure out mask types

        junkMask = psVectorAlloc(mask->numRows * mask->numCols,
                                 mask->type.type);
        junkMask->n = junkMask->nalloc;
        // NOTE: Is there a more efficient way to do this?
        ptr=0;
        for (i=0;i<mask->numRows;i++) {
            for (j=0;j<mask->numCols;j++) {
                junkMask->data.U8[ptr++] = mask->data.U8[i][j];
            }
        }

        stats = psVectorStats(stats, junkData, junkMask, maskVal);
        psFree(junkMask);
    } else {
        stats = psVectorStats(stats, junkData, NULL, 0);
    }
    psFree(junkData);
    return(stats);
}

/*****************************************************************************
    NOTE: We assume that the psHistogram structure out has already been
    allocated and initialized.
    NOTE: verify that image/mask have the, correct types and  sizes.
 *****************************************************************************/
psHistogram *psImageHistogram(psHistogram *out,
                              psImage *in,
                              psImage *mask,
                              int maskVal)
{
    psVector *junkData=NULL;
    psVector *junkMask=NULL;
    int ptr = 0;
    int i = 0;
    int j = 0;

    // NOTE: Verify this action.
    if ((out == NULL) ||
            (in == NULL)) {
        return(NULL);
    }

    junkData = psVectorAlloc(in->numRows * in->numCols, in->type.type);
    junkData->n = junkData->nalloc;

    // NOTE: Is there a more efficient way to do this?  memcopy() won't work.
    // Can we trick the junkData structure to use the image buffer, then
    // untrick it before we deallocate it (so we won't deallocate that buffer
    // twice?

    // NOTE: Make sure you have the numRows/NumCols in the right place.
    ptr=0;
    for (i=0;i<in->numRows;i++) {
        for (j=0;j<in->numCols;j++) {
            junkData->data.F32[ptr++] = in->data.F32[i][j];
        }
    }

    if (mask != NULL) {
        // NOTE: verify that mask data is PS_TYPE_U8.
        // NOTE: figure out mask types

        junkMask = psVectorAlloc(mask->numRows * mask->numCols,
                                 mask->type.type);
        junkMask->n = junkMask->nalloc;

        // NOTE: Is there a more efficient way to do this?
        ptr=0;
        for (i=0;i<mask->numRows;i++) {
            for (j=0;j<mask->numCols;j++) {
                junkMask->data.U8[ptr++] = mask->data.U8[i][j];
            }
        }
        out = psHistogramVector(out, junkData, junkMask, maskVal);
        psFree(junkMask);
    } else {
        out = psHistogramVector(out, junkData, NULL, 0);
    }
    psFree(junkData);

    return(out);
}

float *p_psCalcScaleFactorsFit(int n)
{
    int i = 0;
    float tmp = 0.0;
    float *scalingFactors = (float *) psAlloc(n * sizeof(float));

    for (i=0;i<n;i++) {
        //     ((2.0 * (float) i) / ((float) (n-1))) - 1.0;
        //        tmp = (float) (i + 1);
        tmp = (float) (n - i);
        tmp = (M_PI * (tmp - 0.5)) / ((float) n);
        scalingFactors[i] = cos(tmp);
    }

    return(scalingFactors);
}

/*****************************************************************************
p_psCalcScaleFactorsEval(n): The Chebyshev polynomials are defined over the
interval [-1.0 : 1.0].  Images typically have sizes of 512x512 or more.  In
order to use Chebyshev polynomials, we must scales the coordinates from
0:512 to -1:1.  This routine takes as input an integer N and produces as
output a vector of evenly spaced floating point values between -1.0:1.0.
 *****************************************************************************/
float *p_psCalcScaleFactorsEval(int n)
{
    int i = 0;
    float tmp = 0.0;
    return p_psCalcScaleFactorsFit(n);

    printf("Should not get here\n");
    float *scalingFactors = (float *) psAlloc(n * sizeof(float));
    for (i=0;i<n;i++) {
        //          scalingFactors[i] = ((2.0 * (float) i) / ((float) (n-1))) - 1.0;
        tmp = (float) (n - i);
        tmp = (M_PI * (tmp - 0.5)) / ((float) n);
        scalingFactors[i] = cos(tmp);
    }
    return(scalingFactors);
}

psPolynomial1D **p_psCreateChebyshevPolys(int maxChebyPoly)
{
    psPolynomial1D **chebPolys = NULL;
    int i = 0;
    int j = 0;

    chebPolys = (psPolynomial1D **) psAlloc(maxChebyPoly *
                                            sizeof(psPolynomial1D *));
    for (i=0;i<maxChebyPoly;i++) {
        chebPolys[i] = psPolynomial1DAlloc(i+1);
    }

    // Create the Chebyshev polynomials.  Polynomial i has i-th order.
    chebPolys[0]->coeff[0] = 1;
    chebPolys[1]->coeff[1] = 1;
    for (i=2;i<maxChebyPoly;i++) {
        for (j=0;j<chebPolys[i-1]->n;j++) {
            chebPolys[i]->coeff[j+1] = 2 * chebPolys[i-1]->coeff[j];
        }
        for (j=0;j<chebPolys[i-2]->n;j++) {
            chebPolys[i]->coeff[j]-= chebPolys[i-2]->coeff[j];
        }
    }

    return(chebPolys);
}


/*****************************************************************************
psImageFitPolynomial(): This routine takes as input a 2-D image and produces
as output the coefficients of the Chebyshev polynomials which match that
input image.
  Input:
  Output:
  Internal Data Structures:
    chebPolys[i][j] 
    sums[i][j]: This will contain the sum of 
                input->data.F32[x][y] *
                psEvalPolynomial1D((float) x, chebPolys[i]) *
                psEvalPolynomial1D((float) y, chebPolys[j]);
        over all pixels (x,y) in the image.
 *****************************************************************************/
psPolynomial2D *
psImageFitPolynomial(const psImage *input,
                     psPolynomial2D *coeffs)
{
    int x = 0;
    int y = 0;
    int i = 0;
    int j = 0;
    float **sums = NULL;
    psPolynomial1D **chebPolys = NULL;
    int maxChebyPoly = 0;
    float *cScalingFactors = NULL;
    float *rScalingFactors = NULL;
    float tmp = 0.0;

    // Create the sums[][] data structure.  This will hold the LHS of equation
    // 29 in the ADD: sums[k][l] = SUM { image(x,y) * Tk(x) * Tl(y) }
    sums = (float **) psAlloc(coeffs->nX * sizeof(float *));
    for (i=0;i<coeffs->nX;i++) {
        sums[i] = (float *) psAlloc(coeffs->nY * sizeof(float));
    }

    // We scale the pixel positions to values between -1.0 and 1.0
    rScalingFactors = p_psCalcScaleFactorsFit(input->numRows);
    cScalingFactors = p_psCalcScaleFactorsFit(input->numCols);

    // Determine how many Chebyshev polynomials are needed, then create them.
    maxChebyPoly = coeffs->nX;
    if (coeffs->nY > coeffs->nX) {
        maxChebyPoly = coeffs->nY;
    }
    chebPolys = p_psCreateChebyshevPolys(maxChebyPoly);

    // Sanity check for the Chebyshevs.
    for (i=0;i<coeffs->nX;i++) {
        for (j=0;j<coeffs->nY;j++) {
            tmp = 0.0;
            for (x=0;x<input->numRows;x++) {
                tmp+= psEvalPolynomial1D(rScalingFactors[x], chebPolys[i]) *
                      psEvalPolynomial1D(rScalingFactors[x], chebPolys[j]);

            }
            //printf("SUM(Cheby(%d) * Cheby(%d)) is %f\n", i, j, tmp);
        }
    }

    // Compute the sums[][] data structure.
    for (i=0;i<coeffs->nX;i++) {
        for (j=0;j<coeffs->nY;j++) {
            sums[i][j] = 0.0;
            for (x=0;x<input->numRows;x++) {
                for (y=0;y<input->numCols;y++) {
                    sums[i][j]+= input->data.F32[x][y] *
                                 psEvalPolynomial1D(rScalingFactors[x], chebPolys[i]) *
                                 psEvalPolynomial1D(cScalingFactors[y], chebPolys[j]);
                }
            }
        }
    }

    for (i=0;i<coeffs->nX;i++) {
        for (j=0;j<coeffs->nY;j++) {
            coeffs->coeff[i][j] = sums[i][j];
            coeffs->coeff[i][j]/= (float) (input->numRows * input->numCols);

            if ((i != 0) && (j != 0)) {
                coeffs->coeff[i][j]*= 4.0;
            } else if ((i == 0) && (j == 0)) {
                coeffs->coeff[i][j]*= 1.0;
            } else {
                coeffs->coeff[i][j]*= 2.0;
            }
        }
    }

    // Free the Chebyshev polynomials that were created in this routine.
    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);

    // Free some data
    for (i=0;i<coeffs->nX;i++) {
        psFree(sums[i]);
    }
    psFree(sums);
    psFree(cScalingFactors);
    psFree(rScalingFactors);

    return(coeffs);
}

/*****************************************************************************
 
 *****************************************************************************/
int
psImageEvalPolynomial(const psImage *input,
                      const psPolynomial2D *coeffs)
{
    int x = 0;
    int y = 0;
    int i = 0;
    int j = 0;
    float **sums = NULL;
    psPolynomial1D **chebPolys = NULL;
    int maxChebyPoly = 0;
    float *cScalingFactors = NULL;
    float *rScalingFactors = NULL;
    float polySum = 0.0;

    // Create the sums[][] data structure.  This will hold the LHS of equation
    // 29 in the ADD: sums[k][l] = SUM { image(x,y) * Tk(x) * Tl(y) }
    sums = (float **) psAlloc(coeffs->nX * sizeof(float *));
    for (i=0;i<coeffs->nX;i++) {
        sums[i] = (float *) psAlloc(coeffs->nY * sizeof(float));
    }
    for (i=0;i<coeffs->nX;i++) {
        for (j=0;j<coeffs->nY;j++) {
            sums[i][j] = 0.0;
        }
    }

    // We scale the pixel positions to values between -1.0 and 1.0
    rScalingFactors = p_psCalcScaleFactorsEval(input->numRows);
    cScalingFactors = p_psCalcScaleFactorsEval(input->numCols);

    // Determine how many Chebyshev polynomials are needed, then create them.
    maxChebyPoly = coeffs->nX;
    if (coeffs->nY > coeffs->nX) {
        maxChebyPoly = coeffs->nY;
    }

    chebPolys = p_psCreateChebyshevPolys(maxChebyPoly);

    for (x=0;x<input->numRows;x++) {
        for (y=0;y<input->numCols;y++) {
            polySum = 0.0;
            for (i=0;i<coeffs->nX;i++) {
                for (j=0;j<coeffs->nY;j++) {
                    polySum+= psEvalPolynomial1D(rScalingFactors[x], chebPolys[i]) *
                              psEvalPolynomial1D(cScalingFactors[y], chebPolys[j]) *
                              coeffs->coeff[i][j];

                }
            }
            input->data.F32[x][y] = polySum;
        }
    }

    // Free the Chebyshev polynomials that were created in this routine.
    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);

    // Free some data
    for (i=0;i<coeffs->nX;i++) {
        psFree(sums[i]);
    }
    psFree(sums);
    psFree(cScalingFactors);
    psFree(rScalingFactors);

    return(0);
}

/*****************************************************************************
p_psImagePixelInterpolation(image, x, y): this routine takes as input an
image and coordinates (x, y) and produces as output the corresponding pixel
value at the those coordinates.  For fractional corrdinates (x, y), 2-D
linear interpolation is performed on the image.
 *****************************************************************************/
float psImagePixelInterpolation(psImage *input,
                                float x,
                                float y)
{
    float floorX = 0.0;
    float floorY = 0.0;
    float fracX = 0.0;
    float fracY = 0.0;
    int intFloorX = 0;
    int intFloorY = 0;
    float x1 = 0.0;
    float x2 = 0.0;
    float pixel = 0.0;

    if ((x < 0.0) ||
            (x > ((float) input->numRows-1)) ||
            (y < 0.0) ||
            (y > ((float) input->numCols-1))) {
        psAbort(__func__,
                "Fractional coordinates (%f %f) outside image range.", x, y);
    }

    floorX = floorf(x);
    intFloorX = (int) floorX;
    fracX = x - floorX;

    floorY = floorf(y);
    intFloorY = (int) floorY;
    fracY = y - floorY;

    if (intFloorX == (input->numRows-1)) {
        pixel = input->data.F32[intFloorX][intFloorY];
        if (intFloorY < (input->numCols-1)) {
            pixel+= fracY * (input->data.F32[intFloorX][intFloorY+1] -
                             input->data.F32[intFloorX][intFloorY]);
        }
        return(pixel);
    } else if (intFloorX == (input->numCols-1)) {
        if (intFloorX < (input->numRows-1)) {
            pixel+= fracX * (input->data.F32[intFloorX+1][intFloorY] -
                             input->data.F32[intFloorX][intFloorY]);
        }
        return(pixel);
    }


    x1 = input->data.F32[intFloorX][intFloorY];
    x1+= fracY * (input->data.F32[intFloorX][intFloorY+1] -
                  input->data.F32[intFloorX][intFloorY]);

    x2 = input->data.F32[intFloorX+1][intFloorY];
    x2+= fracY * (input->data.F32[intFloorX+1][intFloorY+1] -
                  input->data.F32[intFloorX+1][intFloorY]);
    pixel = x1;
    pixel+= fracX * (x2 - x1);

    return(pixel);
}




