/*                                                                              *
 *   This file is part of the ESO UVES Pipeline                                 *
 *   Copyright (C) 2004,2005 European Southern Observatory                      *
 *                                                                              *
 *   This library is free software; you can redistribute it and/or modify       *
 *   it under the terms of the GNU General Public License as published by       *
 *   the Free Software Foundation; either version 2 of the License, or          *
 *   (at your option) any later version.                                        *
 *                                                                              *
 *   This program is distributed in the hope that it will be useful,            *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of             *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
 *   GNU General Public License for more details.                               *
 *                                                                              *
 *   You should have received a copy of the GNU General Public License          *
 *   along with this program; if not, write to the Free Software                *
 *   Foundation, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA       *
 *                                                                              */

/*
 * $Author: amodigli $
 * $Date: 2013-02-12 10:56:25 $
 * $Revision: 1.92 $
 * $Name: not supported by cvs2svn $
 *
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

/*----------------------------------------------------------------------------*/
/**
 * @defgroup uves_utils_cpl  Utility functions (low level)
 *
 * This module contains low level library functions that can be removed if/when
 * the corresponding functionality is in the CPL.
 */
/*----------------------------------------------------------------------------*/

/**@{*/

#include <uves_utils_cpl.h>

#include <uves_utils.h>
#include <uves_utils_wrappers.h>
#include <uves_dump.h>
#include <uves_error.h>

#include <cpl.h>
#include <stdbool.h>
#include <string.h>

static cpl_image *filter_median(const cpl_image *image, int radx, int rady,
                bool extrapolate_border);
    

/*----------------------------------------------------------------------------*/
/**
   @brief   Find named property
   @param   plist          propertylist to search
   @param   name           property name
   @param   number         number of property to find (counting from zero).
                           E.g. setting number=1 would return the 2nd property with
                           matching name
   @return  first property with given name, or NULL if not found

   No error is set if the property is not found
*/
/*----------------------------------------------------------------------------*/
const cpl_property *
uves_find_property_const(const uves_propertylist *plist, const char *name,
                   int number)
{
    int i = 0;
    int size = uves_propertylist_get_size(plist);

    assure( number >= 0, CPL_ERROR_ILLEGAL_INPUT, "Number (%d) must be non-negative",
            number);

    for (i = 0; i < size; i++)
    {
        const cpl_property *p = uves_propertylist_get_const(plist, i);

        if (strcmp(cpl_property_get_name(p), name) == 0)
        {
                    if (number == 0)
                        {
                            return p;
                        }
                    else
                        /* Continue search */
                        {
                            number--;
                        }
        }
    }

  cleanup:
    return NULL;
}
cpl_property *
uves_find_property(uves_propertylist *plist, const char *name,
                   int number)
{
    return (cpl_property *) uves_find_property_const(plist, name, number);
}

