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

#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"


/** Useful macro to free all dynamically allocated structures */ 
#define amdlibComputeClosurePhases_FREEALL()       \
    amdlibFree2DArrayWrapping((void **)tablePtr);  \
    amdlibFree2DArrayWrapping((void **)table3Ptr); 
/**
 * Compute closure phases. 
 *
 * This function computes the closure phases, averaged over a bin,
 * from a bin of instantaneous correlated fluxes. 
 * 
 * @param instantCorrFlux instantaneous bin of correlated flux.
 * @param selectedFrames structure containing information relative to frame
 * selection. By default, all frames are selected.
 * @param vis3 data structure (binned) where results are stored.
 * @param iBin index of the result in the output vis3 structure.
 * @param errMsg error description message returned if function fails.
 *
 * @return 
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */

amdlibCOMPL_STAT amdlibComputeClosurePhases(/* Input */
                                            amdlibVIS       *instantCorrFlux,
                                            int             iBin,
                                            amdlibBAND      band,
                                            amdlibSELECTION *selectedFrames,
                                            amdlibERROR_TYPE   errorType,
                                            /* Output */
                                            amdlibVIS3      *vis3,
                                            amdlibERROR_MSG errMsg)
{
    double Re_bispectrum = 0.0;
    double Im_bispectrum = 0.0;
    double Sigma2_bispectrum_re = 0.0;
    double Sigma2_bispectrum_im = 0.0;
    double Re2_bispectrum = 0.0;
    double Im2_bispectrum = 0.0;
    double Re4Im4_bispectrum = 0.0;
    double R, I, s2R, s2I, R12, R23, R13, I12, I23, I13;
    double s2R12, s2R23, s2R13, s2I12, s2I23, s2I13;
    double val;
    
    int iClos;
    int lVis;
    int nbClos   = vis3->nbClosures;
    int nbWlen   = vis3->nbWlen;
    int nbBases  = instantCorrFlux->nbBases;
    int nbOkFrames = selectedFrames->band[band].nbFramesOkForClosure;

    int iFrame, nbGoodFrames, i;
    amdlibVIS_TABLE_ENTRY **tablePtr = NULL;
    amdlibVIS3_TABLE_ENTRY **table3Ptr = NULL;

    amdlibLogTrace("amdlibComputeClosurePhases()");

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

    for (iClos = 0; iClos < nbClos; iClos++) 
    {
        for (lVis = 0; lVis < nbWlen; lVis++)
        { /* begin loop for lVis */

            Re_bispectrum = 0.0;
            Im_bispectrum = 0.0;
            Re2_bispectrum = 0.0;
            Im2_bispectrum = 0.0;
            Re4Im4_bispectrum = 0.0;
            Sigma2_bispectrum_re = 0.0;
            Sigma2_bispectrum_im = 0.0;

            nbGoodFrames=0;
            for (i = 0; i < nbOkFrames; i++)
            {
                /* Whatever whether one, two or three good frames were selected, 
                 * we need to test the goodness of each individual visibility using its flag,
                 * all the 3 visibilities needing to be unflagged. */
                iFrame = selectedFrames->band[band].frameOkForClosure[i];

                if (tablePtr[iFrame][0].flag[lVis] == amdlibFALSE &&
                    tablePtr[iFrame][1].flag[lVis] == amdlibFALSE &&
                    tablePtr[iFrame][2].flag[lVis] == amdlibFALSE)
                {
                    nbGoodFrames++;
                    R12 = tablePtr[iFrame][0].vis[lVis].re;
                    R23 = tablePtr[iFrame][1].vis[lVis].re;
                    R13 = tablePtr[iFrame][2].vis[lVis].re;
                    I12 = tablePtr[iFrame][0].vis[lVis].im;
                    I23 = tablePtr[iFrame][1].vis[lVis].im;
                    I13 = tablePtr[iFrame][2].vis[lVis].im;
                    s2R12 = tablePtr[iFrame][0].sigma2Vis[lVis].re;
                    s2R23 = tablePtr[iFrame][1].sigma2Vis[lVis].re;
                    s2R13 = tablePtr[iFrame][2].sigma2Vis[lVis].re;
                    s2I12 = tablePtr[iFrame][0].sigma2Vis[lVis].im;
                    s2I23 = tablePtr[iFrame][1].sigma2Vis[lVis].im;
                    s2I13 = tablePtr[iFrame][2].sigma2Vis[lVis].im;

                    val = R12*R23*R13 - I12*I23*R13 + I12*R23*I13 + R12*I23*I13;

                    Re_bispectrum += val;
                    Re2_bispectrum += amdlibPow2(val);
                    Re4Im4_bispectrum += amdlibPow2(val*val);
                    val = I12*I23*I13 - R12*R23*I13 + R12*I23*R13 + I12*R23*R13;
                    Im_bispectrum += val;
                    Im2_bispectrum += amdlibPow2(val);
                    Re4Im4_bispectrum += amdlibPow2(val*val);

                    if ((errorType == amdlibTHEORETICAL_ERROR)||
                        (errorType == amdlibSTATISTICAL_ERROR))
                        /* FIXME -- PLEASE MODIFY BELOW TO ADD PROPER 
                         * STATISTICAL ERROR TRETAMENT (action FM) 
                         */
                    {
                        /* Please have a look to the F. Millour thesis
                           (http://tel.archives-ouvertes.fr/tel-00134268),
                           pp.84-85 (eq. 4.34 and 4.35) */
                        Sigma2_bispectrum_re +=
                        s2R12 * (amdlibPow2(R23*R13) + amdlibPow2(I23*I13)) +
                        s2R23 * (amdlibPow2(R12*R13) + amdlibPow2(I12*I13)) + 
                        s2R13 * (amdlibPow2(R12*R23) + amdlibPow2(I12*I23)) +
                        s2I12 * (amdlibPow2(I23*R13) + amdlibPow2(R23*I13)) + 
                        s2I23 * (amdlibPow2(I12*R13) + amdlibPow2(R12*I13)) +
                        s2I13 * (amdlibPow2(I23*R12) + amdlibPow2(R23*I12));
                        
                        Sigma2_bispectrum_im +=
                        s2I12 * (amdlibPow2(R23*R13) + amdlibPow2(I23*I13)) + 
                        s2I23 * (amdlibPow2(R12*R13) + amdlibPow2(I12*I13)) +
                        s2I13 * (amdlibPow2(I12*I23) + amdlibPow2(R12*R23)) +
                        s2R12 * (amdlibPow2(R23*I13) + amdlibPow2(I23*R13)) +
                        s2R23 * (amdlibPow2(R12*I13) + amdlibPow2(I12*R13)) + 
                        s2R13 * (amdlibPow2(I12*R23) + amdlibPow2(R12*I23));
                    }
                    else if(errorType == amdlibSTATISTICAL_ERROR)
                    {
                        Sigma2_bispectrum_re += 0;
                        Sigma2_bispectrum_im += 0;
                    }
                }
            }

            if (nbGoodFrames != 0)
            {
                Re_bispectrum /= nbGoodFrames;
                Im_bispectrum /= nbGoodFrames;
                Re2_bispectrum /= nbGoodFrames;
                Im2_bispectrum /= nbGoodFrames;
                Re4Im4_bispectrum /= nbGoodFrames;
                Sigma2_bispectrum_re /= nbGoodFrames;
                Sigma2_bispectrum_im /= nbGoodFrames;
                
                R = Re_bispectrum;
                I = Im_bispectrum;
                s2R = Sigma2_bispectrum_re;
                s2I = Sigma2_bispectrum_im;
                
                table3Ptr[iBin][iClos].vis3Amplitude[lVis] =
                sqrt(R*R + I*I);
                table3Ptr[iBin][iClos].vis3Phi[lVis] =
                atan2(I,R);
                
                table3Ptr[iBin][iClos].vis3AmplitudeError[lVis] = 
                1 / (R*R + I*I) *R*R*s2R + 
                1 / (R*R+I*I) *I*I*s2I;
                
                /* Please have a look to the F. Millour thesis
                   (http://tel.archives-ouvertes.fr/tel-00134268),
                   pp.55-57 (eq 3.28) and pp.84-85 (eq 4.36 where a typo error
                   removed the M factor) */
                table3Ptr[iBin][iClos].vis3PhiError[lVis] =
                sqrt( (s2R * Im2_bispectrum + s2I * Re2_bispectrum) /
                      Re4Im4_bispectrum );
                table3Ptr[iBin][iClos].flag[lVis] = amdlibFALSE;
            }
            else
            {
                table3Ptr[iBin][iClos].vis3Amplitude[lVis] = amdlibBLANKING_VALUE;
                table3Ptr[iBin][iClos].vis3Phi[lVis] = amdlibBLANKING_VALUE;

                table3Ptr[iBin][iClos].flag[lVis] = amdlibTRUE;
            }
        }
    }

    amdlibComputeClosurePhases_FREEALL();
    return amdlibSUCCESS;
}
#undef amdlibComputeClosurePhases_FREEALL


