/*******************************************************************************
 * JMMC project ( http://www.jmmc.fr ) - Copyright (C) CNRS.
 ******************************************************************************/
/**
 * @file
 * Various (usefull) utilities for matrix manipulations
 */

#define _POSIX_SOURCE 1

/* 
 * System Headers
 */
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "fitsio.h"
/* 
 * Local Headers
 */
#include "amdlib.h"
#include "amdlibProtected.h"

/*
 * Protected functions
 */
/*
 *  This Quickselect routine is based on the algorithm described in
 *  "Numerical recipies in C", Second Edition,
 *  Cambridge University Press, 1992, Section 8.5, ISBN 0-521-43108-5
 */

#define PIX_SWAP(a,b) { double temp=(a);(a)=(b);(b)=temp; }

amdlibCOMPL_STAT amdlibQsortDouble(double *pix_arr, int npix)
{
    int         i,
                ir,
                j,
                k,
                l;
    int *       i_stack ;
    int         j_stack ;
    double  a ;

    ir = npix ;
    l = 1 ;
    j_stack = 0 ;
    i_stack = malloc(npix * sizeof(double)) ;
    for (;;) {
        if (ir-l < 7) {
            for (j=l+1 ; j<=ir ; j++) {
                a = pix_arr[j-1];
                for (i=j-1 ; i>=1 ; i--) {
                    if (pix_arr[i-1] <= a) break;
                    pix_arr[i] = pix_arr[i-1];
                }
                pix_arr[i] = a;
            }
            if (j_stack == 0) break;
            ir = i_stack[j_stack-- -1];
            l  = i_stack[j_stack-- -1];
        } else {
            k = (l+ir) >> 1;
            PIX_SWAP(pix_arr[k-1], pix_arr[l])
            if (pix_arr[l] > pix_arr[ir-1]) {
                PIX_SWAP(pix_arr[l], pix_arr[ir-1])
            }
            if (pix_arr[l-1] > pix_arr[ir-1]) {
                PIX_SWAP(pix_arr[l-1], pix_arr[ir-1])
            }
            if (pix_arr[l] > pix_arr[l-1]) {
                PIX_SWAP(pix_arr[l], pix_arr[l-1])
            }
            i = l+1;
            j = ir;
            a = pix_arr[l-1];
            for (;;) {
                do i++; while (pix_arr[i-1] < a);
                do j--; while (pix_arr[j-1] > a);
                if (j < i) break;
                PIX_SWAP(pix_arr[i-1], pix_arr[j-1]);
            }
            pix_arr[l-1] = pix_arr[j-1];
            pix_arr[j-1] = a;
            j_stack += 2;
            if (j_stack > npix) {
                amdlibLogError("stack too small in amdlibQsortDouble");
                return amdlibFAILURE ;
            }
            if (ir-i+1 >= j-l) {
                i_stack[j_stack-1] = ir;
                i_stack[j_stack-2] = i;
                ir = j-1;
            } else {
                i_stack[j_stack-1] = j-1;
                i_stack[j_stack-2] = l;
                l = i;
            }
        }
    }
    free(i_stack) ;
    return amdlibSUCCESS ;
}

#undef PIX_SWAP

#define PIX_SWAP(a,b) { double temp=(a);(a)=(b);(b)=temp; }
#define IDX_SWAP(a,b) { int temp=(a);(a)=(b);(b)=temp; }

amdlibCOMPL_STAT amdlibQsortDoubleIndexed(double *pix_arr, int *index, int npix)
{
    int         i,
                ir,
                j,
                k,
                l;
    int *       i_stack ;
    int         j_stack ;
    double  a ;
    int ia;

    /* intialise index */
    for (i=0;i<npix;i++) index[i]=i;

    ir = npix ;
    l = 1 ;
    j_stack = 0 ;
    i_stack = malloc(npix * sizeof(double)) ;
    for (;;) {
        if (ir-l < 7) {
            for (j=l+1 ; j<=ir ; j++) {
                a = pix_arr[j-1];
                ia = j-1;
                for (i=j-1 ; i>=1 ; i--) {
                    if (pix_arr[i-1] <= a) break;
                    pix_arr[i] = pix_arr[i-1];
                    index[i] = index[i-1];
                }
                pix_arr[i] = a;
                index[i] = ia;
            }
            if (j_stack == 0) break;
            ir = i_stack[j_stack-- -1];
            l  = i_stack[j_stack-- -1];
        } else {
            k = (l+ir) >> 1;
            PIX_SWAP(pix_arr[k-1], pix_arr[l])
            IDX_SWAP(index[k-1], index[l])
            if (pix_arr[l] > pix_arr[ir-1]) {
                PIX_SWAP(pix_arr[l], pix_arr[ir-1])
                IDX_SWAP(index[l], index[ir-1])
            }
            if (pix_arr[l-1] > pix_arr[ir-1]) {
                PIX_SWAP(pix_arr[l-1], pix_arr[ir-1])
                IDX_SWAP(index[l-1], index[ir-1])
            }
            if (pix_arr[l] > pix_arr[l-1]) {
                PIX_SWAP(pix_arr[l], pix_arr[l-1])
                IDX_SWAP(index[l], index[l-1])
            }
            i = l+1;
            j = ir;
            a = pix_arr[l-1];
            ia = index[l-1];
            for (;;) {
                do i++; while (pix_arr[i-1] < a);
                do j--; while (pix_arr[j-1] > a);
                if (j < i) break;
                PIX_SWAP(pix_arr[i-1], pix_arr[j-1]);
                IDX_SWAP(index[i-1], index[j-1]);
            }
            pix_arr[l-1] = pix_arr[j-1];
            index[l-1] = index[j-1];
            pix_arr[j-1] = a;
            index[j-1] = ia;
            j_stack += 2;
            if (j_stack > npix) {
                amdlibLogError("stack too small in amdlibQsortDouble");
                return amdlibFAILURE ;
            }
            if (ir-i+1 >= j-l) {
                i_stack[j_stack-1] = ir;
                i_stack[j_stack-2] = i;
                ir = j-1;
            } else {
                i_stack[j_stack-1] = j-1;
                i_stack[j_stack-2] = l;
                l = i;
            }
        }
    }
    free(i_stack) ;
    return amdlibSUCCESS ;
}

