/*******************************************************************************
 * JMMC project ( http://www.jmmc.fr ) - Copyright (C) CNRS.
 ******************************************************************************/
/**
 * @file
 * Useful functions related to piston computation.
 */

#define _POSIX_SOURCE 1

/*
 * 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"

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


#define amdlibMAX_PISTON2_ITERATIONS 50
#define amdlibPISTONPRECISION 1E-2
#define amdlibMAX_SWEEP_SIZE_FOR_PISTON 33
/*
 * Local function declaration
 */
double amdlibAbacusErrPhi(double x);
amdlibCOMPL_STAT amdlibQsortDouble(double *pix_arr, int npix);
amdlibCOMPL_STAT amdlibQsortDoubleIndexed(double *pix_arr, int *index, int npix);
static void amdlibFreePiston(amdlibPISTON *piston);
void amdlibComputePiston2T(int                    nbWlen,
                           amdlibDOUBLE           *Wave,
                           amdlibCOMPLEX          *x,
                           amdlibCOMPLEX          *s2x,
                           amdlibDOUBLE           *pistonOPD,
                           amdlibDOUBLE           *sigma,
                           double wlenAvg,
                           double wlenDifAvg,
                           double R);
/*
 * Public functions
 */
/**
 * Release memory allocated to store pistons, and reset the structure members
 * to zero.
 *
 * @param opd pointer to amdlibPISTON structure.
 */
void amdlibReleasePiston(amdlibPISTON *opd)
{
    amdlibLogTrace("amdlibReleasePiston()");
    
    amdlibFreePiston(opd);
    memset (opd, '\0', sizeof(amdlibPISTON));
}

/*
 * Protected functions
 */
/**
 * Allocate memory for storing pistons. 
 *
 * @param piston pointer to amdlibPISTON structure.
 * @param nbFrames number of frames 
 * @param nbBases number of baselines 
 *
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibAllocatePiston(amdlibPISTON *piston,
                                      const int    nbFrames,
                                      const int    nbBases)
{
    int band;
    
    amdlibLogTrace("amdlibAllocatePiston()");
    
    /* First free previous allocated memory */
    if (piston->thisPtr == piston)
    {
        amdlibFreePiston(piston);
    }

    /* Init data structure */
    piston->thisPtr = memset(piston, '\0', sizeof(*piston));
    /* Set array size */
    piston->nbFrames = nbFrames;
    piston->nbBases  = nbBases;
    for (band = amdlibJ_BAND; band <= amdlibK_BAND; band++)
    {
        piston->bandFlag[band] = amdlibFALSE;
        piston->pistonOPDArray[band] = calloc(nbFrames*nbBases, sizeof(amdlibDOUBLE));
        piston->sigmaPistonArray[band] = 
            calloc(nbFrames*nbBases, sizeof(amdlibDOUBLE));
        if ((piston->pistonOPDArray[band] == NULL) ||
            (piston->sigmaPistonArray[band] == NULL))
        {
            amdlibFreePiston(piston);
            return amdlibFAILURE;
        }
    }
    piston->pistonOPD = calloc(nbFrames*nbBases, sizeof(amdlibDOUBLE));
    piston->sigmaPiston = calloc(nbFrames*nbBases, sizeof(amdlibDOUBLE));
    if ((piston->pistonOPD == NULL) || (piston->sigmaPiston == NULL))
    {
        amdlibFreePiston(piston);
        return amdlibFAILURE;
    }
 
    /* Return */
    return amdlibSUCCESS;
}

/**
 * Append amdlibPISTON data structure. 
 *
 * @param srcOpd pointer to source data structure 
 * @param dstOpd pointer to destination data structure  (append to) 
 * @param errMsg error description message returned if function fails.
 *
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibAppendPiston(amdlibPISTON    *dstOpd, 
                                    amdlibPISTON    *srcOpd, 
                                    amdlibERROR_MSG errMsg)
{
    int dentry;
    int sentry;
    int band;

    amdlibLogTrace("amdlibAppendPiston()");

    /* Perform simple check */
    if (dstOpd->nbBases != srcOpd->nbBases)
    {
        amdlibSetErrMsg("Different number of bases");
        return amdlibFAILURE;
    }

    /* Reallocate memory to store additional data */
    for (band = amdlibJ_BAND; band <= amdlibK_BAND; band++)
    {
        dstOpd->pistonOPDArray[band] = realloc(dstOpd->pistonOPDArray[band], 
            (dstOpd->nbFrames + srcOpd->nbFrames) * 
            srcOpd->nbBases * sizeof(amdlibDOUBLE));
        dstOpd->sigmaPistonArray[band] = realloc(dstOpd->sigmaPistonArray[band],            
            (dstOpd->nbFrames + srcOpd->nbFrames) * 
            srcOpd->nbBases * sizeof(amdlibDOUBLE));
        if ((dstOpd->pistonOPDArray[band] == NULL) ||
            (dstOpd->sigmaPistonArray[band] == NULL))
        {
            amdlibSetErrMsg("Could not reallocate memory for piston structure");
            return amdlibFAILURE;
        }
    }
    dstOpd->pistonOPD = realloc(dstOpd->pistonOPD, 
                    (dstOpd->nbFrames + srcOpd->nbFrames) * 
                    srcOpd->nbBases * sizeof(amdlibDOUBLE));
    dstOpd->sigmaPiston = realloc(dstOpd->sigmaPiston,
                    (dstOpd->nbFrames + srcOpd->nbFrames) * 
                    srcOpd->nbBases * sizeof(amdlibDOUBLE));
    if ((dstOpd->pistonOPD == NULL) || (dstOpd->sigmaPiston == NULL))
    {
        amdlibSetErrMsg("Could not reallocate memory for piston structure");
        return amdlibFAILURE;
    }
    
    /* Append fields allocated dynamically */
    dentry = dstOpd->nbFrames * dstOpd->nbBases;

    /* Band Flag sameness insured if check is made beforehand
     * on the sameness of Wlens. No use to update it. */

    for (sentry = 0; sentry < srcOpd->nbFrames * srcOpd->nbBases; sentry++)
    {
        for (band=amdlibJ_BAND; band <= amdlibK_BAND; band++)
        {
            dstOpd->pistonOPDArray[band][dentry] = 
                srcOpd->pistonOPDArray[band][sentry];
            dstOpd->sigmaPistonArray[band][dentry] = 
                srcOpd->sigmaPistonArray[band][sentry];
        }
        dstOpd->pistonOPD[dentry] = srcOpd->pistonOPD[sentry];
        dstOpd->sigmaPiston[dentry] = srcOpd->sigmaPiston[sentry];
        dentry++;
    }
    
    /* Append fixed size fields */
    dstOpd->nbFrames += srcOpd->nbFrames;
    return amdlibSUCCESS;
}


