/*******************************************************************************
 * JMMC project ( http://www.jmmc.fr ) - Copyright (C) CNRS.
 ******************************************************************************/
/**
 * @file
 * Special functions used by AMBER observation software. 
 */
#define _POSIX_SOURCE 1
#define amdlibREMANENCE 0.00

/*
 * System Headers
 */
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <errno.h>
#include "fitsio.h"
#include <time.h>

/*
 * Local Headers
 */
#include "amdlib.h"
#include "amdlibProtected.h"



/** Max number of peaks along Y axis */
#define amdlibMAX_NB_SPEC 15
/** Max number of peaks along X axis */
#define amdlibMAX_NB_PEAKS 10

static int amdlibFindPeakPosition(double *data, int nbData, double threshold,
                                  int nbMaxPeaks, double *peakPos, 
                                  double *peakWeight, double *peakMax);


/**
 * Determine the positions of the columns
 *     
 * This function finds the position of the columns. This consists to find
 * the position, along the X axis, of the peak for each column. If several peaks
 * are found, this function computes the mean weighted position of the peaks. If
 * raw data has more than one row, the mean position of the peaks in the rows
 * for each column is returned. If no peak is found the column position is set
 * to 0.
 * This function returns, in addition to the column positions, the max value
 * found for each column.
 *
 * @param rawData structure where raw data is stored
 * @param colPos position of the peak found for each column. 
 * @param maxVal max value found for each column.
 *
 * @return
 * Always return amdlibSUCCESS
 */
amdlibCOMPL_STAT amdlibExtractColPos(amdlibRAW_DATA *rawData,
                                     amdlibDOUBLE colPos[amdlibMAX_NB_COLS],
                                     amdlibDOUBLE maxVal[amdlibMAX_NB_COLS])
{
    int col;
    int row;
    int iRegion;

    int x, y, frame;
    int nbX,nbY,nbFrames;

    double meanX[512];
    double pixelOffset[512];
    double mean;
    double variance;
    double threshold;

    amdlibLogTrace("amdlibExtractColPos()");
    
    /* Check raw data structure contains data */
    if (rawData->dataLoaded == amdlibFALSE)
    {
        return amdlibFAILURE;
    }

    /* Position of column 0 (i.e. dark) is not computed. Always set to 0 */
    colPos[0] = 0.0;

    /* For each column (excepted column 0 which is dark) */
    for(col=1; col<rawData->nbCols; col++)
    {
        double position[amdlibMAX_NB_ROWS][amdlibMAX_NB_PEAKS];
        double weight[amdlibMAX_NB_ROWS][amdlibMAX_NB_PEAKS];
        double max[amdlibMAX_NB_ROWS][amdlibMAX_NB_PEAKS];
        double meanPos;
        int    nbPos;

        /* For each row */
        for(row=0; row<rawData->nbRows; row++)
        {
            /* Get the region index and size */
            iRegion  = row*rawData->nbCols + col;
            nbX      = rawData->region[iRegion].dimAxis[0];
            nbY      = rawData->region[iRegion].dimAxis[1];
            nbFrames = rawData->region[iRegion].dimAxis[2];

            /* Compute mean value along X axis */
            for (x=0; x<nbX; x++)
            {
                double sum;

                sum  = 0.;
                pixelOffset[x] = rawData->region[iRegion].corner[0];
                for (y=0; y<nbY; y++)
                {
                    for (frame=0; frame<nbFrames; frame++)
                    {
                        sum += rawData->region[iRegion].data[frame*nbX*nbY+y*nbX+x];
                    }
                }
                meanX[x] = sum / (double)(nbY*nbFrames);
            }

            /* Compute the threshold used to find peaks according to the mean
             * value and variance of all pixels in region */
            mean      =0.0;
            variance  =0.0;
            threshold =0.0;
            for(x=0; x<nbX; x++)
            {
                mean     += meanX[x];
                variance += meanX[x]*meanX[x];
            }
            mean     /= (double)(nbX);
            variance -= (double)(nbX) * mean * mean ;
            variance  = variance /(double)(nbX-1);
            variance  = sqrt(variance );

            /* If the image is flat */
            if (variance < 10.0)
            {
                /* Do not search for peak */
                position[row][0] = 0.0;
            }
            else
            {

                threshold = mean + variance  ;   /* 4 sigma */
                nbPos=amdlibFindPeakPosition (meanX, nbX, threshold,
                                              amdlibMAX_NB_PEAKS,
                                              position[row], weight[row],
                                              max[row]);
                if (nbPos != 0)
                {
                    double w = 0.;
                    double wSum = 0.;
                    double m = 0.;
                    int pos;

                    for (pos=0; pos<nbPos; pos++)
                    {
                        w += weight[row][pos];
                        wSum += weight[row][pos] * position[row][pos];
                        if (m < max[row][pos])
                        {
                            m = max[row][pos];
                        }
                    }
                    position[row][0] = wSum/w;
                    max[row][0] = m;
                }
                else
                {
                    position[row][0] = 0.0;
                    max[row][0] = 0.0;
                }
            }
        }

        /* Compute the mean position and max value of the column */
        meanPos = 0.0;
        maxVal[col] = 0.0;
        nbPos = 0;
        for (row=0; row<rawData->nbRows; row++)
        {
            if (position[row][0] != 0.0)
            {
                meanPos += position[row][0];
                nbPos++;
                maxVal[col] += max[row][0];
            }
        }
        if (nbPos != 0)
        {
            meanPos /= nbPos;
            colPos[col] = pixelOffset[lround(meanPos)]+meanPos;
        }
        else
        {
            colPos[col] = 0.0;
        }
    }
    return amdlibSUCCESS;
}