/*----------------------------------------------------------------------------*/
/**
   @brief   Average filter
   @param   image          Image to filter
   @param   radius_x       width of filter window is 2*radius_x + 1
   @param   radius_y       height of filter window is 2*radius_y + 1
   @return  CPL_ERROR_NONE iff OK.

   The @em image must be of type @em CPL_TYPE_DOUBLE.

   There is currently no handling of bad pixels.
*/
/*----------------------------------------------------------------------------*/
cpl_error_code
uves_filter_image_average(cpl_image *image, int radius_x, int radius_y)
{
    cpl_image *aux = NULL;
    double *image_data = NULL;
    double *aux_data = NULL;
    int nx, ny;
    int i;

    /* For bad pixel handling, create a similar auxillary image that counts the bad pixels */
    
    assure( image != NULL, CPL_ERROR_NULL_INPUT, "Null image");
    assure( radius_x >= 0, CPL_ERROR_ILLEGAL_INPUT, "Negative x-radius (%d)", radius_x);
    assure( radius_y >= 0, CPL_ERROR_ILLEGAL_INPUT, "Negative y-radius (%d)", radius_y);
    assure( cpl_image_get_type(image) == CPL_TYPE_DOUBLE, CPL_ERROR_TYPE_MISMATCH,
        "Type is %s. double expected", uves_tostring_cpl_type(cpl_image_get_type(image)));
    
    nx = cpl_image_get_size_x(image);
    ny = cpl_image_get_size_y(image);
    image_data = cpl_image_get_data_double(image);
    
    /* (Disabled:) To avoid problems with overflow (the total flux in the image might
       be larger than INT_MAX) subtract a constant (the average flux), apply the filter,
       then add the constant       */
    
    /* First build auxillary image:
     *
     * aux(x,y) = sum_{i=0,x-1} sum_{j=0,y-1}  image(i,j)
     *          = sum of rectangle (0,0)-(x-1,y-1)
     *
     */

    aux = cpl_image_new(nx+1, ny+1, CPL_TYPE_DOUBLE);          /* Initialized to zero */
    aux_data = cpl_image_get_data(aux);

    /* Column x=0 and row y=0 are already zero and need not be calculated,
     * start from 1.    */

/* Slow:    for (x = 1; x < nx+1; x++)
            {
        for (y = 1; y < ny+1; y++)
        {
*/
    for (i = 0; i < (nx+1)*(ny+1); i++)
    {
        int x = i % (nx+1);
        int y = i / (nx+1);
        
        if ( x >= 1 && y >= 1)
        {
            aux_data[x + y*(nx+1)] = image_data[x-1 + (y-1) * nx]
            + aux_data  [x-1 +    y * (nx+1)]
            + aux_data  [x   + (y-1)* (nx+1)]
            - aux_data  [x-1 + (y-1)* (nx+1)];
        }
        
        /* Proof of induction step
         * (assume that formula holds up to (x-1,y) and (x,y-1) and prove formula for (x,y))
         *
         *  aux(x,y) = image(x-1, y-1) + aux(x-1, y) + aux(x, y-1) - aux(x-1, y-1)  (see code)
         *
         *  = image(x-1, y-1)
         *  + sum_{i=0,x-2}_{j=0,y-1} image(i,j)  _
         *  + sum_{i=0,x-1}_{j=0,y-2} image(i,j)   \_ sum_{j=0,y-2} image(x-1, j)  
         *  - sum_{i=0,x-2}_{j=0,y-2} image(i,j)  _/ 
         * 
         *  = sum_{i=0,x-2}_{j=0,y-1} image(i,j)
         *  + sum_          {j=0,y-1} image(x-1, j)  
         *  
         *  = sum_{j=0,y-1} [ ( sum_{i=0,x-2} image(i,j) ) + image(x-1,j) ]
         *  = sum_{j=0,y-1}     sum_{i=0,x-1} image(i,j)      q.e.d.
         *
         *  It's simpler when you draw it... 
         */
    }

    uves_msg_debug("Finished setting up auxillary image. Get average");

    /* Then calculate average = (flux in window) / (image size) */
    for (i = 0; i < nx*ny; i++)
    {
        int x = (i % nx);
        int y = (i / nx);

        int lower, upper;
        int left, right;
        
        lower = y - radius_y; if (lower <   0) lower = 0;
        upper = y + radius_y; if (upper >= ny) upper = ny - 1;
        
        left  = x - radius_x; if (left  <   0) left  = 0;
        right = x + radius_x; if (right >= nx) right = nx - 1;
        
        image_data[x + y*nx] =
        (
            aux_data[(right+1) + (upper+1)*(nx+1)] +
            aux_data[ left     +  lower   *(nx+1)] -
            aux_data[ left     + (upper+1)*(nx+1)] -
            aux_data[(right+1) +  lower   *(nx+1)]
            )
        /
        ( (double) (upper-lower+1) * (right-left+1) );
    }
    
  cleanup:
    uves_free_image(&aux);
    return cpl_error_get_code();
}