/**
 * Insert amdlibPISTON data structure. 
 *
 * @param srcOpd pointer to source data structure 
 * @param dstOpd pointer to destination data structure  (insert to) 
 * @param insertIndex
 * @param errMsg error description message returned if function fails.
 *
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibInsertPiston(amdlibPISTON    *dstOpd, 
                                    amdlibPISTON    *srcOpd, 
                                    int insertIndex,
                                    amdlibERROR_MSG errMsg)
{
    int dentry;
    int sentry;
    int band;
    int dstNbFrames = dstOpd->nbFrames;
    int finalIndex = insertIndex + srcOpd->nbFrames;

    amdlibLogTrace("amdlibInsertPiston()");

    /* Perform simple check */
    if (insertIndex<0 || insertIndex>=dstOpd->nbFrames)
    {
        amdlibSetErrMsg("Invalid insertion index %d for amdlibInsertVis2",insertIndex);
        return amdlibFAILURE;
    }
    if (dstOpd->nbBases != srcOpd->nbBases)
    {
        amdlibSetErrMsg("Different number of bases");
        return amdlibFAILURE;
    }
    if (dstNbFrames < finalIndex)
    {
        amdlibSetErrMsg("Number of frames (%d) in destination structure"
                        "too small to enable insertion of %d frames at position %d", 
                        dstNbFrames,srcOpd->nbFrames,insertIndex);
        return amdlibFAILURE;
    }

    /* Insert src in dst at frame insertIndex */
    dentry = insertIndex * dstOpd->nbBases;

    for (band=amdlibJ_BAND; band <= amdlibK_BAND; band++)
    {
        /* Band Flag sameness insured if check is made beforehand
         * on the sameness of Wlens. Update it since it it void at start. */
        dstOpd->bandFlag[band]=srcOpd->bandFlag[band];
    }

    for (sentry = 0; sentry < srcOpd->nbFrames * srcOpd->nbBases; sentry++)
    {
        for (band=amdlibJ_BAND; band <= amdlibK_BAND; band++)
        {
            dstOpd->pistonOPDArray[band][dentry] = 
                srcOpd->pistonOPDArray[band][sentry];
            dstOpd->sigmaPistonArray[band][dentry] = 
                srcOpd->sigmaPistonArray[band][sentry];
        }
        dstOpd->pistonOPD[dentry] = srcOpd->pistonOPD[sentry];
        dstOpd->sigmaPiston[dentry] = srcOpd->sigmaPiston[sentry];
        dentry++;
    }

    return amdlibSUCCESS;
}


/**); 
 * Merge amdlibPISTON data structure. 
 *
 * @param opd1 pointer on merged piston structure.
 * @param opd2 pointer on piston structure to merge.
 * @param errMsg error description message returned if function fails.
 *
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibMergePiston(amdlibPISTON    *opd1, 
                                   amdlibPISTON    *opd2, 
                                   amdlibERROR_MSG errMsg)
{
    int band;
    
    amdlibLogTrace("amdlibMergePiston()");

    memcpy(opd1->pistonOPD, opd2->pistonOPD, 
           opd2->nbFrames * opd2->nbBases * sizeof(amdlibDOUBLE));
    memcpy(opd1->sigmaPiston, opd2->sigmaPiston, 
           opd2->nbFrames * opd2->nbBases * sizeof(amdlibDOUBLE));
    for (band=amdlibJ_BAND; band <= amdlibK_BAND; band++)
    {
        if (opd2->bandFlag[band] == amdlibTRUE)
        {
            if (opd1->bandFlag[band] == amdlibTRUE)
            {
                amdlibSetErrMsg("Same information in pistons to merge");
                return amdlibFAILURE;
            }
            opd1->bandFlag[band] = amdlibTRUE;
            memcpy(opd1->pistonOPDArray[band], opd2->pistonOPDArray[band],
                   opd1->nbFrames * opd1->nbBases * sizeof(amdlibDOUBLE));
            memcpy(opd1->sigmaPistonArray[band], opd2->sigmaPistonArray[band],
                   opd1->nbFrames * opd1->nbBases * sizeof(amdlibDOUBLE));
            return amdlibSUCCESS;
        }
    }
    
    amdlibSetErrMsg("Problem whith opd to merge: doesn't contain value");
    return amdlibFAILURE;
}

/**
 * Split amdlibPISTON input data structure into 3 structures, 1 per spectral
 * band. Each new structure only contains information relative to its associated
 * spectral band .
 *
 * @param srcOpd pointer on piston structure to split.
 * @param dstOpd array of splitted piston structures.
 * @param nbWlen array containing the number of wavelengths per spectral band.
 * @param errMsg error description message returned if function fails.
 *
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibSplitPiston(amdlibPISTON    *srcOpd,
                                   amdlibPISTON    *dstOpd,
                                   int             *nbWlen,
                                   amdlibERROR_MSG errMsg)
{
    int band, i;

    amdlibLogTrace("amdlibSplitPiston()");

    for (band=amdlibJ_BAND; band <= amdlibK_BAND; band++)
    {
        if (nbWlen[band] == 0)
        {
            dstOpd[band].thisPtr = NULL;
        }
        else
        {
            /* Allocate memory for dstOpd */
            if (amdlibAllocatePiston(&dstOpd[band], srcOpd->nbFrames, 
                                     srcOpd->nbBases) != amdlibSUCCESS)
            {
                amdlibSetErrMsg("Could not allocate memory for new piston "
                                "structure");
                return amdlibFAILURE;
            }
            
            for (i=amdlibJ_BAND; i <= amdlibK_BAND; i++)
            {
                if (i == band)
                {
                    dstOpd[band].bandFlag[band] = amdlibTRUE;
                }
                else
                {
                    dstOpd[band].bandFlag[i] = amdlibFALSE;
                }
            }
            
            /* Copy fields allocated dynamically */
            memcpy (dstOpd[band].pistonOPDArray[band], 
                    srcOpd->pistonOPDArray[band], 
                    srcOpd->nbFrames * srcOpd->nbBases * sizeof(amdlibDOUBLE));
            memcpy (dstOpd[band].sigmaPistonArray[band], 
                    srcOpd->sigmaPistonArray[band], 
                    srcOpd->nbFrames * srcOpd->nbBases * sizeof(amdlibDOUBLE));

            memcpy (dstOpd[band].pistonOPD, srcOpd->pistonOPD, 
                    srcOpd->nbFrames * srcOpd->nbBases * sizeof(amdlibDOUBLE));
            memcpy (dstOpd[band].sigmaPiston, srcOpd->sigmaPiston, 
                    srcOpd->nbFrames * srcOpd->nbBases * sizeof(amdlibDOUBLE));
        }
    }
   
    return amdlibSUCCESS;
}

/** Useful macro to free all dynamically allocated structures */ 
#define amdlibMeanPiston_FREEALL()                       \
    amdlibFree2DArrayDoubleWrapping(instantOpdPistonPtr); \
    amdlibFree2DArrayDoubleWrapping(instantOpdSigmaPtr);  \
    amdlibFree2DArrayDoubleWrapping(opdPistonPtr);        \
    amdlibFree2DArrayDoubleWrapping(opdSigmaPtr);