/**
 * Determine the positions of the columns
 * Determine the position of the peak along Y axis
 *     
 * This function finds, for each region, the central spectral position. This
 * consists to find the position, along the Y axis, of the peak for each region.
 * If no peak is found the spectral position is set to 0.
 *
 * @param rawData structure where raw data is stored
 * @param spectPos position of the peak found for each region. 
 *
 * @return
 * Always return amdlibSUCCESS
 */
amdlibCOMPL_STAT amdlibExtractSpectPos(amdlibRAW_DATA *rawData,
                                       amdlibDOUBLE spectPos[amdlibMAX_NB_COLS][amdlibMAX_NB_ROWS])
{
    int col;
    int row;
    int iRegion;

    int x, y, frame;
    int nbX,nbY,nbFrames;

    double meanY[512];
    double startPixel[512];
    double mean;
    double variance;
    double threshold;

    amdlibLogTrace("amdlibExtractSpectPos()");
    
    /* Check raw data structure contains data */
    if (rawData->dataLoaded == amdlibFALSE)
    {
        return amdlibFAILURE;
    }

    /* Position of column 0 (i.e. dark) is not computed. Always set to 0 */
    for(row=0; row<rawData->nbRows; row++)
    {
        spectPos[0][row] = 0;
    }

    /* For each column (excepted column 0 which is dark) */
    for(col=1; col<rawData->nbCols; col++)
    {
        int    nbPosition[amdlibMAX_NB_ROWS];
        double position[amdlibMAX_NB_ROWS][amdlibMAX_NB_SPEC];
        double weight[amdlibMAX_NB_ROWS][amdlibMAX_NB_SPEC];
        double max[amdlibMAX_NB_ROWS][amdlibMAX_NB_PEAKS]; /* Not used */

        /* For each row */
        memset(position, '\0', sizeof(position));
        for(row=0; row<rawData->nbRows; row++)
        {
            /* Get the region index and size */
            iRegion = row*rawData->nbCols + col;
            nbX      = rawData->region[iRegion].dimAxis[0];
            nbY      = rawData->region[iRegion].dimAxis[1];
            nbFrames = rawData->region[iRegion].dimAxis[2];

            /* Compute mean value along X axis */
            for (y=0; y<nbY; y++)
            {
                double sum;

                sum  = 0.;
                startPixel[y] = rawData->region[iRegion].corner[1];
                for (x=0; x<nbX; x++)
                {
                    for (frame=0; frame<nbFrames; frame++)
                    {
                        sum +=
                        rawData->region[iRegion].data[frame*nbX*nbY+y*nbX+x];
                    }
                }
                meanY[y] = sum / (double)(nbX*nbFrames);
            }
            /* Compute the threshold used to find peaks according to the mean
             * value and variance of all pixels in region */
            mean      =0.0;
            variance  =0.0;
            threshold =0.0;
            for(y=0; y<nbY; y++)
            {
                mean     += meanY[y];
                variance += meanY[y]*meanY[y];
            }
            mean      /= (double)(nbY);
            variance  -= (double)(nbY)*mean *mean ;
            variance  = variance /(double)(nbY-1);
            variance  = sqrt(variance );

            /* If the image is flat */
            if (variance < 10.0)
            {
                /* Do not search for peak */
                nbPosition[row] = 0;
            }
            else
            {
                threshold = mean + (2. * variance);   /* 4 sigma */
                nbPosition[row] = amdlibFindPeakPosition
                    (meanY, nbY, threshold, amdlibMAX_NB_SPEC, position[row],
                     weight[row], max[row]);
            }
        }

        /* Get the max peak positions on detector */
        for (row=0; row<rawData->nbRows; row++)
        {
            int i, iMax;
            double max;

            if (nbPosition[row] != 0)
            {
                /* Find max peak */
                iMax = 0;
                max = meanY[lround(position[row][iMax])];
                for (i=0;i<nbPosition[row];i++)
                {
                    if (meanY[lround(position[row][i])] >
                        meanY[lround(position[row][iMax])])
                    {
                        iMax = i;
                        max = meanY[lround(position[row][iMax])];
                    }
                }

                spectPos[col][row] = startPixel[lround(position[row][iMax])] +
                position[row][iMax];
            }
            else
            {
                spectPos[col][row] = 0.0;
            }
        }
    }
    return amdlibSUCCESS;
}