#undef PIX_SWAP
#undef IDX_SWAP

#define amdlibELEM_SWAP_SNGL(a,b) { register float t=(a);(a)=(b);(b)=t; }

float amdlibQuickSelectSngl(float* arr, int n) 
{
    int low, high ;
    int median;
    int middle, ll, hh;
    
    low = 0 ; high = n-1 ; median = (low + high) / 2;
    for (;;) {
        if (high <= low) /* One element only */
            return arr[median] ;
        
        if (high == low + 1) {  /* Two elements only */
            if (arr[low] > arr[high])
                amdlibELEM_SWAP_SNGL(arr[low], arr[high]) ;
            return arr[median] ;
        }

        /* Find median of low, middle and high items; swap into position low */
        middle = (low + high) / 2;
        if (arr[middle] > arr[high])    amdlibELEM_SWAP_SNGL(arr[middle], arr[high]) ;
        if (arr[low] > arr[high])       amdlibELEM_SWAP_SNGL(arr[low], arr[high]) ;
        if (arr[middle] > arr[low])     amdlibELEM_SWAP_SNGL(arr[middle], arr[low]) ;
        
        /* Swap low item (now in position middle) into position (low+1) */
        amdlibELEM_SWAP_SNGL(arr[middle], arr[low+1]) ;
        
        /* Nibble from each end towards middle, swapping items when stuck */
        ll = low + 1;
        hh = high;
        for (;;) {
            do ll++; while (arr[low] > arr[ll]) ;
            do hh--; while (arr[hh]  > arr[low]) ;
            
            if (hh < ll)
                break;
            
            amdlibELEM_SWAP_SNGL(arr[ll], arr[hh]) ;
        }
        
        /* Swap middle item (in position low) back into correct position */
        amdlibELEM_SWAP_SNGL(arr[low], arr[hh]) ;
        
        /* Re-set active partition */
        if (hh <= median)
            low = ll;
        if (hh >= median)
            high = hh - 1;
    }
}

#undef amdlibELEM_SWAP_SNGL
/*
 *  This Quickselect routine is based on the algorithm described in
 *  "Numerical recipies in C", Second Edition,
 *  Cambridge University Press, 1992, Section 8.5, ISBN 0-521-43108-5
 */


#define amdlibELEM_SWAP_DBLE(a,b) { register double t=(a);(a)=(b);(b)=t; }

double amdlibQuickSelectDble(double* OutArr, int n) 
{
    int low, high ;
    int median;
    int middle, ll, hh;
    double *arr, value;

    /* this version protects its input array now */
    arr=malloc(n*sizeof(double));
    memcpy(arr,OutArr,n*sizeof(double));

    low = 0 ; high = n-1 ; median = (low + high) / 2;
    for (;;) {
        if (high <= low) 
        { /* One element only */
            value = arr[median] ;
            free(arr);
            return(value);
        }
        if (high == low + 1) {  /* Two elements only */
            if (arr[low] > arr[high])
                amdlibELEM_SWAP_DBLE(arr[low], arr[high]) ;
            value = arr[median];
            free(arr);
            return(value);
        }

        /* Find median of low, middle and high items; swap into position low */
        middle = (low + high) / 2;
        if (arr[middle] > arr[high])    amdlibELEM_SWAP_DBLE(arr[middle], arr[high]) ;
        if (arr[low] > arr[high])       amdlibELEM_SWAP_DBLE(arr[low], arr[high]) ;
        if (arr[middle] > arr[low])     amdlibELEM_SWAP_DBLE(arr[middle], arr[low]) ;
        
        /* Swap low item (now in position middle) into position (low+1) */
        amdlibELEM_SWAP_DBLE(arr[middle], arr[low+1]) ;
        
        /* Nibble from each end towards middle, swapping items when stuck */
        ll = low + 1;
        hh = high;
        for (;;) {
            do ll++; while (arr[low] > arr[ll]) ;
            do hh--; while (arr[hh]  > arr[low]) ;
            
            if (hh < ll)
                break;
            
            amdlibELEM_SWAP_DBLE(arr[ll], arr[hh]) ;
        }
        
        /* Swap middle item (in position low) back into correct position */
        amdlibELEM_SWAP_DBLE(arr[low], arr[hh]) ;
        
        /* Re-set active partition */
        if (hh <= median)
            low = ll;
        if (hh >= median)
            high = hh - 1;
    }
}

#undef amdlibELEM_SWAP_DBLE

/** 
 * Macro to sort two pixel values.
 */
#define amdlibPIX_SWAP(a,b) { amdlibDOUBLE tmp=(a);(a)=(b);(b)=tmp; }
#define amdlibPIX_SORT(a,b) { if ((a)>(b)) amdlibPIX_SWAP((a),(b)); }
/**
 * Compute the median. 
 *
 * This function computes the median values of the entries in the given table.
 * It is superfast (since completely hardcoded median filter for 9 values).
 *
 * @note
 * Formula from: XILINX XCELL magazine, vol. 23 by John L. Smith
 * 
 * @param p table containing the 9 values.
 *
 * @return
 * The median value.
 */