/**
 * Compute mean piston. 
 *
 * @param instantOpd pointer to amdlibPISTON input data structure (bin of
 * intantaneous opds).
 * @param band current treated band.
 * @param iBin index where to store result in the output (binned) structure.
 * @param selectedFrames structure containing information relative to frame
 * selection. By default, all frames are selected.
 * @param opd pointer to amdlibPISTON output data structure (binned).
 *
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibMeanPiston(amdlibPISTON    *instantOpd,
                                  amdlibBAND      band,
                                  int             iBin,
                                  amdlibSELECTION *selectedFrames,
                                  amdlibPISTON    *opd)
{
    int          iFrame; /* usually loop on number of frames */
    int          nbFrames = instantOpd->nbFrames;
    int          iBase; /*usually loop on nbBases*/
    int          nbBases = instantOpd->nbBases;
    int nbGoodFrames = 0;
    static amdlibDOUBLE pistonOPD, sigma2;
    amdlibDOUBLE        **instantOpdPistonPtr = NULL;
    amdlibDOUBLE        **opdPistonPtr = NULL;
    amdlibDOUBLE        **instantOpdSigmaPtr = NULL;
    amdlibDOUBLE        **opdSigmaPtr = NULL;
    static amdlibERROR_MSG errMsg;
    double weight;
    
    amdlibLogTrace("amdlibMeanPiston()");

    if (instantOpd->bandFlag[band] == amdlibFALSE)
    {
        amdlibLogError("Piston for band '%d' not ever computed", band);
        amdlibMeanPiston_FREEALL();
        return amdlibFAILURE;
    }
    
    opd->bandFlag[band] = instantOpd->bandFlag[band];

    instantOpdPistonPtr = 
        amdlibWrap2DArrayDouble(instantOpd->pistonOPDArray[band],
                               instantOpd->nbBases,
                               instantOpd->nbFrames, errMsg);
    if (instantOpdPistonPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibMeanPiston_FREEALL();
        return amdlibFAILURE;
    }
    instantOpdSigmaPtr = 
        amdlibWrap2DArrayDouble(instantOpd->sigmaPistonArray[band],
                               instantOpd->nbBases,
                               instantOpd->nbFrames, errMsg);
    if (instantOpdSigmaPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibMeanPiston_FREEALL();
        return amdlibFAILURE;
    }
    opdPistonPtr = amdlibWrap2DArrayDouble(opd->pistonOPDArray[band], 
                                          opd->nbBases, 
                                          opd->nbFrames, errMsg);
    if (opdPistonPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibMeanPiston_FREEALL();
        return amdlibFAILURE;
    }
    opdSigmaPtr = amdlibWrap2DArrayDouble(opd->sigmaPistonArray[band], 
                                         opd->nbBases, 
                                         opd->nbFrames, errMsg);
    if (opdSigmaPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibMeanPiston_FREEALL();
        return amdlibFAILURE;
    }

    if (nbFrames > 1)
    {
        for (iBase = 0; iBase < nbBases; iBase++)
        {
            if (selectedFrames->band[band].nbSelectedFrames[iBase] != 0)
            {
                /* compute mean piston */
                pistonOPD = 0.0; 
                weight = 0;
                for (iFrame = 0, nbGoodFrames = 0; iFrame < nbFrames; iFrame++)
                {
                    if ((selectedFrames->band[band].isSelectedPt[iBase][iFrame] == amdlibTRUE)&&!amdlibCompareDouble(instantOpdPistonPtr[iFrame][iBase],amdlibBLANKING_VALUE))
                    {
                        weight += 1/amdlibPow2(instantOpdSigmaPtr[iFrame][iBase]);
                        pistonOPD += instantOpdPistonPtr[iFrame][iBase] / amdlibPow2(instantOpdSigmaPtr[iFrame][iBase]);
                        nbGoodFrames++;
                    }
                }
                if (nbGoodFrames != 0)
                {
                    pistonOPD /= weight;
                    sigma2 = (1.0/weight);
                    opdPistonPtr[iBin][iBase] = pistonOPD ;
                    opdSigmaPtr[iBin][iBase] = sqrt(sigma2);
                }
                else
                {
                    opdPistonPtr[iBin][iBase] = amdlibBLANKING_VALUE;
                    opdSigmaPtr[iBin][iBase] = amdlibBLANKING_VALUE;
                }
            }
            else
            {
                opdPistonPtr[iBin][iBase] = amdlibBLANKING_VALUE;
                opdSigmaPtr[iBin][iBase] = amdlibBLANKING_VALUE;
            }
        }
    }            
    else /* Do nothing more than copy, only one frame !.*/
    {
        for (iBase = 0; iBase < nbBases; iBase++)
        {
            if (selectedFrames->band[band].nbSelectedFrames[iBase] != 0)
            {
                opdPistonPtr[iBin][iBase] = instantOpdPistonPtr[0][iBase];
                opdSigmaPtr[iBin][iBase] = instantOpdSigmaPtr[0][iBase];
            }
            else
            {
                opdPistonPtr[iBin][iBase] = amdlibBLANKING_VALUE;
                opdSigmaPtr[iBin][iBase] = amdlibBLANKING_VALUE;
            }
        }
    }

    memcpy(opd->pistonOPD, opd->pistonOPDArray[band], 
           opd->nbBases * opd->nbFrames * sizeof(amdlibDOUBLE));
    memcpy(opd->sigmaPiston, opd->sigmaPistonArray[band], 
           opd->nbBases * opd->nbFrames * sizeof(amdlibDOUBLE));
    amdlibMeanPiston_FREEALL();
    return amdlibSUCCESS;
}
#undef amdlibMeanPiston_FREEALL

/** Useful macro to free all dynamically allocated structures */ 
#define amdlibClosePiston_FREEALL()                       \
    amdlibFree2DArrayDoubleWrapping(instantOpdPistonPtr); \
    amdlibFree2DArrayDoubleWrapping(instantOpdSigmaPtr);  

