/*******************************************************************************
 * JMMC project ( http://www.jmmc.fr ) - Copyright (C) CNRS.
 ******************************************************************************/
/**
 * @file
 * Functions related to doubleing array shift. 
 */
#define _POSIX_SOURCE 1

/*
 * Macros definitions
 */

/*
 * System Headers
 */
#include <string.h>
#include <math.h>

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

#include <complex.h>
#include <fftw3.h>

/*
 * Protected function
 */
/** Useful macro to free all allocated memory */
#define amdlibShift_FREEALL()  free(drc);             \
                               free(tabInPeriodized); \
                               free(fftDirect);       \
                               free(phasor);          \
                               free(phasedFft);       \
                               free(tabDecalPeriodized);
/**
 * Shift array
 *
 * This function shifts array tabIn by 'shift' (possibly non-integer) number in
 * tabOut, wrapping around values.
 *
 * @param nbElem number of element in arrays
 * @param tabIn input array 
 * @param shift shift to perform 
 * @param tabOut resulting shifted array 
 * @param errMsg error description message returned if function fails.
 * 
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibShift(/* INPUT */
                             int          nbElem,
                             double       *tabIn,
                             double       shift,
                             /* OUTPUT */
                             double       *tabOut,
                             amdlibERROR_MSG errMsg)
{
    int i;
    
    /*  Get the integer part (ip) and fractionnal part (fp)
     *  of the offset to apply to tabIn */
    int   ipShift = (int)shift;
    double fpShift = shift - ipShift;
 
    /*  Get the values of the first and last points to substract
     *  the affine part of the signal */
    double m1 = tabIn[0];
    double m2 = tabIn[nbElem - 1];
    double phi;
    double dr;
    fftw_plan p;

    /* Arrays */
    double *drc = NULL;
    double *tabInPeriodized = NULL;
    double *fftDirect = NULL;
    double *phasor = NULL;
    double *phasedFft = NULL;
    double *tabDecalPeriodized = NULL;

    amdlibLogTrace("amdlibShift()");

#if TEST
    amdlibLogTest("amdlibShift()");
    amdlibLogTest("    Array size = %d", nbElem);
    amdlibLogTest("    Shift      = %.3f", shift);
#endif  

    /* Check shift value; could not be greater then array size */
    if (abs(ipShift) > nbElem)
    {
        amdlibSetErrMsg("Invalid shift %.3f; greater than array size %d",
                        shift, nbElem);	
        return amdlibFAILURE;
    }

    /* Define work arrays */
    
    drc = calloc(nbElem, sizeof(double));
    if (drc == NULL)
    {
        amdlibShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (drc)");
        return amdlibFAILURE;
    }
    
    tabInPeriodized = calloc(nbElem, sizeof(double));
    if (tabInPeriodized == NULL)
    {
        amdlibShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (tabInPeriodized)");
        return amdlibFAILURE;
    }
    
    fftDirect = calloc(nbElem, sizeof(double));
    if (fftDirect == NULL)
    {
        amdlibShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (fftDirect)");
        return amdlibFAILURE;
    } 

    phasor = calloc(nbElem, sizeof(double));
    if (phasor == NULL)
    {
        amdlibShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (phasor)");
        return amdlibFAILURE;
    }  
    
    phasedFft = calloc(nbElem, sizeof(double));
    if (phasedFft == NULL)
    {
        amdlibShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (phasedFft)");
        return amdlibFAILURE;
    }  
    
    tabDecalPeriodized = calloc(nbElem, sizeof(double));
    if (tabDecalPeriodized == NULL)
    {
        amdlibShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (tabDecalPeriodized)");
        return amdlibFAILURE;
    }  

    /* */
    if (fpShift != 0.0)
    {
        for (i=0;i<nbElem;i++)
        {
            /*  The value to substract to the signal before the shift */
            dr = ((m2-m1) / (nbElem - 1)) * i + m1;

            /*  The value to add again to the periodized signal after the 
             *  shift */
            drc[i] = dr + ((m2 - m1) / (nbElem - 1)) * -fpShift;
            tabInPeriodized[i] = tabIn[i] - dr;
        }  

#if TEST
        amdlibLogTest("drc =[");
        for (i = 0; i < nbElem - 1; i++)
        {
            amdlibLogTest("%f, ", drc[i]);
        }
        amdlibLogTest("%f]", drc[nbElem - 1]);

        amdlibLogTest("tabInPeriodized =[");
        for (i = 0; i < nbElem - 1; i++)
        {
            amdlibLogTest("%f, ", tabInPeriodized[i]);
        }
        amdlibLogTest("%f]", tabInPeriodized[nbElem-1]);
#endif

#if TEST
        amdlibLogTest("phi =[");
#endif

        phi = 0; /*(2. *  M_PI * fpShift / nbElem) * 1.0;*/
        phasor[0] =  cos(phi);  /* DC component */
	
        for (i = 1; i < (nbElem + 1) / 2; i++)
        {
            /*  The phasor to apply to the fft of the periodized signal
             *  to shift it of fpShift */
            phi = (2. *  M_PI * fpShift / nbElem) * (i);
#if TEST
            amdlibLogTest("%f, ", phi);
#endif

            phasor[i]         =  cos(phi);
            phasor[nbElem - i] =  sin(phi);
        }
        if (nbElem % 2 == 0) /* nbElem is even */
        {
            phi = M_PI * fpShift;
            phasor[nbElem / 2] =  cos(phi);  /* Nyquist freq. */
        }
       
#if TEST
        amdlibLogTest("]");

        amdlibLogTest("phasor =[");
        for (i = 0; i < nbElem - 1; i++)
        {
            amdlibLogTest("%f, ", phasor[i]);
        }
        amdlibLogTest("%f]", phasor[nbElem - 1]);
#endif

        /*  The shift itself */
        p = fftw_plan_r2r_1d(nbElem, tabInPeriodized, fftDirect, FFTW_R2HC, FFTW_ESTIMATE);
        fftw_execute(p);
	fftw_destroy_plan(p);
        

        phasedFft[0] = phasor[0] * fftDirect[0];
        for (i = 1; i < (nbElem + 1) / 2; i++)
        { 
            /* f(x-x0)<=>F(u)exp(i*2*PI/N*u*x0) */
            /* Real part */
            phasedFft[i] = phasor[i] * fftDirect[i] +
                phasor[nbElem - i] * fftDirect[nbElem - i];

            /* Imaginary part */
            phasedFft[nbElem-i] = phasor[i] * fftDirect[nbElem - i] -
                phasor[nbElem - i] * fftDirect[i];
        }

        
        if (nbElem % 2 == 0) /* nbElem is even Real part only */
        {
            phasedFft[nbElem / 2] = 2 * phasor[nbElem / 2] * 
                fftDirect[nbElem / 2];
        }


#if TEST
        amdlibLogTest("fftDirect =[");
        for (i = 0; i < nbElem - 1; i++)
        {
            amdlibLogTest("%f, ", fftDirect[i]);
        }
        amdlibLogTest("%f]", fftDirect[nbElem-1]);

        amdlibLogTest("phasedFft =[");
        for (i = 0; i < nbElem - 1; i++)
        {
            amdlibLogTest("%f, ", phasedFft[i]);
        }
        amdlibLogTest("%f]", phasedFft[nbElem-1]);
#endif

        p = fftw_plan_r2r_1d(nbElem, phasedFft, tabDecalPeriodized, FFTW_HC2R, FFTW_ESTIMATE);
        fftw_execute(p);	
	fftw_destroy_plan(p);
        for (i = 0; i < nbElem; i++)
        {
            tabIn[i] = (1.0 / nbElem) * tabDecalPeriodized[i] + 
                drc[i];
        }
    }
    else if (shift == 0.0) 
    {
        memcpy(tabOut, tabIn, (size_t)nbElem * sizeof(double));
        amdlibShift_FREEALL();	
        return amdlibSUCCESS;
    }
        

    if (ipShift < 0)
    {
        for (i = 0; i < -ipShift; i++)
        {
            tabOut[i + ipShift + nbElem] = tabIn[i];
        }
        for (i = -ipShift; i<nbElem; i++)
        {
            tabOut[i + ipShift] = tabIn[i];
        }
    }
    else
    {
        for (i = 0; i < nbElem; i++)
        {
            tabOut[i] = (i < ipShift) ? 
                tabIn[i - ipShift + nbElem]:tabIn[i - ipShift];
        }
    }


#if TEST
    amdlibLogTest("tabDecalPeriodized =[");
    for (i = 0; i < nbElem - 1; i++)
    {
        amdlibLogTest("%f, ", tabDecalPeriodized[i]);
    }
    amdlibLogTest("%f]", tabDecalPeriodized[nbElem - 1]);

    amdlibLogTest("tabOut =[");
    for (i = 0; i < nbElem - 1; i++)
    {
        amdlibLogTest("%f, ", tabOut[i]);
    }
    amdlibLogTest("%f]", tabOut[nbElem - 1]);

    amdlibLogTest("tabIn =[");
    for (i = 0; i < nbElem - 1; i++)
    {
        amdlibLogTest("%f, ", tabIn[i]);
    }
    amdlibLogTest("%f]", tabIn[nbElem - 1]);
    amdlibLogTest("tabOut (entier) =[");
    for (i = 0; i < nbElem - 1; i++)
    {
        amdlibLogTest("%f, ", tabOut[i]);
    }
    amdlibLogTest("%f]", tabOut[nbElem - 1]);

    if (fpShift == 0.0) 
    {
        amdlibLogTest("tabOut (entier decal) =[");
        for (i = 0; i < nbElem - 1; i++)
        {
            amdlibLogTest("%f, ", tabOut[i]);
        }
        amdlibLogTest("%f]", tabOut[nbElem - 1]);
    }
#endif      

    amdlibShift_FREEALL();

    return amdlibSUCCESS;
}

