/*
 * This file is part of the ESO SINFONI Pipeline
 * Copyright (C) 2004,2005 European Southern Observatory
 *
 * This program 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
 */

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

   File name     :    sinfo_new_resampling.c
   Author         :    Nicolas Devillard
   Created on    :    Jan 04, 1996
   Description    :    resampling routines

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

/*
    $Id: sinfo_new_resampling.c,v 1.10 2012-03-03 09:50:08 amodigli Exp $
    $Author: amodigli $
    $Date: 2012-03-03 09:50:08 $
    $Revision: 1.10 $
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
/*---------------------------------------------------------------------------
                                  Includes
 ---------------------------------------------------------------------------*/
#include <math.h>
#include "sinfo_new_resampling.h"
#include "sinfo_pixel_handling.h"
#include "sinfo_globals.h"
/* #include "my_pi.h" */
#include "sinfo_resampling.h"
/*---------------------------------------------------------------------------
                              Private functions
 ---------------------------------------------------------------------------*/

static void new_reverse_tanh_kernel(double * data, int nn) ;
static double sinfo_new_sinc(double x);

/*---------------------------------------------------------------------------
                              Function codes
 ---------------------------------------------------------------------------*/

/**@{*/
/**
 * @defgroup sinfo_new_resampling Image resampling
 *
 * TBD
 */

/*-------------------------------------------------------------------------*/
/**
  @name        sinfo_new_generate_interpolation_kernel
  @memo        Generate an interpolation kernel to use in this module.
  @param    kernel_type        Type of interpolation kernel.
  @return    1 newly allocated array of doubles.
  @doc

  Provide the name of the kernel you want to generate. Supported kernel
  types are:

  \begin{tabular}{ll}
  NULL            &    default kernel, currently "tanh" \\
  "default"        &    default kernel, currently "tanh" \\
  "tanh"        &    Hyperbolic tangent \\
  "sinc2"        &    Square sinfo_new_sinc \\
  "lanczos"        &    Lanczos2 kernel \\
  "hamming"        &    Hamming kernel \\
  "hann"        &    Hann kernel
  \end{tabular}

  The returned array of doubles is ready of use in the various re-sampling
  functions in this module. It must be deallocated using cpl_free().
 */
/*--------------------------------------------------------------------------*/

double   *
sinfo_new_generate_interpolation_kernel(const char * kernel_type)
{
    double  *    tab ;
    int         i ;
    double      x ;
    double        alpha ;
    double        inv_norm ;
    int         samples = KERNEL_SAMPLES ;

    if (kernel_type==NULL) {
        tab = sinfo_new_generate_interpolation_kernel("tanh") ;
    } else if (!strcmp(kernel_type, "default")) {
        tab = sinfo_new_generate_interpolation_kernel("tanh") ;
    } else if (!strcmp(kernel_type, "sinfo_new_sinc")) {
        tab = cpl_malloc(samples * sizeof(double)) ;
        tab[0] = 1.0 ;
        tab[samples-1] = 0.0 ;
        for (i=1 ; i<samples ; i++) {
            x = (double)KERNEL_WIDTH * (double)i/(double)(samples-1) ;
            tab[i] = sinfo_new_sinc(x) ;
        }
    } else if (!strcmp(kernel_type, "sinc2")) {
        tab = cpl_malloc(samples * sizeof(double)) ;
        tab[0] = 1.0 ;
        tab[samples-1] = 0.0 ;
        for (i=1 ; i<samples ; i++) {
            x = 2.0 * (double)i/(double)(samples-1) ;
            tab[i] = sinfo_new_sinc(x) ;
            tab[i] *= tab[i] ;
        }
    } else if (!strcmp(kernel_type, "lanczos")) {
        tab = cpl_malloc(samples * sizeof(double)) ;
        for (i=0 ; i<samples ; i++) {
            x = (double)KERNEL_WIDTH * (double)i/(double)(samples-1) ;
            if (fabs(x)<2) {
                tab[i] = sinfo_new_sinc(x) * sinfo_new_sinc(x/2) ;
            } else {
                tab[i] = 0.00 ;
            }
        }
    } else if (!strcmp(kernel_type, "hamming")) {
        tab = cpl_malloc(samples * sizeof(double)) ;
        alpha = 0.54 ;
        inv_norm  = 1.00 / (double)(samples - 1) ;
        for (i=0 ; i<samples ; i++) {
            x = (double)i ;
            if (i<(samples-1)/2) {
                tab[i] = alpha + (1-alpha) * cos(2.0*PI_NUMB*x*inv_norm) ;
            } else {
                tab[i] = 0.0 ;
            }
        }
    } else if (!strcmp(kernel_type, "hann")) {
        tab = cpl_malloc(samples * sizeof(double)) ;
        alpha = 0.50 ;
        inv_norm  = 1.00 / (double)(samples - 1) ;
        for (i=0 ; i<samples ; i++) {
            x = (double)i ;
            if (i<(samples-1)/2) {
                tab[i] = alpha + (1-alpha) * cos(2.0*PI_NUMB*x*inv_norm) ;
            } else {
                tab[i] = 0.0 ;
            }
        }
    } else if (!strcmp(kernel_type, "tanh")) {
        tab = sinfo_new_generate_tanh_kernel(TANH_STEEPNESS) ;
    } else {
        sinfo_msg_error("unrecognized kernel type [%s]: aborting generation",
                        kernel_type) ;
        return NULL ;
    }

    return tab ;
}