/**
 * Enforces piston closure if useful (sigma > precision). 
 *
 * @param instantOpd pointer to amdlibPISTON input data structure (bin of
 * intantaneous opds).
 * @param band current treated band.
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibClosePiston(amdlibPISTON    *instantOpd,
                                   double           precision,
                                    amdlibBAND      band)
{
    int iFrame; 
    int nbFrames = instantOpd->nbFrames;
    int iBase; 
    int nbBases = instantOpd->nbBases;
    amdlibDOUBLE        **instantOpdPistonPtr = NULL;
    amdlibDOUBLE        **instantOpdSigmaPtr = NULL;
    static amdlibERROR_MSG errMsg;
    double s[amdlibNBASELINE],p[amdlibNBASELINE];
    int  idx[amdlibNBASELINE];

    amdlibLogTrace("amdlibClosePiston()");
    if (instantOpd->bandFlag[band] == amdlibFALSE)
    {
        amdlibLogError("Piston for band '%d' not ever computed", band);
        return amdlibFAILURE;
    }

    if (amdlibGetUserPref(amdlibMAX_PISTON_ERROR).set==amdlibTRUE)
        precision=amdlibGetUserPref(amdlibMAX_PISTON_ERROR).value;

    amdlibLogInfoDetail("Pistons Closure...");
    if (nbBases==3) 
    {
        instantOpdPistonPtr = 
        amdlibWrap2DArrayDouble(instantOpd->pistonOPDArray[band],
                                instantOpd->nbBases,
                                instantOpd->nbFrames, errMsg);
        if (instantOpdPistonPtr == NULL)
        {
            amdlibLogError("amdlibWrap2DArrayDouble() failed !");
            amdlibLogErrorDetail(errMsg);
            amdlibClosePiston_FREEALL();
            return amdlibFAILURE;
        }
        instantOpdSigmaPtr = 
        amdlibWrap2DArrayDouble(instantOpd->sigmaPistonArray[band],
                                instantOpd->nbBases,
                                instantOpd->nbFrames, errMsg);
        if (instantOpdSigmaPtr == NULL)
        {
            amdlibLogError("amdlibWrap2DArrayDouble() failed !");
            amdlibLogErrorDetail(errMsg);
            amdlibClosePiston_FREEALL();
            return amdlibFAILURE;
        }
        for (iFrame = 0;  iFrame < nbFrames; iFrame++)
        {
            /* truth table */
            p[0]=instantOpdPistonPtr[iFrame][2]-instantOpdPistonPtr[iFrame][1];
            p[1]=instantOpdPistonPtr[iFrame][2]-instantOpdPistonPtr[iFrame][0];
            p[2]=instantOpdPistonPtr[iFrame][0]+instantOpdPistonPtr[iFrame][1];
            for (iBase = 0; iBase < nbBases; iBase++)
            {
                s[iBase]=instantOpdSigmaPtr[iFrame][iBase];
                idx[iBase]=iBase;
            }
            amdlibQsortDoubleIndexed(s, idx, nbBases);
            /* avoid blanking */
            if (!(s[0]<=amdlibBLANKING_VALUE)) /*which is a huge neg value, so in 0*/
            {
                /* Do it if it is really significant */
                if ((sqrt(s[0]*s[0]+s[1]*s[1])<2*s[2])&&(sqrt(s[0]*s[0]+s[1]*s[1]))<precision)
                {
                    instantOpdPistonPtr[iFrame][idx[2]]=p[idx[2]];
                    instantOpdSigmaPtr[iFrame][idx[2]]=sqrt(s[0]*s[0]+s[1]*s[1]);
                }
            }
            else /* try to fill blank values */
            {
                instantOpdPistonPtr[iFrame][idx[0]]=p[idx[0]];
                if (!amdlibCompareDouble(s[1],amdlibBLANKING_VALUE)&&!amdlibCompareDouble(s[2],amdlibBLANKING_VALUE))
                    instantOpdSigmaPtr[iFrame][idx[0]]=sqrt(s[2]*s[2]+s[1]*s[1]);
            }
        }
    }
    amdlibClosePiston_FREEALL();
    return amdlibSUCCESS;
}
#undef amdlibClosePiston_FREEALL

/** Useful macro to free all dynamically allocated structures */ 
#define amdlibTagPiston_FREEALL()                       \
    amdlibFree2DArrayDoubleWrapping(instantOpdPistonPtr); \
    amdlibFree2DArrayDoubleWrapping(instantOpdSigmaPtr);  

/**
 * Blanks silly piston values (sigma > threshold). 
 *
 * @param instantOpd pointer to amdlibPISTON input data structure (bin of
 * intantaneous opds).
 * @param band current treated band.
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibTagPiston(amdlibPISTON    *instantOpd,
                                 double            threshold,
                                 double            errThreshold,
                                 amdlibBAND      band)
{
    int iFrame; 
    int nbFrames = instantOpd->nbFrames;
    int iBase; 
    int nbBases = instantOpd->nbBases;
    amdlibDOUBLE        **instantOpdPistonPtr = NULL;
    amdlibDOUBLE        **instantOpdSigmaPtr = NULL;
    static amdlibERROR_MSG errMsg;
    int nbBlank;

    amdlibLogTrace("amdlibTagPiston()");
    if (instantOpd->bandFlag[band] == amdlibFALSE)
    {
        amdlibLogError("Piston for band '%d' not ever computed", band);
        return amdlibFAILURE;
    }

    instantOpdPistonPtr = 
    amdlibWrap2DArrayDouble(instantOpd->pistonOPDArray[band],
                            instantOpd->nbBases,
                            instantOpd->nbFrames, errMsg);
    if (instantOpdPistonPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibTagPiston_FREEALL();
        return amdlibFAILURE;
    }
    instantOpdSigmaPtr = 
    amdlibWrap2DArrayDouble(instantOpd->sigmaPistonArray[band],
                            instantOpd->nbBases,
                            instantOpd->nbFrames, errMsg);
    if (instantOpdSigmaPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibTagPiston_FREEALL();
        return amdlibFAILURE;
    }
    amdlibLogInfoDetail("Pistons Cleanup...");
    /* blank all pistons with sigma > errThreshold 
     * or abs(piston) > threshold*/
    nbBlank=0;
    if (!amdlibCompareDouble(errThreshold,amdlibBLANKING_VALUE))
    {
        for (iFrame = 0;  iFrame < nbFrames; iFrame++)
        {
            for (iBase = 0; iBase < nbBases; iBase++)
            {
                if (instantOpdSigmaPtr[iFrame][iBase] >= errThreshold)
                {
                    instantOpdSigmaPtr[iFrame][iBase] = amdlibBLANKING_VALUE;
                    instantOpdPistonPtr[iFrame][iBase] = amdlibBLANKING_VALUE;
                    nbBlank++;
                }
            }
        }
    }
    if (!amdlibCompareDouble(threshold,amdlibBLANKING_VALUE))
    {
        for (iFrame = 0;  iFrame < nbFrames; iFrame++)
        {
            for (iBase = 0; iBase < nbBases; iBase++)
            {
                if (!!amdlibCompareDouble(instantOpdSigmaPtr[iFrame][iBase],amdlibBLANKING_VALUE))
                {
                    if ((fabs(instantOpdPistonPtr[iFrame][iBase])>=threshold))
                    {
                        instantOpdSigmaPtr[iFrame][iBase] = amdlibBLANKING_VALUE;
                        instantOpdPistonPtr[iFrame][iBase] = amdlibBLANKING_VALUE;
                        nbBlank++;
                    }
                }
            }
        }
    }
    amdlibLogInfoDetail("Tagged %d pistons as bad, according to filter instructions"
                        "(%5.1f %% of total).",nbBlank,
                        (float)(100.0*nbBlank/(nbFrames*nbBases)));

    amdlibTagPiston_FREEALL();
    return amdlibSUCCESS;
}
#undef amdlibTagPiston_FREEALL

/** Useful macro to free all dynamically allocated structures */ 
#define amdlibBinPiston_FREEALL()                       \
    amdlibFree2DArrayDoubleWrapping(instantOpdPistonPtr); \
    amdlibFree2DArrayDoubleWrapping(instantOpdSigmaPtr);  \
    amdlibFree2DArrayDoubleWrapping(opdPistonPtr);        \
    amdlibFree2DArrayDoubleWrapping(opdSigmaPtr);