amdlibDOUBLE amdlibMedian9(amdlibDOUBLE *p)
{
    amdlibPIX_SORT(p[1], p[2]); amdlibPIX_SORT(p[4], p[5]);
    amdlibPIX_SORT(p[7], p[8]); amdlibPIX_SORT(p[0], p[1]); 
    amdlibPIX_SORT(p[3], p[4]); amdlibPIX_SORT(p[6], p[7]); 
    amdlibPIX_SORT(p[1], p[2]); amdlibPIX_SORT(p[4], p[5]); 
    amdlibPIX_SORT(p[7], p[8]); amdlibPIX_SORT(p[0], p[3]); 
    amdlibPIX_SORT(p[5], p[8]); amdlibPIX_SORT(p[4], p[7]);
    amdlibPIX_SORT(p[3], p[6]); amdlibPIX_SORT(p[1], p[4]); 
    amdlibPIX_SORT(p[2], p[5]); amdlibPIX_SORT(p[4], p[7]); 
    amdlibPIX_SORT(p[4], p[2]); amdlibPIX_SORT(p[6], p[4]);
    amdlibPIX_SORT(p[4], p[2]); 
    return(p[4]) ;
}
#undef amdlibPIX_SORT



/**
 * Invert squared matrix by LU decomposition. 
 *
 * This function inverts a squared (dim,dim) matrix and writes the result in
 * place.
 *
 * @param matrix matrix to invert
 * @param dim dimension of the matrix
 *
 * @return
 * Always return amdlibSUCCESS
 */
amdlibCOMPL_STAT amdlibInvertMatrix(double *matrix, int dim)
{
    int    i;
    int    j;
    int    k;
    double sum = 0.0;
    double x = 1.0;

    amdlibLogTrace("amdlibInvertMatrix()");
    
    if (dim <= 0)
    {
        return amdlibFAILURE;  /* sanity check */
    }
    if (dim == 1) 
    {
        return amdlibFAILURE;  /* must be of dimension >= 2 */
    }

    for (i = 1; i < dim; i++)
    {
        matrix[i] /= matrix[0]; /* normalize row 0 */
    }

    for (i = 1; i < dim; i++)
    {
        for (j = i; j < dim; j++)  /* do a column of L */
        {
            sum = 0.0;
            for (k = 0; k < i; k++)
            {
                sum += matrix[j*dim + k] * matrix[k*dim + i];
            }
            matrix[j*dim + i] -= sum;
        }
        if (i == dim - 1)
        {
            continue;
        }
        for (j = i + 1; j < dim; j++)  /* do a row of U */
        {
            sum = 0.0;
            for (k = 0; k < i; k++)
            {
                sum += matrix[i*dim +k ] * matrix[k*dim + j];
            }
            matrix[i*dim + j] = (matrix[i*dim + j] - sum) / matrix[i*dim + i];
        }
    }
    for (i = 0; i < dim; i++)  /* invert L */
    {
        for (j = i; j < dim; j++)
        {
            x = 1.0;
            if (i != j)
            {
                x = 0.0;
                for (k = i; k < j; k++)
                    x -= matrix[j*dim + k] * matrix[k*dim + i];
            }
            matrix[j*dim + i] = x / matrix[j*dim + j];
        }
    }
    for (i = 0; i < dim; i++)   /* invert U */
    {
        for (j = i; j < dim; j++)
        {
            if (i == j)
            {
                continue;
            }
            sum = 0.0;
            for (k = i; k < j; k++)
            {
                sum += matrix[k*dim + j] * ((i == k) ? 1.0 : matrix[i*dim +k]);
            }
            matrix[i*dim + j] = -sum;
        }
    }

    for (i = 0; i < dim; i++)   /* final inversion */
    {
        for (j = 0; j < dim; j++)
        {
            sum = 0.0;
            for (k = ((i > j) ? i : j); k < dim; k++)
            {
                sum += ((j == k) ? 1.0 : matrix[j*dim + k]) * matrix[k*dim +i];
            }
            matrix[j*dim + i] = sum;
        }
    }
    return amdlibSUCCESS;
}

/**
 * Tranpose matrix. 
 *
 * This function transposes (dim1,dim2) matrix into (dim1,dim2) matrix 
 *
 * @param matrix matrix to transpose
 * @param tmatrix transposed  matrix
 * @param dim1 dimension along X-axis of the matrix
 * @param dim2 dimension along Y-axis of the matrix
 *
 * @return
 * Always return amdlibSUCCESS
 */
void amdlibTransposeMatrix(double *matrix, double *tmatrix, int dim1, int dim2)
{
    int i;
    int j;

    amdlibLogTrace("amdlibTransposeMatrix()");
    
    for (i = 0; i < dim2; i++)
    {
        for (j = 0; j < dim1; j++)
        {
            tmatrix[i*dim1 + j] = matrix[j*dim2 + i]; 
        }
    }
}

/**
 * Computes the product matrix
 *
 * This function computes the product of a (dim1,dim2) matrix with a (dim2,dim3)
 * matrix.
 *
 * @param matrix1 matrix 1 used in product 
 * @param matrix2 matrix 2 used in product  
 * @param matprod matrix resulting of matrix product
 * @param dim1 dimension along X-axis of the matrix 1
 * @param dim2 dimension along Y-axis of the matrix 1 and along X-axis of the
 * matrix 2
 * @param dim3 dimension along Y-axis of the matrix 2
 *
 * @return
 * Always return amdlibSUCCESS
 */
amdlibCOMPL_STAT amdlibProductMatrix(double *matrix1, double *matrix2,
                                     double *matprod, int dim1, int dim2,
                                     int dim3)
{
    int i,j,k;

    amdlibLogTrace("amdlibProductMatrix()");
    
    for (i = 0; i < dim1; i++)
    {
        for (j = 0; j < dim3; j++)
        {
            matprod[i*dim3 + j] = 0.;
            for (k = 0; k < dim2; k++)
            {
                matprod[i*dim3 + j] += matrix1[i*dim2 + k] * 
                    matrix2[k*dim3 + j];
            }
        }
    }
    return amdlibSUCCESS;
}