/*-------------------------------------------------------------------------*/
/**
  @name        sinfo_new_sinc
  @memo        Cardinal sine.
  @param    x    double value.
  @return    1 double.
  @doc

  Compute the value of the function sinfo_sinc(x)=sin(pi*x)/(pi*x) at the
  requested x.
 */
/*--------------------------------------------------------------------------*/

double
sinfo_new_sinc(double x)
{
    if (fabs(x)<1e-4)
        return (double)1.00 ;
    else
        return ((sin(x * (double)PI_NUMB)) / (x * (double)PI_NUMB)) ;
}



/*-------------------------------------------------------------------------*/
/**
  @name        sinfo_new_warp_image_generic
  @memo        Warp an image according to a polynomial transformation.
  @param    image_in        Image to warp.
  @param    kernel_type        Interpolation kernel to use.
  @param    poly_u            Polynomial transform in U.
  @param    poly_v            Polynomial transform in V.
  @return    1 newly allocated image.
  @doc

  Warp an image according to a polynomial transform. Provide two
  polynomials (see poly2d.h for polynomials in this library) Pu and Pv such
  as:

  \begin{verbatim}
  x = sinfo_poly2d_compute(Pu, u, v)
  y = sinfo_poly2d_compute(Pv, u, v)
  \end{verbatim}

  Attention! The polynomials define a reverse transform. (u,v) are
  coordinates in the warped image and (x,y) are coordinates in the original
  image. The transform you provide is used to compute from the warped
  image, which pixels contributed in the original image.

  The output image will have strictly the same size as in the input image.
  Beware that for extreme transformations, this might lead to blank images
  as result.

  See the function sinfo_generate_interpolation_kernel() for possible kernel
  types. If you want to use a default kernel, provide NULL for kernel type.

  The returned image is a newly allocated objet, use cpl_image_delete() to
  deallocate it.

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

cpl_image *
sinfo_new_warp_image_generic(
                cpl_image         *    image_in,
                char          *    kernel_type,
                cpl_polynomial    *    poly_u,
                cpl_polynomial    *    poly_v
)
{
    cpl_image    *    image_out ;
    int             i, j, k ;
    int             lx_out, ly_out ;
    double           cur ;
    double           neighbors[16] ;
    double           rsc[8],
    sumrs ;
    double           x, y ;
    int             px, py ;
    int             pos ;
    int             tabx, taby ;
    double      *    kernel ;
    int                  leaps[16] ;
    int ilx=0;
    int ily=0;
    float* pidata=NULL;
    float* podata=NULL;
    cpl_vector* vx=NULL;
    if (image_in == NULL) return NULL ;


    /* Generate default interpolation kernel */
    kernel = sinfo_new_generate_interpolation_kernel(kernel_type) ;
    if (kernel == NULL) {
        sinfo_msg_error("cannot generate kernel: aborting resampling") ;
        return NULL ;
    }
    ilx=cpl_image_get_size_x(image_in);
    ily=cpl_image_get_size_y(image_in);
    pidata=cpl_image_get_data_float(image_in);

    /* Compute new image size   */
    lx_out = (int)ilx ;
    ly_out = (int)ily ;

    image_out = cpl_image_new(lx_out, ly_out,CPL_TYPE_FLOAT) ;
    podata=cpl_image_get_data_float(image_out);

    /* Pre compute leaps for 16 closest neighbors positions */

    leaps[0] = -1 - ilx ;
    leaps[1] =    - ilx ;
    leaps[2] =  1 - ilx ;
    leaps[3] =  2 - ilx ;

    leaps[4] = -1 ;
    leaps[5] =  0 ;
    leaps[6] =  1 ;
    leaps[7] =  2 ;

    leaps[8] = -1 + ilx ;
    leaps[9] =      ilx ;
    leaps[10]=  1 + ilx ;
    leaps[11]=  2 + ilx ;

    leaps[12]= -1 + 2*ilx ;
    leaps[13]=      2*ilx ;
    leaps[14]=  1 + 2*ilx ;
    leaps[15]=  2 + 2*ilx ;

    vx=cpl_vector_new(2); /* vector of size 2 = polynomial dimension */
    /* Double loop on the output image  */
    for (j=0 ; j < ly_out ; j++) {
        for (i=0 ; i< lx_out ; i++) {
            /* Compute the original source for this pixel   */
            cpl_vector_set(vx,0,(double)i);
            cpl_vector_set(vx,1,(double)j);
            x = cpl_polynomial_eval(poly_u, vx);
            y = cpl_polynomial_eval(poly_v, vx);

            /* Which is the closest integer positioned neighbor?    */
            px = (int)x ;
            py = (int)y ;

            if ((px < 1) ||
                            (px > (ilx-3)) ||
                            (py < 1) ||
                            (py > (ily-3)))
                podata[i+j*lx_out] = (pixelvalue)0.0/0.0 ;
            else {
                /* Now feed the positions for the closest 16 neighbors  */
                pos = px + py * ilx ;
                for (k=0 ; k<16 ; k++)
                    neighbors[k] = (double)(pidata[(int)(pos+leaps[k])]) ;

                /* Which tabulated value index shall we use?    */
                tabx = (x - (double)px) * (double)(TABSPERPIX) ; 
                taby = (y - (double)py) * (double)(TABSPERPIX) ; 

                /* Compute resampling coefficients  */
                /* rsc[0..3] in x, rsc[4..7] in y   */

                rsc[0] = kernel[TABSPERPIX + tabx] ;
                rsc[1] = kernel[tabx] ;
                rsc[2] = kernel[TABSPERPIX - tabx] ;
                rsc[3] = kernel[2 * TABSPERPIX - tabx] ;
                rsc[4] = kernel[TABSPERPIX + taby] ;
                rsc[5] = kernel[taby] ;
                rsc[6] = kernel[TABSPERPIX - taby] ;
                rsc[7] = kernel[2 * TABSPERPIX - taby] ;

                sumrs = (rsc[0]+rsc[1]+rsc[2]+rsc[3]) *
                                (rsc[4]+rsc[5]+rsc[6]+rsc[7]) ;

                /* Compute interpolated pixel now   */
                cur =   rsc[4] * (  rsc[0]*neighbors[0] +
                                rsc[1]*neighbors[1] +
                                rsc[2]*neighbors[2] +
                                rsc[3]*neighbors[3] ) +
                                rsc[5] * (  rsc[0]*neighbors[4] +
                                                rsc[1]*neighbors[5] +
                                                rsc[2]*neighbors[6] +
                                                rsc[3]*neighbors[7] ) +
                                                rsc[6] * (  rsc[0]*neighbors[8] +
                                                                rsc[1]*neighbors[9] +
                                                                rsc[2]*neighbors[10] +
                                                                rsc[3]*neighbors[11] ) +
                                                                rsc[7] * (  rsc[0]*neighbors[12] +
                                                                                rsc[1]*neighbors[13] +
                                                                                rsc[2]*neighbors[14] +
                                                                                rsc[3]*neighbors[15] ) ;

                /* Affect the value to the output image */
                podata[i+j*lx_out] = (pixelvalue)(cur/sumrs) ;
                /* done ! */
            }       
        }
    }
    cpl_vector_delete(vx);
    cpl_free(kernel) ;
    return image_out ;
}