/**
 * Compute Bin piston. 
 *
 * @param instantOpd pointer to amdlibPISTON input data structure (bin of
 * intantaneous opds).
 * @param band current treated band.
 * @param iBin index where to store result in the output (binned) structure.
 * @param selectedFrames structure containing information relative to frame
 * selection. By default, all frames are selected.
 * @param opd pointer to amdlibPISTON output data structure (binned).
 *
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibBinPiston(amdlibPISTON    *instantOpd,
                                 amdlibBAND      band,
                                 int             firstFrame,
                                 int             nbFrames,
                                 int             iBin,
                                 amdlibPISTON    *opd)
{
    int iFrame; /* usually loop on number of frames */
    int iBase; /*usually loop on nbBases*/
    int nbBases = instantOpd->nbBases;
    int nbGoodFrames = 0;
    static amdlibDOUBLE pistonOPD, sigma2;
    amdlibDOUBLE        **instantOpdPistonPtr = NULL;
    amdlibDOUBLE        **opdPistonPtr = NULL;
    amdlibDOUBLE        **instantOpdSigmaPtr = NULL;
    amdlibDOUBLE        **opdSigmaPtr = NULL;
    static amdlibERROR_MSG errMsg;
    double weight;
    
    amdlibLogTrace("amdlibBinPiston()");

    if (instantOpd->bandFlag[band] == amdlibFALSE)
    {
        amdlibLogError("Piston for band '%d' not ever computed", band);
        amdlibBinPiston_FREEALL();
        return amdlibFAILURE;
    }
    
    opd->bandFlag[band] = instantOpd->bandFlag[band];

    instantOpdPistonPtr = 
        amdlibWrap2DArrayDouble(instantOpd->pistonOPDArray[band],
                               instantOpd->nbBases,
                               instantOpd->nbFrames, errMsg);
    if (instantOpdPistonPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibBinPiston_FREEALL();
        return amdlibFAILURE;
    }
    instantOpdSigmaPtr = 
        amdlibWrap2DArrayDouble(instantOpd->sigmaPistonArray[band],
                               instantOpd->nbBases,
                               instantOpd->nbFrames, errMsg);
    if (instantOpdSigmaPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibBinPiston_FREEALL();
        return amdlibFAILURE;
    }
    opdPistonPtr = amdlibWrap2DArrayDouble(opd->pistonOPDArray[band], 
                                          opd->nbBases, 
                                          opd->nbFrames, errMsg);
    if (opdPistonPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibBinPiston_FREEALL();
        return amdlibFAILURE;
    }
    opdSigmaPtr = amdlibWrap2DArrayDouble(opd->sigmaPistonArray[band], 
                                         opd->nbBases, 
                                         opd->nbFrames, errMsg);
    if (opdSigmaPtr == NULL)
    {
        amdlibLogError("amdlibWrap2DArrayDouble() failed !");
        amdlibLogErrorDetail(errMsg);
        amdlibBinPiston_FREEALL();
        return amdlibFAILURE;
    }

    if (nbFrames > 1)
    {
        for (iBase = 0; iBase < nbBases; iBase++)
        {
            /* compute Binned piston */
            pistonOPD = 0.0; 
            weight = 0;
            for (iFrame = firstFrame, nbGoodFrames = 0; 
                 iFrame < firstFrame+nbFrames; iFrame++)
            {
                if (!amdlibCompareDouble(instantOpdPistonPtr[iFrame][iBase],amdlibBLANKING_VALUE))
                {
                    weight += 1/amdlibPow2(instantOpdSigmaPtr[iFrame][iBase]);
                    pistonOPD += instantOpdPistonPtr[iFrame][iBase] / amdlibPow2(instantOpdSigmaPtr[iFrame][iBase]);
                    nbGoodFrames++;
                }
            }
            if (nbGoodFrames != 0)
            {
                pistonOPD /= weight;
                sigma2 = (1.0/weight);
                opdPistonPtr[iBin][iBase] = pistonOPD ;
                opdSigmaPtr[iBin][iBase] = sqrt(sigma2);
            }
            else
            {
                opdPistonPtr[iBin][iBase] = amdlibBLANKING_VALUE;
                opdSigmaPtr[iBin][iBase] = amdlibBLANKING_VALUE;
            }

        }
    }            
    else /* Do nothing more than copy, only one frame !.*/
    {
        for (iBase = 0; iBase < nbBases; iBase++)
        {
            opdPistonPtr[iBin][iBase] = instantOpdPistonPtr[firstFrame][iBase];
            opdSigmaPtr[iBin][iBase] = instantOpdSigmaPtr[firstFrame][iBase];
        }
    }
    amdlibBinPiston_FREEALL();
    return amdlibSUCCESS;
}
#undef amdlibBinPiston_FREEALL


/** Useful macro to free all dynamically allocated structures */ 
#define amdlibComputePiston_FREEALL() amdlibFree2DArrayWrapping((void **)tablePtr);\
free(C);free(S2C);free(Lambda);
 