/**
 * Find peaks in the given value array. 
 *
 * This function finds peaks (i.e. greater than the given threshold value),
 * and compute the weight of each peak.
 *
 * @param data table of values.
 * @param nbData number of values in table. 
 * @param threshold value used to detect peaks 
 * @param nbMaxPeaks number of peaks which can be found; i.e. size of the two
 * following arrays.
 * @param peakPos position of the found peaks
 * @param peakWeight weight of the found peaks
 * @param peakMax maximum value of the found peaks
 *
 * @return
 * The number of the found peaks
 */
int amdlibFindPeakPosition(double *data, int nbData, double threshold,
                           int nbMaxPeaks, double *peakPos, double *peakWeight,
                           double *peakMax)
{
    int    i, nbPeaks = 0;
    double delta;
    double weight = 0.;
    double weightedSum = 0.;
    double max = 0.;

    amdlibBOOLEAN peakFound = amdlibFALSE;

    amdlibLogTrace("amdlibFindPeakPosition()");
    
#if 0
    /* Print out command lines to visualize data using yorick program */
    amdlibLogTrace("data=[");
    for (i = 0; i < nbData; i++)
    {
        amdlibLogTrace("%.1f",  data[i]);
        if(i!=(nbData-1))
            amdlibLogTrace(",");
    }
    amdlibLogTrace("]");
    amdlibLogTrace("thr=array(amdlibDOUBLE(%.1f), dimsof(data)(2))", threshold);
    amdlibLogTrace("plg,data");
    amdlibLogTrace("plg,thr,color=\"red\"");
#endif

    /* For each value in table */
    for (i = 0; i < nbData; i++)
    {
        delta = data[i] - threshold;

        /* If value is greater than threshold */
        if (delta > 0.0)
        {
            /* Update weighted sum and weight */
            weight      += delta;
            weightedSum += i * delta;
            peakFound   = amdlibTRUE;
            /* Update max value */
            if (max < data[i])
            {
                max = data[i];
            }
        }
        /* End if */

        /* If value is lower than threshold (or it is the last value) and peak
         * has been found, corresponding to the end of peak */
        if (((delta <= 0.) || (i == (nbData-1))) && (peakFound == amdlibTRUE))
        {
            /* Compute position of peak (barycenter) */
            peakPos[nbPeaks]    = weightedSum / weight;
            peakWeight[nbPeaks] = weight;
            peakMax[nbPeaks]    = max;

            nbPeaks++;
            /* If max number of peaks is reached */
            if(nbPeaks >= nbMaxPeaks)
            {
                /* Stop */
                return nbPeaks;
            }
            /* End if */

            weightedSum = 0.0;
            weight = 0.0;
            max = 0.0;
            peakFound = amdlibFALSE;
        }
        /* End if */
    }
    /* End for */
    return nbPeaks;
}