/*----------------------------------------------------------------------------*/
/**
   @brief   Median filter
   @param   image               Image to filter
   @param   xwindow             x-radius of filter window
   @param   ywindow             y-radius of filter window
   @param   extrapolate_border  See @c filter_median()

   @return  CPL_ERROR_NONE iff OK.

   If the median window is large, this function is rather inefficient,
   because @c cpl_image_get_median_window() is called for each pixel. A more clever
   implementation would keep track of the running window. 
**/
/*----------------------------------------------------------------------------*/
cpl_error_code
uves_filter_image_median(cpl_image **image, int xwindow, int ywindow, bool extrapolate_border)
{
    cpl_matrix *id = NULL;
    cpl_image *temp = NULL;

    assure( xwindow >= 0 && ywindow >= 0, CPL_ERROR_ILLEGAL_INPUT,
        "Illegal window radius: %d x %d", 
        (2*xwindow + 1),
        (2*ywindow + 1));
    
    UVES_TIME_START("median filter");

    if (xwindow <= 1 && ywindow <= 1)
/* CPL 3 supports   if (xwindow <= 4 && ywindow <= 4) */
    {
        check(( id = cpl_matrix_new(2*xwindow+1, 2*ywindow+1),
            cpl_matrix_fill(id, 1)), "Could not create kernel matrix");
        
        /* Image to cpl_image_filter_median must be float or double */
        if (cpl_image_get_type(*image) == CPL_TYPE_INT)
        {
            temp = cpl_image_cast(*image, CPL_TYPE_DOUBLE);
            uves_free_image(image);
        }
        else
        {
	  temp = cpl_image_duplicate(*image);
	  uves_free_image(image);
        }
        check( *image = uves_image_filter_median(temp, id), "Error applying median filter");
        uves_free_image(&temp);

        /* fixme: the CPL function marks border pixels as bad. Do something
           depending on the extrapolate_border flag */
    }
    else
    {
        temp = *image;
        check( *image = filter_median(temp, xwindow, ywindow, extrapolate_border),
           "Error applying median filter");
        uves_free_image(&temp);
    }

    UVES_TIME_END;    
    
  cleanup:
    uves_free_matrix(&id);
    uves_free_image(&temp);
    return cpl_error_get_code();
}

/** @cond See cpl_tools_get_kth_double */
#define DOUBLE_SWAP(a,b) { register double t=(a);(a)=(b);(b)=t; }
/** @endcond */

/**
     @brief returns the kth value of an array
     @param a  array
     @param n  array size
     @param k  component to evaluate
     
     The array is modified

*/
double uves_utils_get_kth_double(
        double  *   a, 
        int         n, 
        int         k)
{
    register double x ;
    register int    i, j, l, m ;

    l=0 ; m=n-1 ;
    while (l<m) {
        x=a[k] ;
        i=l ;
        j=m ;
        do {
            while (a[i]<x) i++ ;
            while (x<a[j]) j-- ;
            if (i<=j) {
                DOUBLE_SWAP(a[i],a[j]) ;
                i++ ; j-- ;
            }
        } while (i<=j) ;
        if (j<k) l=i ;
        if (k<i) m=j ;
    }
    return a[k] ;
}

/**
   @brief returns median (not CPL median) of an array
   @param a  array
   @param n  array size
   
   The array is modified

*/
double
uves_tools_get_median(double *a, int n)
{
    if (n % 2 == 0)
    {
        return
        (uves_utils_get_kth_double(a, n, n/2) +
         uves_utils_get_kth_double(a, n, n/2-1))/2.0;
        
    }
    else
    {
        return uves_utils_get_kth_double(a, n, (n-1)/2);
    }
}



