/* $Id: resampling.c,v 1.1.1.1 2001/08/27 11:27:25 rpalsa Exp $
 * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 * COPYRIGHT (c) 2001 European Southern Observatory
 * LICENSE: GNU General Public License version 2 or later
 *
 * PROJECT:   VLT Data Flow System
 * AUTHOR:    Ralf Palsa -- ESO/DMD/DPG
 * SUBSYSTEM: Instrument pipelines
 *
 * PURPOSE:
 * DESCRIPTION:
 *   The contents of this file has been extracted from the eclipse library
 *   and modified. 
 *
 *   The original file was resampling.c (Revision 1.19, 2001/07/20) by
 *   N. Devillard.
 *
 * $Name: fsmosaic-1_0 $
 * $Revision: 1.1.1.1 $
 * ----------------------------------------------------------------------------
 */

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

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>

#include "e_error.h"
#include "resampling.h"


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


/**
 * @memo
 *   Cardinal sine.
 *
 * @return 1 double.
 *
 * @param x  double value.
 *
 * @doc
 *   Compute the value of the function sinc(x)=sin(pi*x)/(pi*x) at the
 *   requested x.
 *
 * @author N. Devillard
 */

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


#define KERNEL_SW(a,b) tempr=(a);(a)=(b);(b)=tempr

/**
 * @memo
 *   Bring a hyperbolic tangent kernel from Fourier to normal space.
 *
 * @return void
 *
 * @param data  Kernel samples in Fourier space.
 * @param nn    Number of samples in the input kernel.
 *
 * @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.
 *
 * @author N. Devillard
 */

static void reverse_tanh_kernel(double * data, int nn)
{

    unsigned long n, mmax, m, i, j, istep;

    double wtemp, wr, wpr, wpi, wi, theta;
    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) {
        istep = mmax << 1;
        theta = 2 * PI_NUMB / mmax;
        wtemp = sin(0.5 * theta);
        wpr = -2.0 * wtemp * wtemp;
        wpi = sin(theta);
        wr  = 1.0;
        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


/**
 * @memo
 *   Generate a hyperbolic tangent kernel.
 *
 * @return 1 pointer to a newly allocated array of doubles.
 *
 * @param steep  Steepness of the hyperbolic tangent parts.
 *
 * @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 free().
 *
 * @author N. Devillard
 */

double * 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 = 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
     */

    reverse_tanh_kernel(x, np);


    /*
     * Allocate and fill in returned array
     */

    kernel = malloc(samples * sizeof(double));
    for (i=0 ; i<samples ; i++) {
        kernel[i] = 2.0 * width * x[2*i] * inv_np;
    }
    free(x);
    return kernel;
}


/**
 * @memo
 *   Generate an interpolation kernel to use in this module.
 *
 * @return 1 newly allocated array of doubles.
 *
 * @param kernel_type  Type of interpolation kernel.
 *
 * @doc
 *   Provide the name of the kernel you want to generate. Supported kernel
 *   types are:
 *   \begin{description}
 *     \item[#NULL#]    default kernel, currently "tanh" 
 *     \item[#default#] default kernel, currently "tanh"
 *     \item[#tanh#]    Hyperbolic tangent
 *     \item[#sinc2#]   Square sinc
 *     \item[#lanczos#] Lanczos2 kernel
 *     \item[#hamming#] Hamming kernel
 *     \item[#hann#]    Hann kernel
 *   \end{description}
 *
 *   The returned array of doubles is ready of use in the various re-sampling
 *   functions in this module. It must be deallocated using free().
 *
 * @author N. Devillard
 */

double *generate_interpolation_kernel(char *kernel_type)
{
    double *tab;
    double x;
    double alpha;
    double inv_norm;

    int i;
    int samples = KERNEL_SAMPLES;

    if (kernel_type==NULL) {
	tab = generate_interpolation_kernel("tanh");
    } else if (!strcmp(kernel_type, "default")) {
	tab = generate_interpolation_kernel("tanh");
    } else if (!strcmp(kernel_type, "sinc")) {
	tab = 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] = sinc(x);
	}
    } else if (!strcmp(kernel_type, "sinc2")) {
	tab = 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] = sinc(x);
	    tab[i] *= tab[i];
	}
    } else if (!strcmp(kernel_type, "lanczos")) {
	tab = 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] = sinc(x) * sinc(x/2);
	    } else {
		tab[i] = 0.00;
	    }
	}
    } else if (!strcmp(kernel_type, "hamming")) {
	tab = 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 = 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 = generate_tanh_kernel(TANH_STEEPNESS);
    } else {
	e_error("unrecognized kernel type [%s]: aborting generation",
		kernel_type);
	return NULL ;
    }

    return tab;
}