/** Useful macro to free all allocated memory */
#define amdlibComputeShift_FREEALL() free(fft_tab2);      \
                                     free(fft_tab1);      \
                                     free(interSpectrum); \
                                     free(convolPadded);

/**
 * Computes shift between to value arrays
 *
 * This function computes the shift (possibly non-integer) between two arrays.
 *
 * @param nbElem number of element in arrays
 * @param tab1 first array 
 * @param tab2 second array 
 * @param shift computed shift between arrays
 * @param sigma2Shift error on computed shift
 * @param errMsg error description message returned if function fails.
 * 
 * @return
 * amdlibSUCCESS on successful completion. Otherwise amdlibFAILURE is returned.
 */
amdlibCOMPL_STAT amdlibComputeShift(/* INPUT */
                                    int             nbElem,
                                    double       *tab1,
                                    double       *tab2,
                                    /* OUTPUT */
                                    double       *shift,
                                    double       *sigma2Shift,
                                    amdlibERROR_MSG errMsg)
{
    int i;
    int nPhi = (nbElem + 1) / 2;

    /* Interpolated spectrum */
    double *interSpectrum = NULL;
    double *convolPadded = NULL;
    double *fft_tab1 = NULL;
    double *fft_tab2 = NULL ;
    int padFactor;
    int mxx;
    double max;


    fftw_plan p1;
    fftw_plan p2;

    amdlibLogTrace("amdlibComputeShift()");
    
    padFactor = 32;

    /* Allocate memory */
    interSpectrum = calloc(nbElem * padFactor, sizeof(double));
    if (interSpectrum == NULL)
    {
        amdlibComputeShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (interSpectrum)");		
        return amdlibFAILURE;
    }
    convolPadded = calloc(nbElem * padFactor, sizeof(double));
    if (convolPadded == NULL)
    {
        amdlibComputeShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (convolPadded)");
        return amdlibFAILURE;
    }
    fft_tab1     = calloc(nbElem, sizeof(double));
    if (fft_tab1 == NULL)
    {
        amdlibComputeShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (fft_tab1)");
        return amdlibFAILURE;
    }
    fft_tab2     = calloc(nbElem, sizeof(double));
    if (fft_tab2 == NULL)
    {
        amdlibComputeShift_FREEALL();
        amdlibSetErrMsg("Could not allocate memory (fft_tab2)");
        return amdlibFAILURE;
    }

    p1 = fftw_plan_r2r_1d(nbElem, tab1, fft_tab1, FFTW_R2HC, FFTW_ESTIMATE);
    fftw_execute(p1);
    fftw_destroy_plan(p1);
    p1 = fftw_plan_r2r_1d(nbElem,tab2, fft_tab2, FFTW_R2HC, FFTW_ESTIMATE);
    fftw_execute(p1);
    fftw_destroy_plan(p1);

    interSpectrum[0] = fft_tab1[0] * fft_tab2[0];
    for (i = 1; i < nPhi; i++)
    {
        interSpectrum[i] =
            fft_tab1[i] * fft_tab2[i] +
            fft_tab1[nbElem - i] * fft_tab2[nbElem - i];

        interSpectrum[nbElem * padFactor - i] =
            fft_tab1[nbElem - i] * fft_tab2[i] -
            fft_tab1[i] * fft_tab2[nbElem - i];
    }
    if (nbElem % 2 == 0) /* nbElem is even */
    {
        interSpectrum[nPhi - 1] = fft_tab1[nbElem / 2] * fft_tab2[nbElem / 2] +
            fft_tab1[nbElem / 2] * fft_tab2[nbElem / 2];
    }

    p2 = fftw_plan_r2r_1d(nbElem * padFactor, interSpectrum, convolPadded,FFTW_HC2R, FFTW_ESTIMATE);
    fftw_execute(p2);
    fftw_destroy_plan(p2);

    mxx = 0;
    max = convolPadded[mxx];
    for (i = 1; i < nbElem * padFactor; i++)
    {
        if(convolPadded[i] > max)
        {
            max = convolPadded[i];
            mxx = i;
        }
    }

    if (mxx > nbElem * padFactor / 2)
    {
        mxx = mxx - nbElem * padFactor;
    }

    *shift = -1 * (double) mxx / (double) padFactor;
    *sigma2Shift = 1.0 / (double) padFactor;

    /* Free memory */
    amdlibComputeShift_FREEALL();

    return amdlibSUCCESS;
}