/** Useful macro to free all dynamically allocated structures */ 
#define amdlibBinClosurePhases_FREEALL()       \
    amdlibFree2DArrayWrapping((void **)tablePtr);  \
    amdlibFree2DArrayWrapping((void **)table3Ptr); 
/**
 * Bin closure phases. 
 *
 * This function computes the closure phases, averaged over a bin,
 * from a bin of instantaneous correlated fluxes. 
 * 
 * @param instantCorrFlux instantaneous bin of correlated flux.
 * @param selectedFrames structure containing information relative to frame
 * selection. By default, all frames are selected.
 * @param vis3 data structure (binned) where results are stored.
 * @param iBin index of the result in the output vis3 structure.
 * @param errMsg error description message returned if function fails.
 *
 * @return 
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */

amdlibCOMPL_STAT amdlibBinClosurePhases(/* Input */
                                            amdlibVIS       *instantCorrFlux,
                                            int             firstFrame,
                                            int             nbFrames,
                                            int             iBin,
                                            amdlibBAND      band,
                                            amdlibERROR_TYPE   errorType,
                                            /* Output */
                                            amdlibVIS3      *vis3,
                                            amdlibERROR_MSG errMsg)
{
    double Re_bispectrum = 0.0;
    double Im_bispectrum = 0.0;
    double Sigma2_bispectrum_re = 0.0;
    double Sigma2_bispectrum_im = 0.0;
    double Re2_bispectrum = 0.0;
    double Im2_bispectrum = 0.0;
    double Re4Im4_bispectrum = 0.0;
    double R, I, s2R, s2I, R12, R23, R13, I12, I23, I13;
    double s2R12, s2R23, s2R13, s2I12, s2I23, s2I13;
    double val;
    
    int iClos;
    int lVis;
    int nbClos   = vis3->nbClosures;
    int nbWlen   = vis3->nbWlen;
    int nbBases  = instantCorrFlux->nbBases;

    int iFrame, runningFrame,nbGoodFrames;
    amdlibVIS_TABLE_ENTRY **tablePtr = NULL;
    amdlibVIS3_TABLE_ENTRY **table3Ptr = NULL;

    amdlibLogTrace("amdlibBinClosurePhases()");

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

    for (iClos = 0; iClos < nbClos; iClos++) 
    {
        for (lVis = 0; lVis < nbWlen; lVis++)
        { /* begin loop for lVis */
            
            Re_bispectrum = 0.0;
            Im_bispectrum = 0.0;
            Re2_bispectrum = 0.0;
            Im2_bispectrum = 0.0;
            Re4Im4_bispectrum = 0.0;
            Sigma2_bispectrum_re = 0.0;
            Sigma2_bispectrum_im = 0.0;
            
            nbGoodFrames=0;
            for (iFrame = 0, runningFrame=firstFrame; iFrame < nbFrames;
                 iFrame++, runningFrame++)
            {
                if (tablePtr[runningFrame][0].flag[lVis] == amdlibFALSE &&
                    tablePtr[runningFrame][1].flag[lVis] == amdlibFALSE &&
                    tablePtr[runningFrame][2].flag[lVis] == amdlibFALSE)
                {
                    nbGoodFrames++;
                    R12 = tablePtr[runningFrame][0].vis[lVis].re;
                    R23 = tablePtr[runningFrame][1].vis[lVis].re;
                    R13 = tablePtr[runningFrame][2].vis[lVis].re;
                    I12 = tablePtr[runningFrame][0].vis[lVis].im;
                    I23 = tablePtr[runningFrame][1].vis[lVis].im;
                    I13 = tablePtr[runningFrame][2].vis[lVis].im;
                    s2R12 = tablePtr[runningFrame][0].sigma2Vis[lVis].re;
                    s2R23 = tablePtr[runningFrame][1].sigma2Vis[lVis].re;
                    s2R13 = tablePtr[runningFrame][2].sigma2Vis[lVis].re;
                    s2I12 = tablePtr[runningFrame][0].sigma2Vis[lVis].im;
                    s2I23 = tablePtr[runningFrame][1].sigma2Vis[lVis].im;
                    s2I13 = tablePtr[runningFrame][2].sigma2Vis[lVis].im;
                    
                    val = R12*R23*R13 - I12*I23*R13 + I12*R23*I13 + R12*I23*I13;
                    Re_bispectrum += val;
                    Re2_bispectrum += amdlibPow2(val);
                    Re4Im4_bispectrum += amdlibPow2(val*val);
                    
                    val = I12*I23*I13 - R12*R23*I13 + R12*I23*R13 + I12*R23*R13;
                    Im_bispectrum += val;
                    Im2_bispectrum += amdlibPow2(val);
                    Re4Im4_bispectrum += amdlibPow2(val*val);
                    
                    if ((errorType == amdlibTHEORETICAL_ERROR)||
                        (errorType == amdlibSTATISTICAL_ERROR))
                        /* FIXME -- PLEASE MODIFY BELOW TO ADD PROPER 
                         * STATISTICAL ERROR TRETAMENT (action FM) 
                         */
                    {
                        /* Please have a look to the F. Millour thesis
                           (http://tel.archives-ouvertes.fr/tel-00134268),
                           pp.84-85 (eq. 4.34 and 4.35) */
                        Sigma2_bispectrum_re +=
                        s2R12 * (amdlibPow2(R23*R13) + amdlibPow2(I23*I13)) +
                        s2R23 * (amdlibPow2(R12*R13) + amdlibPow2(I12*I13)) + 
                        s2R13 * (amdlibPow2(R12*R23) + amdlibPow2(I12*I23)) +
                        s2I12 * (amdlibPow2(I23*R13) + amdlibPow2(R23*I13)) + 
                        s2I23 * (amdlibPow2(I12*R13) + amdlibPow2(R12*I13)) +
                        s2I13 * (amdlibPow2(I23*R12) + amdlibPow2(R23*I12));
                        
                        Sigma2_bispectrum_im +=
                        s2I12 * (amdlibPow2(R23*R13) + amdlibPow2(I23*I13)) + 
                        s2I23 * (amdlibPow2(R12*R13) + amdlibPow2(I12*I13)) +
                        s2I13 * (amdlibPow2(I12*I23) + amdlibPow2(R12*R23)) +
                        s2R12 * (amdlibPow2(R23*I13) + amdlibPow2(I23*R13)) +
                        s2R23 * (amdlibPow2(R12*I13) + amdlibPow2(I12*R13)) + 
                        s2R13 * (amdlibPow2(I12*R23) + amdlibPow2(R12*I23));
                    }
                    else if(errorType == amdlibSTATISTICAL_ERROR)
                    {
                        Sigma2_bispectrum_re += 0;
                        Sigma2_bispectrum_im += 0;
                    }
                }
            }
            if (nbGoodFrames>0)
            {
                Re_bispectrum /= nbGoodFrames;
                Im_bispectrum /= nbGoodFrames;
                Re2_bispectrum /= nbGoodFrames;
                Im2_bispectrum /= nbGoodFrames;
                Re4Im4_bispectrum /= nbGoodFrames;
                Sigma2_bispectrum_re /= nbGoodFrames;
                Sigma2_bispectrum_im /= nbGoodFrames;
                
                R = Re_bispectrum;
                I = Im_bispectrum;
                s2R = Sigma2_bispectrum_re;
                s2I = Sigma2_bispectrum_im;
                
                table3Ptr[iBin][iClos].vis3Amplitude[lVis] =
                sqrt(R*R + I*I);
                table3Ptr[iBin][iClos].vis3Phi[lVis] =
                atan2(I,R);
                
                table3Ptr[iBin][iClos].vis3AmplitudeError[lVis] = 
                1 / (R*R + I*I) *R*R*s2R + 
                1 / (R*R+I*I) *I*I*s2I;
                
                /* Please have a look to the F. Millour thesis
                   (http://tel.archives-ouvertes.fr/tel-00134268),
                   pp.55-57 (eq 3.28) and pp.84-85 (eq 4.36 where a typo error
                   removed the M factor) */
                table3Ptr[iBin][iClos].vis3PhiError[lVis] =
                sqrt( (s2R * Im2_bispectrum + s2I * Re2_bispectrum) /
                      Re4Im4_bispectrum );
                table3Ptr[iBin][iClos].flag[lVis] = amdlibFALSE;
            }
            else
            {
                table3Ptr[iBin][iClos].vis3Amplitude[lVis] = amdlibBLANKING_VALUE;
                table3Ptr[iBin][iClos].vis3Phi[lVis] = amdlibBLANKING_VALUE;

                table3Ptr[iBin][iClos].flag[lVis] = amdlibTRUE;
            }
        }
    }

    amdlibBinClosurePhases_FREEALL();
    return amdlibSUCCESS;
}
#undef amdlibBinClosurePhases_FREEALL