/**
 * Compute the covariance of 2 observables x and y
 *
 * This function computes the covariance of 2 observables x and y
 * matrixes. These observables are should be stored in multi-dimensional array
 * having the following dimensions [nbFrames][nbWlen][nbBases].
 * The resulting covariance is stored in a vector [nbWlen][nbBases].
 *
 * @param x first observable  
 * @param y second observable
 * @param lambda wavelength index at which covariance is computed
 * @param nbWlen number of wavelengths
 * @param nbFrames number of frames
 * @param nbBases number of bases
 * @param cov_xy vector where result is stored 
 */
void amdlibComputeMatrixCov(double *x, double *y, int lambda, int nbWlen, 
                      int nbFrames, int nbBases, double *cov_xy)
{
    int    kbase;
    int    ifram;
    double prod = 0.;
    double av_x = 0.;
    double av_y = 0.;

    amdlibLogTrace("amdlibComputeMatrixCov()");
    
    for (kbase = 0; kbase < nbBases; kbase++)
    {
        prod = 0.;
        av_x = 0.;
        av_y = 0.;
        for (ifram = 0; ifram < nbFrames; ifram++)
        {
            /* sum(xy) */
            prod += x[nbBases*nbWlen*ifram + nbBases*lambda + kbase] *
                y[nbBases*nbWlen*ifram + nbBases*lambda + kbase];
            /* sum(x) */
            av_x += x[nbBases*nbWlen*ifram + nbBases*lambda + kbase];
            /* sum(y) */
            av_y += y[nbBases*nbWlen*ifram + nbBases*lambda + kbase];
        }
        /* averages */
        prod /= nbFrames;
        av_x /= nbFrames;
        av_y /= nbFrames;
        /* cov(x,y) = average(xy)-average(x)*average(y) */
        cov_xy[nbBases*lambda + kbase] = prod - (av_x * av_y);
    }
}

/**
 * Compute the covariance of 2 vectors x and y
 *
 * This function computes the covariance of 2 vectors x and y.
 * having the dimension [nbFrames].
 * The resulting covariance is stored in a double.
 *
 * @param x first observable  
 * @param y second observable
 * @param nbFrames number of frames
 */
double amdlibComputeCov(double *x, double *y, int nbFrames)
{
    int    ifram;
    double prod = 0.;
    double av_x = 0.;
    double av_y = 0.;
    double cov_xy = 0.;

    prod = 0.;
    av_x = 0.;
    av_y = 0.;
    for (ifram = 0; ifram < nbFrames; ifram++)
    {
        /* sum(xy) */
        prod += x[ifram] * y[ifram];
        /* sum(x) */
        av_x += x[ifram];
        /* sum(y) */
        av_y += y[ifram];
    }
    /* averages */
    prod /= nbFrames;
    av_x /= nbFrames;
    av_y /= nbFrames;
    /* cov(x,y) = average(xy)-average(x)*average(y) */
    cov_xy  = prod - (av_x * av_y);
    return (cov_xy);
}

double amdlibComputeWeightedCov(int n, double *x, double *wx, 
                                double *y, double *wy)
{
    int    i, nv=0;
    double prod = 0.;
    double av_x = 0.;
    double av_y = 0.;
    double cov_xy = 0.;

    prod = 0.;
    av_x = 0.;
    av_y = 0.;
    for (i = 0; i < n; i++)
    {
        if ((x[i] == x[i]) && (wx[i] > 0) && (y[i] == y[i]) && (wx[i] > 0))
        {
            nv++;
            /* sum(xy) */
            prod += x[i] * y[i];
            /* sum(x) */
            av_x += x[i];
            /* sum(y) */
            av_y += y[i];
        }
    }
    if (nv > 0)
    {
        /* averages */
        prod /= nv;
        av_x /= nv;
        av_y /= nv;
        /* cov(x,y) = average(xy)-average(x)*average(y) */
        cov_xy  = prod - (av_x * av_y);
        return (cov_xy);
    }
    return(0.0);
}


void amdlibLinearFit(int n, double *x, double *wx, double *y, 
                       double *wy, double *a, double *b)
{
    double sx,sy,sxy,mx,my;
    
    sx=amdlibPow2(amdlibRmsTable(n,x,wx));
    sy=amdlibPow2(amdlibRmsTable(n,y,wy));
    mx=amdlibAvgTable(n,x,wx);
    my=amdlibAvgTable(n,y,wy);
    sxy=amdlibComputeWeightedCov(n,x,wx,y,wy);
    *b=sxy/sx;
    *a=my-(*b)*mx;
}

void amdlibBoxCarSmooth(int n, double *y, int w)
{
    double *x;
    int i,j;
    
    x=calloc(n,sizeof(double));
    for (i=0;i<n;i++) 
    {
        x[i]=y[i];
    }
    
    for (i=((w-1)/2)-1;i<n-(w+1)/2;i++)
    {
        x[i]=0.0;
        for (j=0;j<w;j++)
        {
            x[i]+=y[i+j-w/2];
        }
    }
    for (i=((w-1)/2)-1;i<n-(w+1)/2;i++)
    {
        y[i]=x[i]/w;
    }
    free(x);
}