/**
 * Compute piston. 
 *
 * This function computes the piston_OPD for each frame and each baseline 
 * starting from the interspectra. Computes relevant sequence of data, and
 * passes them to amdlibComputePiston2T according to the number of baselines.
 * 
 * @param instantCorrFlux instantaneous correlated flux.
 * @param band current treated band.
 * @param wave wavelength data structure used.
 * @param instantOpd resulting computed piston.
 * @param errMsg error description message returned if function fails.
 *
 * @return 
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibComputePiston(amdlibVIS              *instantCorrFlux,
                                     /* instantCorrFlux because it is a subset 
                                      * of the orginal vis */
                                     amdlibBAND             band,
                                     amdlibWAVELENGTH       *wave,
                                     amdlibPISTON           *instantOpd,
                                     amdlibERROR_MSG        errMsg)
{
    int iFrame; /* usually loop on number of frames */
    int nbFrames = instantCorrFlux->nbFrames;
    int iBase;
    int nbBases = instantCorrFlux->nbBases;
    int lVis, okVis;
    int nbLVis = wave->nbWlen;
    int nbTel = 2;
    amdlibVIS_TABLE_ENTRY **tablePtr = NULL;

    amdlibCOMPLEX *C=NULL,*S2C=NULL;
    amdlibDOUBLE  *Lambda=NULL;

    double wlenDifAvg, wlenAvg, R;

    /* Contrary to previous use, wlenAvg is wlenMax, and wlenDifAvg
     * is wlenDifmin. This to cover every case related to resolution
     * change within the band.*/

    amdlibLogTrace("amdlibComputePiston()");

    wlenAvg=wave->wlen[0];
    wlenDifAvg=fabs(wave->bandwidth[0]);

    for (lVis = 1; lVis < nbLVis; lVis++)
    {
        wlenAvg = amdlibMax(wlenAvg, wave->wlen[lVis]);
        wlenDifAvg = amdlibMin(wlenDifAvg, fabs(wave->bandwidth[lVis]));
    }

    R = wlenAvg / wlenDifAvg; /* R computed at max wavelength, but
                               * overstimated at the smallest wavelength diff
                               * useful for correct exploration of piston
                               * possibilities */
    R = R/2 ; /* True resolution if 2 pixels by spectral element - best case */

    /* Wrap instantCorrFlux->table structure to easy the reading of the code*/
    tablePtr = 
        (amdlibVIS_TABLE_ENTRY **)amdlibWrap2DArray(instantCorrFlux->table,
                                                nbBases, nbFrames,
                                                sizeof(amdlibVIS_TABLE_ENTRY),
                                                errMsg);
    if (tablePtr == NULL)
    {
        amdlibComputePiston_FREEALL();
        return amdlibFAILURE;
    }

    C = calloc(nbLVis, sizeof(amdlibCOMPLEX));   
    S2C = calloc(nbLVis, sizeof(amdlibCOMPLEX));   
    Lambda = calloc(nbLVis, sizeof(amdlibDOUBLE));   
    if(nbBases == 3)
    {
        nbTel = 3;
    }
    for (iFrame=0; iFrame < nbFrames; iFrame++)
    {
        iBase=0;
        for (lVis=0, okVis=0; lVis<nbLVis; lVis++)
        {
            if (tablePtr[iFrame][iBase].flag[lVis]==amdlibFALSE)
            {
                C[okVis].re=tablePtr[iFrame][iBase].vis[lVis].re;
                C[okVis].im=tablePtr[iFrame][iBase].vis[lVis].im;
                S2C[okVis].re=tablePtr[iFrame][iBase].sigma2Vis[lVis].re;
                S2C[okVis].im=tablePtr[iFrame][iBase].sigma2Vis[lVis].im;
                Lambda[okVis]=wave->wlen[lVis];
                okVis++;
            }
        }
        if (okVis>2)
        {
            amdlibComputePiston2T(okVis, Lambda, C, S2C,
                                  &instantOpd->pistonOPDArray[band][nbBases*iFrame+iBase],
                                  &instantOpd->sigmaPistonArray[band][nbBases*iFrame+iBase], wlenAvg, wlenDifAvg, R);
        }
        else
        {
            instantOpd->pistonOPDArray[band][nbBases*iFrame+iBase]=amdlibBLANKING_VALUE;
            instantOpd->sigmaPistonArray[band][nbBases*iFrame+iBase]=amdlibBLANKING_VALUE;
        }
        if (nbTel==3)
        {
            iBase=1;
            for (lVis=0, okVis=0; lVis<nbLVis; lVis++)
            {
                if (tablePtr[iFrame][iBase].flag[lVis]==amdlibFALSE)
                {
                    C[okVis].re=tablePtr[iFrame][iBase].vis[lVis].re;
                    C[okVis].im=tablePtr[iFrame][iBase].vis[lVis].im;
                    S2C[okVis].re=tablePtr[iFrame][iBase].sigma2Vis[lVis].re;
                    S2C[okVis].im=tablePtr[iFrame][iBase].sigma2Vis[lVis].im;
                    Lambda[okVis]=wave->wlen[lVis];
                    okVis++;
                }
            }
            if (okVis>2)
            {
                amdlibComputePiston2T(okVis, Lambda, C, S2C,
                                      &instantOpd->pistonOPDArray[band][nbBases*iFrame+iBase],
                                      &instantOpd->sigmaPistonArray[band][nbBases*iFrame+iBase], wlenAvg, wlenDifAvg, R);
            }
            else
            {
                instantOpd->pistonOPDArray[band][nbBases*iFrame+iBase]=amdlibBLANKING_VALUE;
                instantOpd->sigmaPistonArray[band][nbBases*iFrame+iBase]=amdlibBLANKING_VALUE;
            }
            iBase=2;
            for (lVis=0, okVis=0; lVis<nbLVis; lVis++)
            {
                if (tablePtr[iFrame][iBase].flag[lVis]==amdlibFALSE)
                {
                    C[okVis].re=tablePtr[iFrame][iBase].vis[lVis].re;
                    C[okVis].im=tablePtr[iFrame][iBase].vis[lVis].im;
                    S2C[okVis].re=tablePtr[iFrame][iBase].sigma2Vis[lVis].re;
                    S2C[okVis].im=tablePtr[iFrame][iBase].sigma2Vis[lVis].im;
                    Lambda[okVis]=wave->wlen[lVis];
                    okVis++;
                }
            }
            if (okVis>2)
            {
                amdlibComputePiston2T(okVis, Lambda, C, S2C,
                                      &instantOpd->pistonOPDArray[band][nbBases*iFrame+iBase],
                                      &instantOpd->sigmaPistonArray[band][nbBases*iFrame+iBase], wlenAvg, wlenDifAvg, R);
            }
            else
            {
                instantOpd->pistonOPDArray[band][nbBases*iFrame+iBase]=amdlibBLANKING_VALUE;
                instantOpd->sigmaPistonArray[band][nbBases*iFrame+iBase]=amdlibBLANKING_VALUE;
            }
        }
    }  /* end loop iFrame */

    instantOpd->bandFlag[band] = amdlibTRUE;
    memcpy(instantOpd->pistonOPD, instantOpd->pistonOPDArray[band],
           instantOpd->nbFrames * instantOpd->nbBases * sizeof(amdlibDOUBLE));
    memcpy(instantOpd->sigmaPiston, instantOpd->sigmaPistonArray[band],
           instantOpd->nbFrames * instantOpd->nbBases * sizeof(amdlibDOUBLE));
    amdlibComputePiston_FREEALL();
    return amdlibSUCCESS;
}
#undef amdlibComputePiston_FREEALL

#define amdlibComputePiston2T_FREEALL() free(waveNr); free(testPist); \
    free(cVisC); free(cVisCN); free(cVisC0); free(cVisCN0); free(cVisC1);\
    free(cVisCN1); free(cVisC2); free(cVisCN2); free(chi2);

/**
 * Compute piston, iterative phasor method. 
 */