/**
 * Computes a crude weighted average of all closures phase in a closure
 * phase structure. 
 *
 * @param vis3 input/output: closure structure, where averaged values are
 * writen in member "averageClosure".
 */
void amdlibAverageClosurePhases(amdlibVIS3 *vis3)
{
    double spectralClosurePhase[amdlibNB_SPECTRAL_CHANNELS];
    double spectralClosurePhaseErr[amdlibNB_SPECTRAL_CHANNELS];
    double averageClosurePhase;
    double averageClosurePhaseErr;
    double c,e;

    int iClos;
    int lVis;
    int iFrame;
    const int nbClos = 1; /* Number of Closures FIXED */
    int nbWlen =  vis3->nbWlen;
    int nbFrames = vis3->nbFrames;
    int nbGoodClos = 0;
    int nc = 0;

    amdlibLogTrace("amdlibAverageClosurePhases()");
    
    amdlibVIS3_TABLE_ENTRY **table3Ptr = NULL;
    static amdlibERROR_MSG errMsg;

    table3Ptr = 
        (amdlibVIS3_TABLE_ENTRY **)amdlibWrap2DArray(vis3->table,
                                                nbClos, nbFrames,
                                                sizeof(amdlibVIS3_TABLE_ENTRY),
                                                errMsg);
    if (table3Ptr == NULL)
    {
        amdlibFree2DArrayWrapping((void **)table3Ptr);
    }
    
    averageClosurePhase = 0;
    averageClosurePhaseErr = 0;
    for (iClos = 0; iClos < nbClos; iClos++)
    { 
        for (lVis = 0; lVis < nbWlen; lVis++)
        {
            nc = 0;
            spectralClosurePhase[lVis] = 0;
            spectralClosurePhaseErr[lVis] = 0;
            for (iFrame = 0; iFrame < nbFrames; iFrame++)
            { 
                if (table3Ptr[iFrame][iClos].flag[lVis]==amdlibFALSE) 
                {
                    c = table3Ptr[iFrame][iClos].vis3Phi[lVis];
                    e = table3Ptr[iFrame][iClos].vis3PhiError[lVis];
                    spectralClosurePhase[lVis] +=
                    table3Ptr[iFrame][iClos].vis3Phi[lVis];
                    spectralClosurePhaseErr[lVis] +=
                    table3Ptr[iFrame][iClos].vis3PhiError[lVis];
                    nc++;
                }
            }
            if (nc>0)
            {
                spectralClosurePhase[lVis] /= nc;
                spectralClosurePhaseErr[lVis] /= nc;
                averageClosurePhase += spectralClosurePhase[lVis];
                averageClosurePhaseErr += spectralClosurePhaseErr[lVis];
                nbGoodClos++;
            }
            else
            {
                spectralClosurePhase[lVis] = amdlibBLANKING_VALUE;
                spectralClosurePhaseErr[lVis] = amdlibBLANKING_VALUE;
            }
        }
        if (nbGoodClos>0) 
        {
            vis3->averageClosure = averageClosurePhase / nbGoodClos;
            vis3->averageClosureError = averageClosurePhaseErr / 
            nbGoodClos;
        }
        else
        {
            vis3->averageClosure = amdlibBLANKING_VALUE;
            vis3->averageClosureError = amdlibBLANKING_VALUE;
        }
    }
    amdlibFree2DArrayWrapping((void **)table3Ptr);
}


/*___oOo___*/