void amdlibHalfComplexGaussianShape(int n, double *y, double fwhm)
{
    int i;
    double c,x, val;
    double fact1=2.354820044;

    c=2.0/fwhm;
    c=c/fact1;
    y[0]=1;
    /* This is a multiplying factor on the complex, i.e., the real and
     * imaginary parts which are stored in a halfComplex as follows:
     * r0, r1, r2, ..., rn/2, i(n+1)/2-1, ..., i2, i1 */
    y[0] = 1; 
    for (i = 1; i < (n + 1) / 2; i++)
    {
        x=2.0*(double)i/(double)n;
        val=exp(-1*x*x/2/c/c) ;
        y[i] = val; 
        y[n- i] = val;
    }
    if (n % 2 == 0) 
    {
        x=2.0;
        val=exp(-1*x*x/2/c/c) ;
        y[n/ 2] = val; 
    }

}

void amdlibGaussSmooth(int n, double *y, double w)
{
#include <complex.h>
#include <fftw3.h>
    double *x,*fft,*g;
    double lin;
    double debut = y[0];
    double fin = y[n - 1];
    int i;

    fftw_plan p;

    x=calloc(n,sizeof(double));
    fft=calloc(n,sizeof(double));
    g=calloc(n,sizeof(double));

    /* compute gauss fft shape */
    amdlibHalfComplexGaussianShape(n, g, w);

    /*remove the linear function between start and end */
    for (i=0;i<n;i++) 
    {
        lin = ((fin-debut) / (n - 1)) * i + debut;
        x[i]=y[i]-lin;
    }

    /* we use fftw's HalfComplex format in amdlib */
    p = fftw_plan_r2r_1d(n, x, fft, FFTW_R2HC, FFTW_ESTIMATE);
    fftw_execute(p);
    fftw_destroy_plan(p);
    /* multiply */

    for (i = 0; i < n ; i++)
    {
        fft[i] *= g[i]; 
    }

    p = fftw_plan_r2r_1d(n, fft, x, FFTW_HC2R, FFTW_ESTIMATE);
    fftw_execute(p);
    fftw_destroy_plan(p);

    /* give back with normalization N (fftw) and initial slope */
    for (i=0;i<n;i++) 
    {
        lin = ((fin-debut) / (n - 1)) * i + debut;
        y[i]=x[i]/n+lin;
    }
    free(g);
    free(fft);
    free(x);
}

void amdlibLinearize(int n, double *y, double *wy)
{
    double *x,*wx;
    double a=0,b=0;
    int i;
    x=calloc(n,sizeof(double));
    wx=calloc(n,sizeof(double));
    for (i=0;i<n;i++) 
    {
        x[i]=i;
        wx[i]=1.0;
    }
    amdlibLinearFit(n,x,wx,y,wy,&a,&b);
    for (i=0;i<n;i++) 
    {
        y[i]=a+b*x[i];
    }
    
    free(x);
    free(wx);
}

/**
 * Calculate ponderated mean.
 *
 * @param nbPix size of table.
 * @param table input data.
 * @param sigma2 ponderation values.
 *
 * @return the average value.
 */
double amdlibAvgTable(int    nbPix,
                      double *table,
                      double *sigma2)
{
    double ponderation = 0;
    double avg = 0;
    int    iPix;

    for (iPix = 0; iPix < nbPix; iPix++)
    {
        if ((table[iPix] == table[iPix]) && (sigma2[iPix] > 0))
        {
            avg += table[iPix] / sigma2[iPix];
            ponderation += 1.0 / sigma2[iPix];
        }
    }
    if (ponderation > 0)
    {
        avg /= ponderation;
    }

    return avg;
}

/**
 * Calculate the residual rms.
 *
 * @param nbPix size of table.
 * @param table input data.
 * @param sigma2 ponderation values.
 *
 * @return the value of rms.
 */
double amdlibRmsTable(int    nbPix,
                      double *table,
                      double *sigma2)
{
    double ponderation = 0;
    double avg = 0;
    double rms = 0;
    int    iPix;

    avg = amdlibAvgTable(nbPix, table, sigma2);

    for (iPix = 0; iPix < nbPix; iPix++)
    {
        if ((table[iPix] == table[iPix]) && (sigma2[iPix] > 0))
        {
            rms += amdlibPow2(table[iPix] - avg) / sigma2[iPix];
            ponderation += 1.0 / sigma2[iPix];
        }
    }
    
    if (ponderation > 0)
    {
        rms /= ponderation;
        rms = sqrt(rms);
    }
    return rms;
}


/**
 * Calculate mean.
 *
 * @param nbPix size of table.
 * @param table input data.
 * @return the average value.
 */
double amdlibAvgValues(int nbPix, double *table)
{
    double ponderation = 0;
    double avg = 0;
    int    iPix;
    
    for (iPix = 0 ; iPix < nbPix; iPix++)
    {
        if (table[iPix] == table[iPix]) /*nan-protected*/
        {
            avg += table[iPix] ;
            ponderation += 1.0 ;
        }
    }
    if (ponderation > 0)
    {
        avg /= ponderation;
    }

    return avg;
}

/**
 * Calculate the residual rms.
 *
 * @param nbPix size of table.
 * @param table input data.
 * @return the value of rms.
 */
double amdlibRmsValues(int nbPix, double *table)
{
    double ponderation = 0;
    double avg = 0;
    double rms = 0;
    int    iPix;

    avg = amdlibAvgValues(nbPix, table);

    for (iPix = 0; iPix < nbPix; iPix++)
    {
        if (table[iPix] == table[iPix]) /*nan-protected*/
        {
            rms += amdlibPow2(table[iPix] - avg);
            ponderation += 1.0;
        }
    }
    
    if (ponderation > 0)
    {
        rms /= ponderation;
        rms = sqrt(rms);
    }
    return rms;
}


/**
 * Compute signed square root
 *
 * @param a input value.
 *
 * @return the value computed.
 */
double amdlibSignedSqrt(double a)
{
    double val = a;
    if (a != 0.0)
    {
        val = (a / fabs(a)) * sqrt(fabs(a));
    }
    return val;
}