/**
 * Split glued image into data regions
 *
 * This function split the glued image into regions. It is assumed that regions
 * is organised in rows and column, as shown below:
 * \code
 *  +----+-------+-------+------+
 * |    |       |       |      |
 * |    |       |       |      |
 * |    |       |       |      |
 * +----+-------+-------+------+
 * |    |       |       |      |
 * |    |       |       |      |
 * +----+-------+-------+------+
 * \endcode
 *
 * The detector configuration is given by the nbRows and nbCols parameters,
 * which respectively defines the number of rows and columns, and therefore the
 * number of regions. The dimensions of each regions are given by the colWidth
 * and rowHeight parameters.

 * @param gluedImage pointer to the glued image
 * @param nbRows number of rows
 * @param nbCols number of column
 * @param colWidth width of the regions for each column 
 * @param rowHeight height of the regions for each row 
 * @param rawData structure where raw data will be stored
 * @param errMsg error description message returned if function fails.
 *
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibGluedImage2RawData(amdlibDOUBLE *gluedImage,
                                          int nbRows, int nbCols, 
                                          int colWidth[amdlibMAX_NB_COLS],
                                          int rowHeight[amdlibMAX_NB_ROWS],
                                          amdlibRAW_DATA *rawData,
                                          amdlibERROR_MSG errMsg)
{
    amdlibBOOLEAN detCfgChanged;
    int iRow, iCol, iY;
    int iRegion;
    int nbOfElements;
    int startPos;

    amdlibLogTrace("amdlibGluedImage2RawData()");
    
    /* If raw data structure is not initailized, do it */
    if (rawData->thisPtr != rawData)
    {
        amdlibInitRawData(rawData);
    }

    /* Check the detector configuration against region configuration */
    detCfgChanged = amdlibFALSE;
    if ((nbRows * nbCols) != rawData->nbRegions)
    {
        detCfgChanged = amdlibTRUE;
    }
    else
    {
        for (iRow=0; iRow < nbRows ; iRow++)
        {
            for (iCol = 0; iCol < nbCols; iCol++)
            {
                iRegion = iRow *  nbCols + iCol;
                if ((rawData->region[iRegion].dimAxis[0] != colWidth[iCol]) ||
                    (rawData->region[iRegion].dimAxis[1] != rowHeight[iRow]))
                {
                    detCfgChanged = amdlibTRUE;
                }
            }
        }
    }

    /* If detector configuration has changed, free and allocate new data
     * structure */
    if (detCfgChanged)
    {
        /* Free memory */
        amdlibReleaseRawData(rawData);

        /* Allocate memory for region array */
        rawData->nbCols=nbCols;
        rawData->nbRows=nbRows;
        rawData->nbRegions=nbRows * nbCols;
        rawData->nbFrames = 1;
       /* Allocate memory for time Tag (1 value) */
        rawData->timeTag = calloc(rawData->nbFrames,sizeof(*(rawData->timeTag)));
        if (amdlibAllocateRegions
            (&rawData->region, rawData->nbRegions) != amdlibSUCCESS)
        {
            amdlibSetErrMsg("Could not allocate memory for regions");
            return amdlibFAILURE;
        }

        /* Allocate memory for regions */
        for (iRow = 0; iRow < nbRows ; iRow++)
        {
            for (iCol = 0; iCol < nbCols; iCol++)
            {
                iRegion = iRow * nbCols + iCol;
                nbOfElements = colWidth[iCol] * rowHeight[iRow];
                rawData->region[iRegion].data =
                calloc( nbOfElements, sizeof(amdlibDOUBLE));
                if (rawData->region[iRegion].data == NULL)
                {
                    amdlibSetErrMsg("Could not allocate memory for data of "
                                    "region %d (size %dx%d)", iRegion,
                                    colWidth[iCol], rowHeight[iRow]);
                    return amdlibFAILURE;
                }
                rawData->region[iRegion].dimAxis[0]=colWidth[iCol];
                rawData->region[iRegion].dimAxis[1]=rowHeight[iRow];
                rawData->region[iRegion].dimAxis[2]=rawData->nbFrames;
            }
        }
        /* Allocate rms structure */
        if (amdlibAllocateRegions
            (&rawData->variance, rawData->nbRegions) != amdlibSUCCESS)
        {
            amdlibSetErrMsg("Could not allocate memory for rms regions");
            return amdlibFAILURE;
        }

        /* Allocate memory for variance */
        for (iRow = 0; iRow < nbRows ; iRow++)
        {
            for (iCol = 0; iCol < nbCols; iCol++)
            {
                iRegion = iRow * nbCols + iCol;
                nbOfElements = colWidth[iCol] * rowHeight[iRow];
                rawData->variance[iRegion].data =
                calloc( nbOfElements, sizeof(amdlibDOUBLE));
                if (rawData->variance[iRegion].data == NULL)
                {
                    amdlibSetErrMsg("Could not allocate memory for data of "
                                    "variance region %d (size %dx%d)", iRegion,
                                    colWidth[iCol], rowHeight[iRow]);
                    return amdlibFAILURE;
                }
                rawData->variance[iRegion].dimAxis[0]=colWidth[iCol];
                rawData->variance[iRegion].dimAxis[1]=rowHeight[iRow];
                rawData->variance[iRegion].dimAxis[2]=1;
            }
        }
    }

    /* Split glued image into region data */
    startPos = 0;
    for (iRow = 0; iRow < nbRows ; iRow++)
    {
        for (iY = 0; iY < rowHeight[iRow]; iY++)
        {
            for (iCol = 0; iCol < nbCols; iCol++)
            {
                iRegion = iRow *  nbCols + iCol;

                memcpy(&rawData->region[iRegion].data[iY*colWidth[iCol]],
                       &gluedImage[startPos], colWidth[iCol] * sizeof(amdlibDOUBLE));
                startPos += colWidth[iCol];
            }
        }
    }
    /*Fill in a Time Tag*/
    rawData->timeTag[0] = time(NULL) / 86400.0; /*One frame*/

    /* Inform the data is loaded in structure, but there is not calibrated */
    rawData->dataLoaded = amdlibTRUE;
    rawData->dataCalibrated = amdlibFALSE;

    /* No reference to P2VM */
    rawData->p2vmId = 0;

    return amdlibSUCCESS;
}