void amdlibComputePiston2T(int                    nbLVis,
                           amdlibDOUBLE           *wlen,
                           amdlibCOMPLEX          *cpxVisTable,
                           amdlibCOMPLEX          *s2cpxVisTable,
                           amdlibDOUBLE           *pistonOPD,
                           amdlibDOUBLE           *sigma,
                           double                 wlenAvg,
                           double                 wlenDifAvg,
                           double                 R
                           )
{
    int lVis;
    int doOnce=1;
    int i, iMin=0, N, nbTest, iTest;
    double *waveNr, *testPist, *chi2;
    amdlibCOMPLEX *cVisC, *cVisCN, *cVisC0, *cVisCN0,*cVisC1, *cVisCN1, 
                  *cVisC2, *cVisCN2;
    amdlibCOMPLEX phasor, avgVis, rmsVis, conjAvgVis;

    double x,range,finesse;
    double opd0, opd1, opd2, Dopd, chi20, chi21, chi22;
    double r;
    double coefGrow = 1.5, coefShrink = 2.;

    amdlibLogTrace("amdlibComputePiston2T()");

    waveNr = calloc(nbLVis, sizeof(*waveNr)); 
    cVisC = calloc(nbLVis, sizeof(*cVisC)); 
    cVisCN = calloc(nbLVis, sizeof(*cVisCN)); 
    cVisC0 = calloc(nbLVis, sizeof(*cVisC0)); 
    cVisCN0 = calloc(nbLVis, sizeof(*cVisCN0)); 
    cVisC1 = calloc(nbLVis, sizeof(*cVisC1)); 
    cVisCN1 = calloc(nbLVis, sizeof(*cVisCN1)); 
    cVisC2 = calloc(nbLVis, sizeof(*cVisC2)); 
    cVisCN2 = calloc(nbLVis, sizeof(*cVisCN2)); 

    for (lVis = 0; lVis < nbLVis; lVis++)
    {
        waveNr[lVis] = 2 * M_PI / wlen[lVis];
    }

    /* Explore a large range, but limit the number of points to something
     * sensible: */
    if (R<70) /*LR = 35*/
    {
        nbTest = 4*(int)(R)+1;
    }
    else /* Avoid aliasing by looking to the central part */
    {
        R /= 2; /* 1 mm at MR */
        nbTest = amdlibMin(amdlibMAX_SWEEP_SIZE_FOR_PISTON,4*(int)(R)+1);
    }

    chi2 = calloc(nbTest, sizeof(*chi2)); 
    testPist = calloc(nbTest, sizeof(*testPist)); 

    opd0=0.0;

    range = 1.2 * R * wlenAvg;
    finesse = range / (nbTest-1);

    while ((finesse>wlenAvg)||(doOnce==1))
    {
        doOnce=0;
        for (iTest = 0; iTest < nbTest; iTest++)
        {

            testPist[iTest] = (-range/2) + opd0 + iTest * range / (nbTest-1);
        }

        for (iTest = 0; iTest < nbTest; iTest++)
        {
            for (i = 0; i < nbLVis; i++)
            {
                x = waveNr[i] * testPist[iTest];
                phasor.re = cos(x);
                phasor.im = -sin(x);
                amdlibCpxMul(cpxVisTable[i], phasor, cVisC[i]);
            }
            conjAvgVis.re = 0.0;
            conjAvgVis.im = 0.0;
            for (i = 0; i < nbLVis; i++)
            {
                conjAvgVis.re += cVisC[i].re;
                conjAvgVis.im -= cVisC[i].im;
            }
            conjAvgVis.re /= nbLVis ;
            conjAvgVis.im /= nbLVis ;
            for (i = 0; i < nbLVis; i++)
            {
                amdlibCpxMul(cVisC[i], conjAvgVis, cVisCN[i]);
            }
            /* rms cVisCN */
            avgVis.re = 0.0;
            avgVis.im = 0.0;
            for (i = 0; i < nbLVis; i++)
            {
                avgVis.re += cVisCN[i].re;
                avgVis.im += cVisCN[i].im;
            }
            avgVis.re /= nbLVis ;
            avgVis.im /= nbLVis ;
            rmsVis.re = 0.0;
            rmsVis.im = 0.0;
            for (i = 0; i < nbLVis; i++)
            {
                rmsVis.re += amdlibPow2(cVisCN[i].re-avgVis.re);
                rmsVis.im += amdlibPow2(cVisCN[i].im-avgVis.im);
            }
            rmsVis.re /= nbLVis ;
            rmsVis.im /= nbLVis ;
            rmsVis.re = sqrt(rmsVis.re);
            rmsVis.im = sqrt(rmsVis.im);
            chi2[iTest] = (rmsVis.re * rmsVis.im) / (avgVis.re*avgVis.re+avgVis.im*avgVis.im);
        }

        iMin = amdlibFindIndexOfMinimum(chi2, nbTest);
        opd0 = testPist[iMin];

        range /= 2 ;
        finesse = range / (nbTest-1) ;
    }
    /*use opd0 as random generator seed -- piston is notoriously random */
    srand((unsigned int)(opd0*1E12));
    r = (double)rand() / (double)RAND_MAX;
    Dopd=wlenDifAvg;
    for (N = 0; N < amdlibMAX_PISTON2_ITERATIONS; N++)
    {
        for (lVis = 0; lVis < nbLVis; lVis++)
        {
            x = waveNr[lVis] * opd0;
            phasor.re = cos(x);
            phasor.im = -sin(x);
            amdlibCpxMul(cpxVisTable[lVis], phasor, cVisC0[lVis]);
        }
        conjAvgVis.re = 0.0;
        conjAvgVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            conjAvgVis.re += cVisC0[i].re;
            conjAvgVis.im -= cVisC0[i].im;
        }
        conjAvgVis.re /= nbLVis ;
        conjAvgVis.im /= nbLVis ;
        for (lVis = 0; lVis < nbLVis; lVis++)
        {
            amdlibCpxMul(cVisC0[lVis], conjAvgVis, cVisCN0[lVis]);
        }
        avgVis.re = 0.0;
        avgVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            avgVis.re += cVisCN0[i].re;
            avgVis.im += cVisCN0[i].im;
        }
        avgVis.re /= nbLVis ;
        avgVis.im /= nbLVis ;
        rmsVis.re = 0.0;
        rmsVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            rmsVis.re += amdlibPow2(cVisCN0[i].re - avgVis.re);
            rmsVis.im += amdlibPow2(cVisCN0[i].im - avgVis.im);
        }
        rmsVis.re /= nbLVis ;
        rmsVis.im /= nbLVis ;
        rmsVis.re = sqrt(rmsVis.re);
        rmsVis.im = sqrt(rmsVis.im);
        chi20 = rmsVis.re * rmsVis.im / (avgVis.re*avgVis.re+avgVis.im*avgVis.im);

        opd1 = opd0 + Dopd;
        for (lVis = 0; lVis < nbLVis; lVis++)
        {
            x = waveNr[lVis] * opd1;
            phasor.re = cos(x);
            phasor.im = -sin(x);
            amdlibCpxMul(cpxVisTable[lVis], phasor, cVisC1[lVis]);
        }
        conjAvgVis.re = 0.0;
        conjAvgVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            conjAvgVis.re += cVisC1[i].re;
            conjAvgVis.im -= cVisC1[i].im;
        }
        conjAvgVis.re /= nbLVis ;
        conjAvgVis.im /= nbLVis ;
        for (lVis = 0; lVis < nbLVis; lVis++)
        {
            amdlibCpxMul(cVisC1[lVis], conjAvgVis, cVisCN1[lVis]);
        }
        avgVis.re = 0.0;
        avgVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            avgVis.re += cVisCN1[i].re;
            avgVis.im += cVisCN1[i].im;
        }
        avgVis.re /= nbLVis ;
        avgVis.im /= nbLVis ;
        rmsVis.re = 0.0;
        rmsVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            rmsVis.re += amdlibPow2(cVisCN1[i].re - avgVis.re);
            rmsVis.im += amdlibPow2(cVisCN1[i].im - avgVis.im);
        }
        rmsVis.re /= nbLVis ;
        rmsVis.im /= nbLVis ;
        rmsVis.re = sqrt(rmsVis.re);
        rmsVis.im = sqrt(rmsVis.im);
        chi21 = rmsVis.re * rmsVis.im / (avgVis.re*avgVis.re+avgVis.im*avgVis.im);
        
        opd2 = opd0 - Dopd;
        for (lVis = 0; lVis < nbLVis; lVis++)
        {
            x = waveNr[lVis] * opd2;
            phasor.re = cos(x);
            phasor.im = -sin(x);
            amdlibCpxMul(cpxVisTable[lVis], phasor, cVisC2[lVis]);
        }
        conjAvgVis.re = 0.0;
        conjAvgVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            conjAvgVis.re += cVisC2[i].re;
            conjAvgVis.im -= cVisC2[i].im;
        }
        conjAvgVis.re /= nbLVis ;
        conjAvgVis.im /= nbLVis ;
        for (lVis = 0; lVis < nbLVis; lVis++)
        {
            amdlibCpxMul(cVisC2[lVis], conjAvgVis, cVisCN2[lVis]);
        }
        avgVis.re = 0.0;
        avgVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            avgVis.re += cVisCN2[i].re;
            avgVis.im += cVisCN2[i].im;
        }
        avgVis.re /= nbLVis ;
        avgVis.im /= nbLVis ;
        rmsVis.re = 0.0;
        rmsVis.im = 0.0;
        for (i = 0; i < nbLVis; i++)
        {
            rmsVis.re += amdlibPow2(cVisCN2[i].re - avgVis.re);
            rmsVis.im += amdlibPow2(cVisCN2[i].im - avgVis.im);
        }
        rmsVis.re /= nbLVis ;
        rmsVis.im /= nbLVis ;
        rmsVis.re = sqrt(rmsVis.re);
        rmsVis.im = sqrt(rmsVis.im);
        chi22 = rmsVis.re * rmsVis.im / (avgVis.re*avgVis.re+avgVis.im*avgVis.im);

        if ((chi21 >= chi20) && (chi20 > chi22))
        {
            opd0 = opd0 - Dopd / coefShrink;
            Dopd = Dopd * coefGrow;
        }
        else if ((chi21 <= chi20) && (chi20 < chi22))
        {
            opd0 = opd0 + Dopd / coefShrink;
            Dopd = Dopd * coefGrow;
        }
        else if ((chi21 > chi20) && (chi20 < chi22))
        {
            Dopd = Dopd / coefShrink;
        }
        else
        {
            r = (double)rand() / (double)RAND_MAX;
            if (Dopd == 0)
            {
                Dopd = (r - 0.5) * wlenDifAvg;
            }
            if (r > 0.5)
            {
                opd0 -= Dopd / coefShrink;
            }
            else
            {
                opd0 += Dopd / coefShrink;
            }
        }
        if  (amdlibPow2(Dopd /wlenDifAvg ) < amdlibPow2(amdlibPISTONPRECISION))
        {
            break;
        }

    }

    if (opd0>R*wlenAvg)
    {
        *pistonOPD =  amdlibBLANKING_VALUE;
        *sigma =  amdlibBLANKING_VALUE;
        amdlibComputePiston2T_FREEALL();
        return;
    }
    else
    {
        *pistonOPD =  opd0;
    }
    /* Heuristic: chi20 above is not a true chi2 since there is no use of
     * the observed variance.( Using the variance gives silly results in chi2).
     * transform the chi2 as an idicator of the error on the phase (in
     * radians), call AbacusErrPhi to amplify the really wrong fits. */
    *sigma=amdlibAbacusErrPhi(2*M_PI*chi20/R)*R*wlenAvg/2/M_PI;
    amdlibComputePiston2T_FREEALL();

    return;
}