#define amdlibCorrect3DVisTableFromAchromaticPiston_FREEALL() \
    amdlibFree3DArrayComplex(tmpCpxVis);        

/**
 * Removes a constant phase (achromatic piston) on a complexVisibility vector.
 */
amdlibCOMPL_STAT amdlibCorrect3DVisTableFromAchromaticPiston(
                                            amdlibCOMPLEX ***cpxVisTable, 
                                            amdlibCOMPLEX ***cNopTable,
                                            int nbFrames, 
                                            int nbBases, 
                                            int nbLVis, 
                                            amdlibDOUBLE *wlen,
                                            amdlibDOUBLE **pst,
                                            amdlibERROR_MSG errMsg)
{

    int iFrame; 
    int iBase; 
    int lVis, nbGoodVis;
    double x;
    amdlibCOMPLEX phasor, cpxVis, cpxVisAvg;
    amdlibCOMPLEX *** tmpCpxVis = NULL;

    tmpCpxVis = amdlibAlloc3DArrayComplex(nbLVis, nbBases, nbFrames, errMsg);
    if (tmpCpxVis == NULL)
    {
        amdlibCorrect3DVisTableFromAchromaticPiston_FREEALL();
        return amdlibFAILURE;
    }
    for (iFrame = 0; iFrame < nbFrames; iFrame++)
    {
        for (iBase = 0; iBase < nbBases; iBase++)
        {
            if( !(amdlibCompareDouble(pst[iFrame][iBase],amdlibBLANKING_VALUE)))
	    {
                for (lVis = 0; lVis < nbLVis; lVis++) /*Flagged Vis not important here*/
		{
                    /* Note older versions artificially removed -1*pst 
                     * since the pst sign in amdlib was kept wrong because
                     * the OPD correction at ESO was in the wrong direction.
                     * To use this new version in the OS at Paranal, one must
                     * change the sign of the displacements of, e.g., the Delay
                     * lines that are used in templates.
                     * x = (2 * M_PI/wlen[lVis]) * -1*pst[iFrame][iBase]; */
                    x = (2 * M_PI/wlen[lVis]) * pst[iFrame][iBase];
                    phasor.re = cos(x);
                    phasor.im = -sin(x);
                    cpxVis.re = cpxVisTable[iFrame][iBase][lVis].re;
                    cpxVis.im = cpxVisTable[iFrame][iBase][lVis].im;
 
                    /* Store complex Product of the phasor with the coherent flux
                       in cNopTable*/
                    amdlibCpxMul(cpxVis, phasor, cNopTable[iFrame][iBase][lVis]);
		}
                /*Flag cNopTable where cpxVis was Flagged */
                for (lVis = 0; lVis < nbLVis; lVis++) 
		{
                    if (amdlibCompareDouble(cpxVisTable[iFrame][iBase][lVis].re,amdlibBLANKING_VALUE))
                    {
                        cNopTable[iFrame][iBase][lVis].re=amdlibBLANKING_VALUE;
                        cNopTable[iFrame][iBase][lVis].im=amdlibBLANKING_VALUE;
                    }
                }
                /* Recenter this by removing the mean, taking account of bad Vis */
                /* Compute the mean over lambda */
                cpxVisAvg.re = 0.0;
                cpxVisAvg.im = 0.0;
                for (nbGoodVis=0, lVis = 0; lVis < nbLVis; lVis++)
		{
                    if (!amdlibCompareDouble(cNopTable[iFrame][iBase][lVis].re,amdlibBLANKING_VALUE))
                    {
                        cpxVisAvg.re += cNopTable[iFrame][iBase][lVis].re;
                        cpxVisAvg.im += cNopTable[iFrame][iBase][lVis].im;
                        nbGoodVis++;
                    }
		}
                cpxVisAvg.re /= nbGoodVis;
                cpxVisAvg.im /= nbGoodVis;
                /* Store conjugate complex values in tmpCpxVis as (constant) 
                 * vector */
                for (lVis = 0; lVis < nbLVis; lVis++)
		{
                    tmpCpxVis[iFrame][iBase][lVis].re = cpxVisAvg.re;
                    tmpCpxVis[iFrame][iBase][lVis].im = -cpxVisAvg.im;
		}
                /*Remove constant by multiplying the two */
                for (lVis = 0; lVis < nbLVis; lVis++)
		{
                    phasor.re = tmpCpxVis[iFrame][iBase][lVis].re;
                    phasor.im = tmpCpxVis[iFrame][iBase][lVis].im;
                    cpxVis.re = cNopTable[iFrame][iBase][lVis].re;
                    cpxVis.im = cNopTable[iFrame][iBase][lVis].im;
                    /* Store Complex Product of the phasor with the coherent flux
                       in cNopTable*/
                    amdlibCpxMul(cpxVis, phasor, cNopTable[iFrame][iBase][lVis]);
		}
                /*re-Flag cNopTable where cpxVis was Flagged */
                for (lVis = 0; lVis < nbLVis; lVis++) 
		{
                    if (amdlibCompareDouble(cpxVisTable[iFrame][iBase][lVis].re,amdlibBLANKING_VALUE))
                    {
                        cNopTable[iFrame][iBase][lVis].re=amdlibBLANKING_VALUE;
                        cNopTable[iFrame][iBase][lVis].im=amdlibBLANKING_VALUE;
                    }
                }
	    }
            else
            {
                for (lVis = 0; lVis < nbLVis; lVis++)
		{
                    cNopTable[iFrame][iBase][lVis].re=amdlibBLANKING_VALUE;
                    cNopTable[iFrame][iBase][lVis].im=amdlibBLANKING_VALUE;
                }
            }
        }
    }
    amdlibCorrect3DVisTableFromAchromaticPiston_FREEALL();

    return amdlibSUCCESS;
}