/*-------------------------------------------------------------------------*/
/**
  @name        sinfo_new_generate_tanh_kernel
  @memo        Generate a hyperbolic tangent kernel.
  @param    steep    Steepness of the hyperbolic tangent parts.
  @return    1 pointer to a newly allocated array of doubles.
  @doc

  The following function builds up a good approximation of a box filter. It
  is built from a product of hyperbolic tangents. It has the following
  properties:

  \begin{itemize}
  \item It converges very quickly towards +/- 1.
  \item The converging transition is very sharp.
  \item It is infinitely differentiable everywhere (i.e. smooth).
  \item The transition sharpness is scalable.
  \end{itemize}

  The returned array must be deallocated using cpl_free().
 */
/*--------------------------------------------------------------------------*/

#define hk_gen(x,s) (((tanh(s*(x+0.5))+1)/2)*((tanh(s*(-x+0.5))+1)/2))

double * sinfo_new_generate_tanh_kernel(double steep)
{
    double  *   kernel ;
    double  *   x ;
    double      width ;
    double      inv_np ;
    double      ind ;
    int         i ;
    int         np ;
    int         samples ;

    width   = (double)TABSPERPIX / 2.0 ; 
    samples = KERNEL_SAMPLES ;
    np      = 32768 ; /* Hardcoded: should never be changed */
    inv_np  = 1.00 / (double)np ;

    /*
     * Generate the kernel expression in Fourier space
     * with a correct frequency ordering to allow standard FT
     */
    x = cpl_malloc((2*np+1)*sizeof(double)) ;
    for (i=0 ; i<np/2 ; i++) {
        ind      = (double)i * 2.0 * width * inv_np ;
        x[2*i]   = hk_gen(ind, steep) ;
        x[2*i+1] = 0.00 ;
    }
    for (i=np/2 ; i<np ; i++) {
        ind      = (double)(i-np) * 2.0 * width * inv_np ;
        x[2*i]   = hk_gen(ind, steep) ;
        x[2*i+1] = 0.00 ;
    }

    /* 
     * Reverse Fourier to come back to image space
     */
    new_reverse_tanh_kernel(x, np) ;

    /*
     * Allocate and fill in returned array
     */
    kernel = cpl_malloc(samples * sizeof(double)) ;
    for (i=0 ; i<samples ; i++) {
        kernel[i] = 2.0 * width * x[2*i] * inv_np ;
    }
    cpl_free(x) ;
    return kernel ;
}