/**
 * @memo
 *   Invert a linear transformation.
 *
 * @return 1 newly allocated array of 6 doubles.
 *
 * @param trans  Transformation to invert.
 *
 * @doc
 *   Given 6 parameters a, b, c, d, e, f defining a linear transform such as:
 *   \begin{verbatim}
 *     u = ax + by + c
 *     v = dx + ey + f
 *   \end{verbatim}
 *
 *   The inverse transform is also linear, and is defined by:
 *   \begin{verbatim}
 *     x = Au + Bv + C
 *     y = Du + Ev + F
 *   \end{verbatim}
 *
 *   where:
 *   \begin{verbatim}
 *     if G = (ae-bd)
 *       A =  e/G
 *       B = -b/G
 *       C = (bf-ce)/G
 *       D = -d/G
 *       E =  a/G
 *       F = (cd-af)/G
 *   \end{verbatim}
 *
 *   Notice that if G=0 (ae=bd) the transform cannot be reversed.
 *
 *   The returned array must be deallocated using free().
 *
 * @author N. Devillard
 */

double *invert_linear_transform(double *trans)
{
    double *i_trans;
    double det;

    if (trans==NULL) return NULL;
    det = (trans[0] * trans[4]) - (trans[1] * trans[3]);
    if (fabs(det) < 1e-6) {
        e_error("NULL determinant: cannot invert transform") ;
        return NULL;
    }

    i_trans = calloc(6, sizeof(double));

    i_trans[0] =  trans[4] / det; 
    i_trans[1] = -trans[1] / det;
    i_trans[2] = ((trans[1] * trans[5]) - (trans[2] * trans[4])) / det;
    i_trans[3] = -trans[3] / det;
    i_trans[4] =  trans[0] / det;
    i_trans[5] = ((trans[2] * trans[3]) - (trans[0] * trans[5])) / det;

    return i_trans;
}


/**
 * @memo
 *   Rotate and shift an image according to a linear transformation.
 *
 * @return The function returns a pointer to the transformed image if no
 *   error occurred, otherwise a #NULL# pointer is returned.
 *
 * @param image_out    Transformed image.
 * @param image_in     Image to be rotated and shifted.
 * @param param        Linear transformation definition.
 * @param kernel_type  Interpolation kernel to use.
 *
 * @doc
 *   Warp an image according to a linear transformation. The transform is
 *   given as a set of 6 doubles, such as:
 *   \begin{verbatim}
 *     u = t[0].x + t[1].y + t[2]
 *     v = t[3].x + t[4].y + t[5]
 *   \end{verbatim}
 *
 *   Where (u,v) are the coordinates of a pixel in the warped image, and (x,y)
 *   are the coordinates of a pixel in the original image.
 *   The transformation must be invertible for this function to work. The
 *   warping algorithm is implemented as a reverse warping.
 *
 *   See the function generate_interpolation_kernel() for possible kernel
 *   types. If you want to use a default kernel, provide NULL for kernel type.
 *
 * @author N. Devillard, R. Palsa
 */

image_t *image_transform(image_t *image_out, image_t *image_in, double *param,
                         char *kernel_type)
{

    int i, j, k;
    int px, py;
    int pos;
    int tabx, taby;
    int leaps[16];
    int lx_out = image_out->hsize;
    int ly_out = image_out->vsize;

    double cur;
    double *invert_transform;
    double neighbors[16];
    double rsc[8], sumrs;
    double x, y;
    double *kernel;


    assert(image_in != NULL);
    assert(param != NULL);

    invert_transform = invert_linear_transform(param);
    if (invert_transform == NULL) {
        e_error("cannot compute invert transform: aborting warping");
        return NULL;
    }


    /*
     * Generate default interpolation kernel
     */

    kernel = generate_interpolation_kernel(kernel_type);
    if (kernel == NULL) {
        e_error("cannot generate kernel: aborting resampling");
        return NULL;
    }


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

    leaps[0] = -1 - image_in->hsize;
    leaps[1] =    - image_in->hsize;
    leaps[2] =  1 - image_in->hsize;
    leaps[3] =  2 - image_in->hsize;

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

    leaps[8] = -1 + image_in->hsize;
    leaps[9] =      image_in->hsize;
    leaps[10]=  1 + image_in->hsize;
    leaps[11]=  2 + image_in->hsize;

    leaps[12]= -1 + 2*image_in->hsize;
    leaps[13]=      2*image_in->hsize;
    leaps[14]=  1 + 2*image_in->hsize;
    leaps[15]=  2 + 2*image_in->hsize;

    /* 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   */

            x = invert_transform[0] * (double)i +
		invert_transform[1] * (double)j + invert_transform[2]; 

            y = invert_transform[3] * (double)i +
		invert_transform[4] * (double)j + invert_transform[5]; 

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

            if ((px < 1) || (px > (image_in->hsize-3)) ||
                (py < 1) || (py > (image_in->vsize-3)))
                image_out->data[i+j*lx_out] = (pixel_t)0.0;
            else {
                /* Now feed the positions for the closest 16 neighbors  */
                pos = px + py * image_in->hsize;
                for (k=0 ; k<16 ; k++)
                    neighbors[k] = (double)(image_in->data[(int)(pos + 
								 leaps[k])]);

                /* Which tabulated value index shall we use?    */
                tabx = (int)((x - (double)px) * (double)(TABSPERPIX)); 
                taby = (int)((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 */
                image_out->data[i+j*lx_out] = (pixel_t)(cur/sumrs);
                /* done ! */
            }       
        }
    }
    free(kernel);
    free(invert_transform);
    return image_out;
}