int amdlibFindIndexOfMinimum(double *data, int N)
{
    double min = data[0];
    int theIndex = 0;
    int i;
    for (i = 1; i < N; i++)
    {
        if (data[i] < min)
        {
            min = data[i];
            theIndex = i;
        }
    }
    return (theIndex);
}

double amdlibArrayDoubleMinimum(double *data, int N)
{
    double min = data[0];
    int i;
    for (i = 1; i < N; i++)
    {
        if (data[i] < min)
        {
            min = data[i];
        }
    }
    return (min);
}

int amdlibFindIndexOfMaximum(double *data, int N)
{
    double max = data[0];
    int theIndex = 0;
    int i;
    for (i = 1; i < N; i++)
    {
        if (data[i] > max)
        {
            max = data[i];
            theIndex = i;
        }
    }
    return (theIndex);
}

double amdlibArrayDoubleMaximum(double *data, int N)
{
    double max = data[0];
    int i;
    for (i = 1; i < N; i++)
    {
        if (data[i] > max)
        {
            max = data[i];
        }
    }
    return (max);
}

double amdlibArrayDoubleSum(double *data, int N)
{
    double sum = 0;
    int i;
    for (i = 1; i < N; i++)
    {
        sum += data[i];
    }
    return (sum);
}

int amdlibCompareDouble(double x, double y)
 {
     if ( fabs(x-y) < (double)amdlibPRECISION )
     {
         return 1;
     }
     else
     {
         return 0;
     }
 }

amdlibCOMPL_STAT amdlibComputeUVCoords(amdlibSCIENCE_DATA *data,
                                       int nbBases,
                                       amdlibDOUBLE **uCoord, /* uCoord[iFrame][iBase]*/
                                       amdlibDOUBLE **vCoord) /* vCoord[iFrame][iBase]*/
{
    double ha,x,y,z;
    double xx,yy,zz;
    double cl,sl,ch,sh,cd,sd;
    int iBase, iFrame, nbFrames=data->nbFrames;
    double deltaT;

    /* play it for lst and lst+duration*/
    cl=cos(data->issInfo.geoLat);
    cd=cos(data->issInfo.dec);
    sl=sin(data->issInfo.geoLat);
    sd=sin(data->issInfo.dec);
    /* get baseline vectors in equatorial local coords */
    for (iBase = 0; iBase < nbBases; iBase++)
    {
        int tel1;
        int tel2;
        if (nbBases == 1)
        {
            tel1 = 0;
            tel2 = 1;
        }
        else
        {
            tel1 = amdlibMin((iBase % nbBases) , ((iBase+1) % nbBases) );
            tel2 = amdlibMax((iBase % nbBases) , ((iBase+1) % nbBases) );
        }
        
        /*axes x and y seem reverted wrt the aspro convention. I use aspro sign:*/
        x=(-1*data->issInfo.stationCoordinates[0][tel2])-(-1*data->issInfo.stationCoordinates[0][tel1]);
        y=(-1*data->issInfo.stationCoordinates[1][tel2])-(-1*data->issInfo.stationCoordinates[1][tel1]);
        z=data->issInfo.stationCoordinates[2][tel2]-data->issInfo.stationCoordinates[2][tel1];
    
        /* convert to equatorial local */
        xx=0*x-sl*y+cl*z;
        yy=x+0*y+0*z;
        zz=0*x+cl*y+sl*z;

        for (iFrame=0; iFrame < nbFrames; iFrame++)
        {
            deltaT = (data->timeTag[iFrame]-data->timeTag[0]); /*seconds*/
            /* hour angle start */
            ha=fmod(data->issInfo.lst-data->issInfo.ra+(deltaT*M_PI/3600.0/12.0),2*M_PI);
            ch=cos(ha);
            sh=sin(ha);
            /* project onto star direction */
            uCoord[iFrame][iBase]=sh*xx+ch*yy;
            vCoord[iFrame][iBase]=-sd*ch*xx+sd*sh*yy+cd*zz;
        }
    }
    return amdlibSUCCESS;
}