/*-------------------------------------------------------------------------*/
/**
  @name        new_reverse_tanh_kernel
  @memo        Bring a hyperbolic tangent kernel from Fourier to normal space.
  @param    data    Kernel samples in Fourier space.
  @param    nn        Number of samples in the input kernel.
  @return    void
  @doc

  Bring back a hyperbolic tangent kernel from Fourier to normal space. Do
  not try to understand the implementation and DO NOT MODIFY THIS FUNCTION.
 */
/*--------------------------------------------------------------------------*/

#define KERNEL_SW(a,b) tempr=(a);(a)=(b);(b)=tempr
static void new_reverse_tanh_kernel(double * data, int nn)
{
    unsigned long   n,
    mmax,
    m,
    i, j;



    double  tempr,
    tempi;

    n = (unsigned long)nn << 1;
    j = 1;
    for (i=1 ; i<n ; i+=2) {
        if (j > i) {
            KERNEL_SW(data[j-1],data[i-1]);
            KERNEL_SW(data[j],data[i]);
        }
        m = n >> 1;
        while (m>=2 && j>m) {
            j -= m;
            m >>= 1;
        }
        j += m;
    }
    mmax = 2;
    while (n > mmax) {
        unsigned long istep = mmax << 1;
        double theta = 2 * PI_NUMB / mmax;
        double wtemp = sin(0.5 * theta);
        double wpr = -2.0 * wtemp * wtemp;
        double wpi = sin(theta);
        double wr  = 1.0;
        double wi  = 0.0;
        for (m=1 ; m<mmax ; m+=2) {
            for (i=m ; i<=n ; i+=istep) {
                j = i + mmax;
                tempr = wr * data[j-1] - wi * data[j];
                tempi = wr * data[j]   + wi * data[j-1];
                data[j-1] = data[i-1] - tempr;
                data[j]   = data[i]   - tempi;
                data[i-1] += tempr;
                data[i]   += tempi;
            }
            wr = (wtemp = wr) * wpr - wi * wpi + wr;
            wi = wi * wpr + wtemp * wpi + wi;
        }
        mmax = istep;
    }
}
#undef KERNEL_SW



/*-------------------------------------------------------------------------*/
/**
  @name        sinfo_new_show_interpolation_kernel
  @memo        Print out an interpolation kernel values on stdout.
  @param    kernel_name        Name of the kernel to print out.
  @return    void
  @doc

  Takes in input a kernel name, generates the corresponding kernel and
  prints it out on stdout, then discards the generated kernel.

  For debugging purposes mostly.
 */
/*--------------------------------------------------------------------------*/

void sinfo_new_show_interpolation_kernel(char * kernel_name)
{
    double    *    ker ;
    int            i ;
    double        x ;


    ker = sinfo_new_generate_interpolation_kernel(kernel_name) ;
    if (ker == NULL)
        return ;

    (void)fprintf(stdout, "# Kernel is %s\n", kernel_name) ;
    x = 0.00 ;
    for (i=0 ; i<KERNEL_SAMPLES ; i++) {
        (void)fprintf(stdout, "%g %g\n", x, ker[i]) ;
        x += 1.00 / (double)TABSPERPIX ;
    }
    cpl_free(ker) ;
    return ;
}
/**@}*/