/*----------------------------------------------------------------------------*/
/**
   @brief   Median filter
   @param   image   Image to filter
   @param   radx    x-radius of median window
   @param   rady    y-radius of median window
   @param   extrapolate_border  If false (which should be considered
                                the default value), a smaller window is used
                at the image borders (where the full window 
                would be partially outside of the image).
                If true, the median is calculated only for
                points where the full window is inside the
                image, and the border region is obtained
                by constant extrapolation.
   @return  Filtered image, or NULL on error.

   Apply a @em radx by @em rady median filter to the input image.
   Each pixel is replaced by the median of a local window of size (2* @em radx + 1)*
   (2 * @em rady + 1) centered on the pixel.

   
*/
/*----------------------------------------------------------------------------*/
static cpl_image *
filter_median(const cpl_image *image, int radx, int rady, bool extrapolate_border)
{
    int x, y;
    int nx = cpl_image_get_size_x(image);
    int ny = cpl_image_get_size_y(image);
    cpl_image *result = cpl_image_new(nx, ny, CPL_TYPE_DOUBLE);
    double *result_data;
    const double *image_data;
    double *window = NULL;

    window = cpl_malloc(sizeof(double) * (2*radx+1)*(2*rady+1));
    assure_mem( result );
    assure( cpl_image_get_type(image) == CPL_TYPE_DOUBLE,
            CPL_ERROR_UNSUPPORTED_MODE, "Type is %s", 
            uves_tostring_cpl_type(cpl_image_get_type(image)));

    result_data = cpl_image_get_data_double(result);
    image_data = cpl_image_get_data_double_const(image);

    for (y = 1; y <= ny; y++)
    {
        for (x = 1; x <= nx; x++)
        {
            int x1, y_1, x2, y2;
            
            x1 = x - radx; y_1 = y - rady;
            x2 = x + radx; y2  = y + rady;

            if (extrapolate_border)
            {
                /* At edge of image, move median box, so
                   that entire box is inside of image */
                if (x1 < 1)
                {
                    x2 += (1 - x1);
                    x1 += (1 - x1);
                }
                if (nx < x2)
                {
                    x1 -= (x2 - nx);
                    x2 -= (x2 - nx);
                }
                
                if (y_1 < 1)
                {
                    y2  += (1 - y_1);
                    y_1 += (1 - y_1);
                }
                if (ny < y2)
                {
                    y_1 -= (y2 - ny);
                    y2  -= (y2 - ny);
                }
            }
            else { /* Rely on the use of min/max below */ }
            
#if 0
            result_data[(x-1) + (y-1)*nx] = 
            cpl_image_get_median_window(image,  
                            uves_max_int(1,  x1),
                            uves_max_int(1,  y_1),
                            uves_min_int(nx, x2),
                            uves_min_int(ny, y2));

#else
            /* This saves a few (~10-20) percent execution time */
            {
            int i, j, k;
            
            k = 0;
            for (j  = uves_max_int(1 , y_1)-1;
                 j <= uves_min_int(ny, y2 )-1;
                 j++)
                for (i  = uves_max_int(1,  x1)-1; 
                 i <= uves_min_int(nx, x2)-1; 
                 i++)
                {
                    window[k++] = image_data[i + j*nx];
                }
            
            result_data[(x-1) + (y-1)*nx] = 
                uves_utils_get_kth_double(window,k,(((k)&1)?((k)/2):(((k)/2)-1))) ;
            }
#endif        
        }
    }
    

    assure( cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(), 
        "Error calculating %dx%d median filter", radx, rady);
    
  cleanup:
    cpl_free(window);
    return result;
}