amdlibCOMPL_STAT amdlibComputeBaselines(amdlibISS_INFO *iss, int nbBases, 
                                        double duration)
{
    double ha,x,y,z,u,v,w;
    double xx[amdlibNBASELINE],yy[amdlibNBASELINE],zz[amdlibNBASELINE];
    double cl,sl,ch,sh,cd,sd;
    int i;

    /* play it for lst and lst+duration*/
    cl=cos(iss->geoLat);
    cd=cos(iss->dec);
    sl=sin(iss->geoLat);
    sd=sin(iss->dec);
    /* get baseline vectors in equatorial local coords */
    for (i = 0; i < nbBases; i++)
    {
        int tel1;
        int tel2;
        if (nbBases == 1)
        {
            tel1 = 0;
            tel2 = 1;
        }
        else
        {
            tel1 = amdlibMin((i % nbBases) , ((i+1) % nbBases) );
            tel2 = amdlibMax((i % nbBases) , ((i+1) % nbBases) );
        }

        /*axes x and y seem reverted wrt the aspro convention. I use aspro sign:*/
        x=(-1*iss->stationCoordinates[0][tel2])-(-1*iss->stationCoordinates[0][tel1]);
        y=(-1*iss->stationCoordinates[1][tel2])-(-1*iss->stationCoordinates[1][tel1]);
        z=iss->stationCoordinates[2][tel2]-iss->stationCoordinates[2][tel1];

        /* convert to equatorial local */
        xx[i]=0*x-sl*y+cl*z;
        yy[i]=x+0*y+0*z;
        zz[i]=0*x+cl*y+sl*z;
    }
    
    for (i = 0; i < nbBases; i++)
    {
        /* hour angle start */
        ha=fmod(iss->lst-iss->ra,2*M_PI);
        ch=cos(ha);
        sh=sin(ha);
        /* project onto star direction */
        u=sh*xx[i]+ch*yy[i];
        v=-sd*ch*xx[i]+sd*sh*yy[i]+cd*zz[i];
        w=cd*ch*xx[i]-cd*sh*yy[i]+sd*zz[i];
        /* Update PBL and PBLA at Start and End to enable further interpolation of
         * u and v in amdlibVisibilities. */
        iss->projectedBaseStart[i]=sqrt(u*u+v*v);
        /* PBLA is a PA East of North */
        iss->projectedAngleStart[i]=fmod(90.0-(atan2(v,u)*180.0/M_PI),360.0);
        /* hour angle end. duration is in seconds of time, convert to radians */
        ha=fmod(iss->lst-iss->ra+(duration*M_PI/3600.0/12.0),2*M_PI);
        ch=cos(ha);
        sh=sin(ha);
        /* project onto star direction */
        u=sh*xx[i]+ch*yy[i];
        v=-sd*ch*xx[i]+sd*sh*yy[i]+cd*zz[i];
        w=cd*ch*xx[i]-cd*sh*yy[i]+sd*zz[i];
        /* Update PBL and PBLA at Start and End to enable further interpolation of
         * u and v in amdlibVisibilities. */
        iss->projectedBaseEnd[i]=sqrt(u*u+v*v);
        iss->projectedAngleEnd[i]=fmod(90.0-(atan2(v,u)*180.0/M_PI),360.0);
    }
    return amdlibSUCCESS;
}


double amdlibAbacusErrPhi(double x)
/**
 * Estimate true phase rms from the cross-spectrum variance.
 * see Petrov, Roddier and Aime, JOSAA vol 3, N°5, may 1986 p 634.
 * and Petrov's Thesis, p. 50 ff.
 * I replace the piecewise interpolation usually used by a polynomial
 * approximation of the function:
 * if z=log10(y), 
 * then z=(C[1]*X^7+C[2]*X^6+C[3]*X^5+C[4]*X^4+C[5]*X^3+C[6]*X^2+C[7]*X+C[8])
 * and y=10^z.
 * where
 * C[01]= 2.71918080109099
 * C[02]=-17.1901043936273
 * C[03]= 45.0654103760899
 * C[04]=-63.4441678243197
 * C[05]= 52.3098941426378
 * C[06]=-25.8090699917488
 * C[07]= 7.84352873962491
 * C[08]=-1.57308595820081
 * This interpolation is valid in the range x=[0.1,1.74413]. 
 * Error is 1% everywhere except above x=1.73 where it is 10%
 * Below x=0.1: y=x
 * Above x=M_PI/sqrt(3.0), y = blanking (impossible value)
 * Above x=1.74413, we take: y=0.691/(pi/sqrt(3.0)-x)
 */
{
    const double Asymptot=M_PI/sqrt(3.0);
    double c[8]={2.7191808010909,
                 -17.1901043936273,
                 45.0654103760899,
                 -63.4441678243197,
                 52.3098941426378,
                 -25.8090699917488,
                 7.84352873962491,
                 -1.57308595820081};
    
    double x2,x3,x4,x5,x6,x7,z;
    if(x>Asymptot)
    { 
        return((double)amdlibBLANKING_VALUE);
    }
    if(x>1.74413)
    {
        return(0.691/(Asymptot-x));
    }
    if(x<0.1)
    {
        return(x);
    }
    x2=x*x;
    x3=x2*x;
    x4=x2*x2;
    x5=x3*x2;
    x6=x3*x3;
    x7=x6*x;
    z=c[0]*x7+c[1]*x6+c[2]*x5+c[3]*x4+c[4]*x3+c[5]*x2+c[6]*x+c[7];
    return pow(10,z);
}

double amdlibRefractiveAirIndex(double lambda, 
                                double temp,
                                double pressure,
                                double partialWaterVaporPressure)
/*
*   Compute the refractive air index by Edlen's formula.
*   It can be found in 'Owens, 1967', p-56 Eq.32
*
*   Inputs:
*    - lambda: (double) wavelength in nm (AMBER).
*    - temp, double: temperature in [K]
*    - pressure, double: pressure in [Pascal]
*    - partialWaterVaporPressure, double: partial pressure of water vapor in [Pascal]
*
*   Outputs:
*    - amdlibRefractiveAirIndex, double: 
*      refractive index of air at lambda.
*      nb: the air index n is defined as
*      n = 1 + amdlibRefractiveAirIndex*1e-8
*
*   This function is validated. It can be improved by the work of Vannier M.
*   which try to take into account a better description of the water vapor
*   dispersion.
*   credits: JB Le Bouquin  
*   SEE ALSO: Owens, 1967, p-56 Eq.32
*/
{
    double t,p,f,s2,l,r;
    /* initial conversion */
    t = temp - 273.15;       /* t in [deg Centigrade] */
    p = pressure / 1.333224e+02; /* p in [torricelli] */
    f = partialWaterVaporPressure / 1.333224e+02; /* f in [torricelli] */
    l=1.0E3*lambda;      /* constants below are for lambda in [micron] */
    s2 = 1.0/(l*l);
    
    /* air refraction , Owens, 1967, p-56  Eq 32 */
    r = (8342.13 + 2406030./(130.-s2) + 15997/(38.9-s2)) *
    (p/720.775) *
    (1 + p*(0.817 - 0.0133*t)*1e-6) / (1. + 0.0036610*t) +
    f*(5.722 - 0.0457*s2); 
  
  return r;
}

/*___oOo___*/