/**
 * Display piston structure.
 *
 * @param opd pointer to amdlibPISTON structure.
 */
void amdlibDisplayPiston(amdlibPISTON *opd)
{
    int nbFrames, nbBases;
    int iBand, iFrame, iBase, iCell;
    amdlibBOOLEAN  bandFlag;
    amdlibDOUBLE bandPistonOPD;
    amdlibDOUBLE bandSigmaPiston;

    amdlibLogTrace("amdlibDisplayPiston()");

    /* Display number of frames */
    nbFrames = opd->nbFrames;
    printf("nbFrames = %d\n", nbFrames);
    
    /* Display number of base */
    nbBases = opd->nbBases;
    printf("nbBases = %d\n", nbBases);

    /* Display arrays[amdlibNB_BAND] */
    for (iBand = 0; iBand < amdlibNB_BANDS; iBand++)
    {
        /* bandFlag */
        bandFlag = opd->bandFlag[iBand];
        printf("bandFlag[%d] = %d\n", iBand, bandFlag);
    }
    /* Display different arrays[nbBand][nbFrames][nbBases] */
    for (iBand = 0; iBand < amdlibNB_BANDS; iBand++)
    {
        for (iFrame = 0; iFrame < nbFrames; iFrame++)
        {
            for (iBase = 0; iBase < nbBases; iBase++)
            {            
                /* Set cell[iFrame][iBase] */
                iCell = iFrame * nbBases + iBase;
                printf("---> band[%d] - cell frame/base[%d][%d]\n",iBand, 
                       iFrame, iBase);
                
                /* pistonOPDArray */
                bandPistonOPD = opd->pistonOPDArray[iBand][iCell];
                printf("pistonOPDArray[%d][%d][%d] = %f - ", iBand, iFrame, 
                       iBase, bandPistonOPD);

                /* sigmaPistonArray */
                bandSigmaPiston = opd->sigmaPistonArray[iBand][iCell];
                printf("sigmaPistonArray[%d][%d][%d] = %f\n", iBand, iFrame,
                       iBase, bandSigmaPiston);
            }
        }
    }
}


/**
 * Free memory allocated for pistons. 
 *
 * @param piston pointer to amdlibPISTON structure.
 */
void amdlibFreePiston(amdlibPISTON *piston)
{
    int band;
    
    amdlibLogTrace("amdlibFreePiston()");
    
    /* Check thisPtr and do nothing if unitialized*/
    if (piston->thisPtr != piston)
    {
        return;
    }

    for (band=amdlibJ_BAND; band <= amdlibK_BAND; band++)
    {
        if (piston->pistonOPDArray[band] != NULL)
        {
            free(piston->pistonOPDArray[band]);
        }
        piston->pistonOPDArray[band] = NULL;
        if (piston->sigmaPistonArray[band] != NULL)
        {
            free(piston->sigmaPistonArray[band]);
        }
        piston->sigmaPistonArray[band] = NULL;
    }
    
    if (piston->pistonOPD != NULL)
    {
        free(piston->pistonOPD);
    }
    piston->pistonOPD = NULL;
    if (piston->sigmaPiston != NULL)
    {
        free(piston->sigmaPiston);
    }
    piston->sigmaPiston = NULL;

    piston->thisPtr = NULL;
}

/*___oOo___*/