/*----------------------------------------------------------------------------*/
/**
   @brief   Fit a 2d gaussian to an image sub-window
   @param   image       The image to fit.
   @param   noise       The uncertainty (one sigma, gaussian errors assumed) 
                        associated with the image.
                        If NULL, constant uncertainties are assumed, and 
            then @em dx0 and @em dy0 cannot be determined.
   @param   x1          Lower left corner (inclusive, FITS convention).
   @param   y_1         Lower left corner (inclusive, FITS convention).
   @param   x2          Upper right corner (inclusive, FITS convention).
   @param   y2          Upper right corner (inclusive, FITS convention).
   @param   x0          (output) x-center of best fit gaussian.
   @param   y_0          (output) y-center of best fit gaussian.
   @param   sigmax      (output) x-width of best fit gaussian. Positive on success.
   @param   sigmay      (output) y-width of best fit gaussian. Positive on success.
   @param   amplitude   (output) Height of the best fit gaussian, 
                        relative to the background level. May be NULL.
   @param   dx0         (output) Uncertainty of @em x0. May be NULL.
   @param   dy0         (output) Uncertainty of @em y_0. May be NULL.

   The function does not make a 2d gaussian fit, but fits a one-dimensional gaussian
   to the marginal distributions in x and y. See also @c uves_fit_gaussian_1d_image().

   @note A CPL_ERROR_CONTINUE is set in the case (and only in that case) that
   the fitting algorithm fails to converge or converges to a non-sensical result.

*/
/*----------------------------------------------------------------------------*/

cpl_error_code
uves_fit_gaussian_2d_image(const cpl_image *image, const cpl_image *noise,
               int x1, int y_1,
               int x2, int y2,
               double *x0, double *y_0, double *sigmax, double *sigmay,
               double *amplitude,
               double *dx0, double *dy0
    )
{
    cpl_image  *marginal_x = NULL;
    cpl_image  *marginal_y = NULL;
    cpl_image  *marginal_x_noise = NULL;
    cpl_image  *marginal_y_noise = NULL;
    cpl_image  *variance = NULL;
    cpl_matrix *covariance = NULL;

    int nx, ny;
    double norm_x, norm_y;
    double background_x, background_y;

    /* Check input */
    assure( image != NULL, CPL_ERROR_NULL_INPUT, "Null image");
    nx = cpl_image_get_size_x(image);
    ny = cpl_image_get_size_y(image);
    assure( noise != NULL || (dx0 == NULL && dy0 == NULL), CPL_ERROR_INCOMPATIBLE_INPUT,
        "Cannot compute uncertainty of fit with no noise image specified");
    assure( noise == NULL || 
        (cpl_image_get_size_x(noise) == nx &&
         cpl_image_get_size_y(noise) == ny),
        CPL_ERROR_INCOMPATIBLE_INPUT,
        "Size of input image (%dx%d) and noise image (%" CPL_SIZE_FORMAT "x%" CPL_SIZE_FORMAT ") differ", 
        nx, ny,
        cpl_image_get_size_x(noise),
        cpl_image_get_size_y(noise));
    assure( 1 <= x1 && x1 <= x2 && x2 <= nx &&
        1 <= y_1 && y_1 <= y2 && y2 <= ny, CPL_ERROR_ILLEGAL_INPUT,
        "Illegal window: (%d, %d)-(%d, %d)", x1, y_1, x2, y2);
    assure( x0 != NULL, CPL_ERROR_NULL_INPUT, "Null x-center");
    assure( y_0 != NULL, CPL_ERROR_NULL_INPUT, "Null y-center");
    assure( sigmax != NULL, CPL_ERROR_NULL_INPUT, "Null sigma_x");
    assure( sigmay != NULL, CPL_ERROR_NULL_INPUT, "Null sigma_y");
    /* amplitude, dx0, dy0 may be NULL */

    if (noise != NULL)
    {
        /* Variance = noise^2 */
        check(( variance = cpl_image_extract(noise, x1, y_1, x2, y2),
            cpl_image_power(variance, 2.0)),
           "Error creating variance image");
    }
    
    /* Collapse along columns (result is horizontal) */
    check( marginal_x = cpl_image_collapse_window_create(image,
                             x1, y_1, x2, y2,
                             0),           /* Sum of columns */
       "Error collapsing window (%d, %d) - (%d, %d)", x1, y_1, x2, y2);  

    if (noise != NULL)
    {
        /* Sigma of sum = sqrt [ sum sigma_i^2 ] */
        
        check( marginal_x_noise = cpl_image_collapse_window_create(variance,
                                       1, 1, 
                                       x2-x1+1, y2-y_1+1,
                                       0), /* Sum of columns */
           "Error collapsing window (1, 1) - (%d, %d)", x2-x1+1, y2-y_1+1);

        /* Sqrt */
        cpl_image_power(marginal_x_noise, 0.5);
    }
    
    /* Collapse along rows (result is vertical) */
    check( marginal_y = cpl_image_collapse_window_create(image,
                             x1, y_1, x2, y2,
                             1),           /* Sum of rows */
       "Error collapsing window (%d, %d) - (%d, %d)", x1, y_1, x2, y2);  
    
    if (noise != NULL)
    {
        check( marginal_y_noise = cpl_image_collapse_window_create(variance,
                                       1, 1,
                                       x2-x1+1, y2-y_1+1,
                                       1), /* Sum of rows */
           "Error collapsing window (1, 1) - (%d, %d)", x2-x1+1, y2-y_1+1);

        /* Sqrt */
        cpl_image_power(marginal_y_noise, 0.5);
    }

    /* Fit x-distribution */
    uves_fit_1d_image(marginal_x, marginal_x_noise, NULL,
              true,                       /* Horizontal ?                  */
              false, false,               /* Fix/fit background ?          */
              1, x2 - x1 + 1, 1,          /* xlo, xhi, y                   */
              x0, sigmax, &norm_x, &background_x, NULL,
              NULL, NULL,                 /* mse, red. chi^2               */
              (dx0 != NULL) ? &covariance : NULL,
              uves_gauss, uves_gauss_derivative, 4);

    /* Set code 'CPL_ERROR_CONTINUE' if fitting failed, check for unexpected errors */
    assure( cpl_error_get_code() != CPL_ERROR_CONTINUE ||
        cpl_error_get_code() != CPL_ERROR_SINGULAR_MATRIX,
        CPL_ERROR_CONTINUE, "Fitting along x failed");
    assure( cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(), 
        "Fitting along x failed");
     
    /* Map to world-coordinates */
    *x0 += (x1 - 1);
    
    if (dx0 != NULL)
    {
        *dx0 = cpl_matrix_get(covariance, 0, 0);
    }


    /* Fit y-distribution */
    uves_free_matrix(&covariance);
    uves_fit_1d_image(marginal_y, marginal_y_noise, NULL,
              false,                      /* Horizontal ?                  */
              false, false,               /* Fix/fit background ?          */
              1, y2 - y_1 + 1, 1,          /* ylo, yhi, x                   */
              y_0, sigmay, &norm_y, &background_y, NULL,
              NULL, NULL,                 /* mse, red. chi^2               */
              (dy0 != NULL) ? &covariance : NULL,
              uves_gauss, uves_gauss_derivative, 4);
    
    /* Set code 'CPL_ERROR_CONTINUE' if fitting failed, check for unexpected errors */
    assure( cpl_error_get_code() != CPL_ERROR_CONTINUE ||
        cpl_error_get_code() != CPL_ERROR_SINGULAR_MATRIX,
        CPL_ERROR_CONTINUE, "Fitting along y failed");
    assure( cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(), 
        "Fitting along y failed");
    
    /* Map to world-coordinates */
    *y_0 += (y_1 - 1);
    
    if (dy0 != NULL)
    {
        *dy0 = cpl_matrix_get(covariance, 0, 0);
    }
    
    /* Set amplitude  = N / [ sqrt(2pi sigmax^2) sqrt(2pi sigmay^2) ].
     *
     * The fitted norm (area), N, is the same (up to numerical errors) in both directions,
     * so use geometric average as an estimate of N.
     */
    if (amplitude != NULL)
    {
        *amplitude = sqrt(norm_x * norm_y) / (2*M_PI * (*sigmax) * (*sigmay));
    }
    
  cleanup:
    uves_free_matrix(&covariance);
    uves_free_image(&variance);
    uves_free_image(&marginal_x);
    uves_free_image(&marginal_x_noise);
    uves_free_image(&marginal_y);
    uves_free_image(&marginal_y_noise);
    
    return cpl_error_get_code();
}


/**@}*/