/*
 * 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
 */
/*************************************************************************
* E.S.O. - VLT project
*
*
*
* who       when      what
* --------  --------  ----------------------------------------------
* schreib  05/03/03  created
*/

/************************************************************************
*   NAME
*        sinfo_shift_images.c -
*        some procedures to shift images in spectral direction
*
*   SYNOPSIS
*   #include "sinfo_shift_images.h"
*
*   1) double sinfo_new_determine_shift_by_correlation ( cpl_image * refImage, 
*                                           cpl_image * shiftedImage )
*   2) cpl_image * sinfo_new_shift_image_in_spec ( cpl_image * shiftedImage, 
                                                   double shift, 
                                                   double * sub_shift )
*   3) cpl_image * 
       sinfo_new_fine_shift_image_in_spec_poly(cpl_image * shiftedImage, 
                                                           double sub_shift, 
                                                           int order )
*   4) cpl_image * 
       sinfo_new_fine_shift_image_in_spec_cubicspline(cpl_image * shiftedImage, 
                                                      double sub_shift )
*   5) cpl_imagelist * sinfo_align_cube_to_reference(cpl_imagelist * cube, 
*                                       cpl_image * refIm,
*                                       int order,
*                                       int shift_indicator )
*
*
*   DESCRIPTION
*
*   1) determines the sub-pixel shift of to emission line
*      frames by cross sinfo_correlation and fitting the sinfo_correlation
*      function by a Gaussian
*   2) shifts an image by a given amount to integer pixel accuracy
*   3) shifts an image by a given amount to sub-pixel accuracy
*   4) shifts an image by a given amount to sub-pixel accuracy
*   5) shifts images stacked in a cube by a given amount to sub-pixel accuracy
*
*   FILES
*
*   ENVIRONMENT
*
*   RETURN VALUES
*
*   CAUTIONS
*
*   EXAMPLES
*
*   SEE ALSO
*
*   BUGS
*
*------------------------------------------------------------------------
*/

/*
 * System Headers
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
#include "sinfo_vltPort.h"
#define POSIX_SOURCE 1
/*
 * Local Headers
 */
#include "sinfo_function_1d.h"
#include "sinfo_shift_images.h"
#include "sinfo_new_resampling.h"
#include "sinfo_globals.h"


static int filecounter ;
/**@{*/
/**
 * @defgroup sinfo_shift_images Functions to shift images
 *
 * TBD
 */

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

/**
 @brief determines the sub-pixel shift of to emission line frames by 
        cross sinfo_correlation and fitting the correlation function 
        by a Gaussian
 @name sinfo_new_determine_shift_by_correlation()
 @param refImage: reference image
 @param shiftedImage: image shifted in spectral direction
                      with respect to the reference image
 @return shift in sub-pixel accuracy 
 */

double sinfo_new_determine_shift_by_correlation ( cpl_image * refImage, 
                                     cpl_image * shiftedImage )
{
    int          col, row ;
    int           i, j, k, width;
    unsigned long convsize ;
    float       * lineref ;
    float       * line ;
    float       * offset2 ;
    double      * result ;
    double       mean_offset2 ;
    /*int          magFactor ;*/
    int          maxlag ;
    float      * refres ;
    float      * myres ;
    int          halfsearch ;
    int          delta ;
    double       xcorr_max ;
    /*float        arg ;*/
    float      par[MAXPAR] ;
    float      derv_par[MAXPAR] ;
    int        iters, xdim, ndat ;
    int        numpar, its ;
    int        * mpar ;
    float      tol, lab ;
    float      * xdat, * wdat ;
    Vector     * peak;
    char       filename[FILE_NAME_SZ] ;
    FILE       * fp ;

    int rlx=0;
    int rly=0;
    int slx=0;
    int sly=0;
    float* prdata=NULL;
    float* psdata=NULL;

    if ( NULL == refImage || NULL == shiftedImage )
    {
        sinfo_msg_error("image not given!") ;
        return ZERO ;
    }
    rlx=cpl_image_get_size_x(refImage);
    rly=cpl_image_get_size_x(refImage);
    prdata=cpl_image_get_data_float(refImage);
    slx=cpl_image_get_size_x(shiftedImage);
    sly=cpl_image_get_size_x(shiftedImage);
    psdata=cpl_image_get_data_float(shiftedImage);

    if ( rlx != slx || rly != sly )
    {
        sinfo_msg_error("image size not compatible!") ;
        return ZERO ;
    }
    snprintf(filename, MAX_NAME_SIZE-1,"offset%d.list", filecounter) ;

    fp = fopen(filename, "w") ;

    convsize = sly;

    lineref = (float*) cpl_calloc(convsize, sizeof(float) ) ;
    line = (float*) cpl_calloc(convsize, sizeof(float) ) ;

    offset2 = (float*) cpl_calloc(slx, sizeof(float) ) ;

    for ( col = 0 ; col < slx ; col++ )
    {
        /* initialize arrays */
        for ( row = 0 ;  row < (int) convsize ; row++ )
        {
            lineref[row] = 0. ;
            line[row] = 0. ;
        }

        /* magnify spectral sinfo_vector by magFactor */
        for ( row = 0 ; row < (sly) ; row++ )
        {
      lineref[row] = prdata[col+row*slx] ;   /* AM: why slx? */
      line[row] = psdata[col+row*slx] ;
        }

        myres = sinfo_function1d_filter_lowpass(line, convsize, 
                                                LOW_PASS_GAUSSIAN, 3) ;
        refres = sinfo_function1d_filter_lowpass(lineref, convsize, 
                                                 LOW_PASS_GAUSSIAN, 4) ;

        /*  now do a cross correlaton of both convolved spectral vectors */
        halfsearch = convsize / 2 ;
        result = sinfo_new_xcorrel( myres, convsize, refres, convsize, 
                                    halfsearch, &delta, &maxlag, &xcorr_max ) ;

        if ( xcorr_max < 0. )
        {
            sinfo_function1d_del ( refres ) ;
            sinfo_function1d_del ( myres ) ;
            cpl_free (result) ; 
            continue ;
        }

        /* in this section, we fit the sinfo_correlation function with a 
           gauss, and find its peak, th
           us getting subpixel-accuracy */

        i = maxlag; j = i+1;
        while (result[j] < result[i]) 
        {
            i++; j++;
        }
        i = maxlag; k = i-1;
        while (result[k] < result[i]) 
        {
            i--; k--;
        }
        width = j-k+1;
        /* allocate memory for the spectral sinfo_vector */
        if ( NULL == (peak = sinfo_new_vector (width)) )
        {
            sinfo_msg_error ("cannot allocate new Vector ") ;
            fclose(fp);
            return ZERO ;
        }


        /* allocate memory */
        xdat = (float *) cpl_calloc( peak -> n_elements, sizeof (float) ) ;
        wdat = (float *) cpl_calloc( peak -> n_elements, sizeof (float) ) ;
        mpar = (int *)   cpl_calloc( MAXPAR, sizeof (int) ) ;

        /* determine the values of the spectral sinfo_vector given as input */
        /* go through the chosen column */

        for ( i = 0 ; i < width ; i++ )
        {
            peak -> data[i] = result[k+i] ;
            xdat[i] = i;
            wdat[i] = 1.0;
        }

        /* set initial values for the fitting routine */
        xdim     = XDIM;
        ndat     = peak -> n_elements ;
        numpar   = MAXPAR ;
        tol      = TOL ;
        lab      = LAB ;
        its      = ITS ;
        par[1] = width/2.0 ;
        par[2] = (float) (maxlag - k) ;
        par[3] = (peak -> data[0] + peak -> data[peak->n_elements - 1]) / 2.0 ;
        par[0]  = result[maxlag] - (par[3]) ;

        for ( i = 0 ; i < MAXPAR ; i++ )
        {
            derv_par[i] = 0.0 ;
            mpar[i] = 1 ;
        }

        /* finally, do the least square fit using a sinfo_gaussian */
        if ( 0 > ( iters = sinfo_new_lsqfit_c( xdat, &xdim, peak -> data, wdat, 
                                         &ndat, par, derv_par, mpar,
                                         &numpar, &tol, &its, &lab )) )
        {
            sinfo_msg_warning ("sinfo_new_lsqfit_c: least squares fit failed "
                               "in col: %d, error no.: %d", col, iters) ;
            sinfo_new_destroy_vector ( peak ) ;
            cpl_free ( xdat ) ;
            cpl_free ( wdat ) ;
            cpl_free ( mpar ) ;
            sinfo_function1d_del ( refres ) ;
            sinfo_function1d_del ( myres ) ;
            cpl_free (result) ; 
            continue ;
        }

        sinfo_new_destroy_vector ( peak ) ;
        cpl_free (xdat) ;
        cpl_free (wdat) ;
        cpl_free (mpar) ;
        sinfo_function1d_del ( refres ) ;
        sinfo_function1d_del ( myres ) ;
        cpl_free (result) ; 
 
        offset2[col] = (float)( k+par[2] - sly/2) ;
        fprintf(fp, "offset: %f in col: %d\n", offset2[col], col) ;
    }

    mean_offset2 = (double)sinfo_new_clean_mean (offset2, slx, 15., 15. ) ;
    fprintf(fp, "mean offset: %f\n", mean_offset2) ;
    fclose(fp) ;

    cpl_free ( lineref ) ;
    cpl_free ( line ) ;
    cpl_free ( offset2 ) ; 
    filecounter++ ;
    if (filecounter > 100 ) filecounter = 0 ;

    return mean_offset2 ;
}


/**
@brief shifts an image by a given amount to integer pixel accuracy
@name  sinfo_new_shift_image_in_spec()
@param shiftedImage: image to shift in spectral direction
@param shift: amount of shift, output of sinfo_determineShiftByCorrelation 
@param sub_shift: non-integer rest of shift < 1
@return shifted image  
 */


cpl_image * 
sinfo_new_shift_image_in_spec ( cpl_image * shiftedImage, 
                                double shift, 
                                double * sub_shift )
{
    cpl_image * retIm ;
    int        col, row ;
    int        intshift ;
    int ilx=0;
    int ily=0;
    int olx=0;
    int oly=0;

    float* pidata=NULL;
    float* podata=NULL;

    if ( shiftedImage == NULL )
    {
        sinfo_msg_error("no image given!") ;
        return NULL ;
    }

    ilx=cpl_image_get_size_x(shiftedImage);
    ily=cpl_image_get_size_y(shiftedImage);
    pidata=cpl_image_get_data_float(shiftedImage);

    intshift = sinfo_new_nint (shift) ;
    *sub_shift = shift - (double) intshift ;
    if ( intshift == 0 )
    {
        retIm =cpl_image_duplicate ( shiftedImage ) ;
        return retIm ;
    }
    else
    {
        if ( NULL == (retIm = cpl_image_new(ilx,ily,CPL_TYPE_FLOAT)) )
        {
            sinfo_msg_error ("could not allocate memory!") ;
            return NULL ;
        }
    }

    olx=cpl_image_get_size_x(retIm);
    oly=cpl_image_get_size_y(retIm);
    podata=cpl_image_get_data_float(retIm);

    for ( col = 0 ; col < ilx ; col++ )
    {
        for ( row = 0 ; row < ily ; row++ )
        {
            if ( (row-intshift >= 0 ) && (row - intshift < oly) )
            {
                podata[col+(row-intshift)*olx] = pidata[col+row*olx] ;
            }
        }
    }
    return retIm ;
}

/**
@brief shifts an image by a given amount to sub-pixel accuracy
@name sinfo_new_fine_shift_image_in_spec_poly()
@param shiftedImage: image to shift in spectral direction
@param sub_shift: amount of shift < 1, output of  sinfo_shiftImageInSpec
@param order:     order of polynomial
@return shifted image  
*/

cpl_image * 
sinfo_new_fine_shift_image_in_spec_poly ( cpl_image * shiftedImage, 
                                          double sub_shift, 
                                          int order )
{
    cpl_image * retIm ;

    float* spec=NULL ;
    float* corrected_spec=NULL ;
    float* xnum=NULL ;

    float sum, new_sum ;
    float eval/*, dy*/ ;
    float * imageptr ;
    int row, col ;
    int firstpos ;
    int n_points ;
    int i ;
    int flag;
    int ilx=0;
    int ily=0;
    int olx=0;
    /* int oly=0; */

    float* pidata=NULL;
    float* podata=NULL;

    if ( shiftedImage == NULL )
    {
        sinfo_msg_error("no image given!") ;
        return NULL ;
    }
    ilx=cpl_image_get_size_x(shiftedImage);
    ily=cpl_image_get_size_y(shiftedImage);
    pidata=cpl_image_get_data_float(shiftedImage);

    if ( order <= 0 )
    {
        sinfo_msg_error("wrong order of interpolation polynom given!") ;
        return NULL ;
    }

    /* allocate memory */
    if ( NULL == (retIm = cpl_image_new(ilx, ily,CPL_TYPE_FLOAT)) )
    {
        sinfo_msg_error ("could not allocate memory!") ;
        return NULL ;
    }

    olx=cpl_image_get_size_x(retIm);
    /* oly=cpl_image_get_size_y(retIm); */
    podata=cpl_image_get_data_float(retIm);

    n_points = order + 1 ;
    if ( n_points % 2 == 0 )
    {
        firstpos = (int)(n_points/2) - 1 ;
    }
    else
    {
        firstpos = (int)(n_points/2) ;
    }

    spec=cpl_calloc(ily,sizeof(float)) ;
    corrected_spec=cpl_calloc(ily,sizeof(float)) ;
    xnum=cpl_calloc(order+1,sizeof(float)) ;


    /* fill the xa[] array for the polint function */
    for ( i = 0 ; i < n_points ; i++ )
    {
        xnum[i] = i ;
    }

    for ( col = 0 ; col < ilx ; col++ )
    {
        for ( row = 0 ; row < ily ; row++ )
        {
            corrected_spec[row] = 0. ;
        }
        sum = 0. ;
        for ( row = 0 ; row < ily ; row++ )
        {
            spec[row] = pidata[col + row*ilx] ;
            if (isnan(spec[row]) )
            {
                spec[row] = 0. ;
                  
                for ( i = row - firstpos ; i < row-firstpos+n_points ; i++ )
                {
                    if ( i < 0 ) continue ;
                    if ( i >= ily) continue  ;
                    corrected_spec[i] = ZERO ;
                }
            }
            if ( row != 0 && row != ily - 1 )
            {
                sum += spec[row] ;
            }
        }
        
        new_sum = 0. ;
        for ( row = 0 ; row < ily ; row++ )
        {
            /* ---------------------------------------------------------------
             * now determine the arrays of size n_points with which the
             * polynom is determined and determine the position eval
             * where the polynom is evaluated in polynomial interpolation.
             * Take care of the points near the row edges!
             */
            if (isnan(corrected_spec[row])) continue ;
            if ( row - firstpos < 0 )
            {
                imageptr = &spec[0] ;
                eval     = sub_shift + row ;
            }
            else if ( row - firstpos + n_points >= ily )
            {
                imageptr = &spec[ily - n_points] ;
                eval     = sub_shift + row + n_points - ily ;
            }
            else
            {
                imageptr = &spec[row-firstpos] ;
                eval     = sub_shift + firstpos ;
            }

        flag=0;
            corrected_spec[row]=sinfo_new_nev_ille( xnum, imageptr, 
                                                    order, eval, &flag) ;
            if ( row != 0 && row != ily - 1 )
            {
                new_sum += corrected_spec[row] ;
            }
        }

        for ( row = 0 ; row < ily ; row++ )
        {
            if ( new_sum == 0. )
            {
                new_sum = 1. ;
            }
            if ( row == 0 )
            {
                podata[col+row*olx] = ZERO ;
            }
            else if ( row == ily - 1 )
            {
                podata[col+row*olx] = ZERO ;
            }
            else if ( isnan(corrected_spec[row]) )
            {
                podata[col+row*olx] = ZERO ;
            }
            else
            {
                corrected_spec[row] *= sum / new_sum ;
                podata[col+row*olx] = corrected_spec[row] ;
            }
        }
    }
    cpl_free(spec) ;
    cpl_free(corrected_spec) ;
    cpl_free(xnum) ;
    return retIm ;
}
/**
@brief shifts an image by a given amount to sub-pixel accuracy   
@name sinfo_new_fine_shift_image_in_spec_cubicspline()
@param shiftedImage: image to shift in spectral direction
@param sub_shift: amount of shift < 1, output of  sinfo_shiftImageInSpec
@return shifted image  
 */

cpl_image * 
sinfo_new_fine_shift_image_in_spec_cubic_spline ( cpl_image * shiftedImage, 
                                                  double sub_shift )
{
    cpl_image * retIm ;
    /*float second_deriv[shiftedImage -> ly] ;*/
    float* spec=NULL ;
    float* corrected_spec=NULL ;
    float* xnum=NULL ;
    float* eval=NULL ;
    float sum, new_sum ;
    int row, col ;
    int i ;
    int ilx=0;
    int ily=0;
    int olx=0;
    /* int oly=0; */

    float* pidata=NULL;
    float* podata=NULL;

    if ( shiftedImage == NULL )
    {
        sinfo_msg_error("no image given!") ;
        return NULL ;
    }
    ilx=cpl_image_get_size_x(shiftedImage);
    ily=cpl_image_get_size_y(shiftedImage);
    pidata=cpl_image_get_data_float(shiftedImage);

    /* allocate memory */
    if ( NULL == (retIm = cpl_image_new(ilx,ily,CPL_TYPE_FLOAT)) )
    {
        sinfo_msg_error ("could not allocate memory!") ;
        return NULL ;
    }
    olx=cpl_image_get_size_x(retIm);
    /* oly=cpl_image_get_size_y(retIm); */
    podata=cpl_image_get_data_float(retIm);

    xnum=cpl_calloc(ily,sizeof(float)) ;
    /* fill the xa[] array for the spline function */
    for ( i = 0 ; i < ily ; i++ )
    {
        xnum[i] = i ;
    }

    spec=cpl_calloc(ily,sizeof(float)) ;
    corrected_spec=cpl_calloc(ily,sizeof(float)) ;
    eval=cpl_calloc(ily,sizeof(float)) ;

    for ( col = 0 ; col < ilx ; col++ )
    {
        sum = 0. ;
        for ( row = 0 ; row < ily ; row++ )
        {
            spec[row] = pidata[col + row*ilx] ;
            if (isnan(spec[row]) )
            {
                for ( i = row-1 ; i <= row+1 ; i++ )
                {
                    if ( i < 0 ) continue ;
                    if ( i >= ily) continue ;
                    corrected_spec[i] = ZERO ;
                }  
        spec[row] = 0. ;
            }
            sum += spec[row] ;
            eval[row] = (float)sub_shift+(float)row ;
        }
        /* now we do the spline interpolation*/
        if ( -1 == sinfo_function1d_natural_spline( xnum, spec, ily, eval, 
                                              corrected_spec, ily ) )
        {
            sinfo_msg_error("error in spline interpolation!") ;
            return NULL ;
        }
        
        new_sum = 0. ;
        for ( row = 0 ; row < ily ; row++ )
        {
            if ( isnan(corrected_spec[row]) )
            {
                continue ;
            }   
        new_sum += corrected_spec[row] ;
        }

        for ( row = 0 ; row < ily ; row++ )
        {
            if ( new_sum == 0. ) new_sum =1. ;
            {
                if ( isnan(corrected_spec[row]) )
                {
                    podata[col+row*olx] = ZERO ;
                }
                else
                {
                    corrected_spec[row] *= sum / new_sum ;
                    podata[col+row*olx] = corrected_spec[row] ;
                }
            }
        }
    }
    cpl_free(xnum);
    cpl_free(spec) ;
    cpl_free(corrected_spec) ;
    cpl_free(eval) ;

    return retIm ;
}

/**
@brief shifts images stacked in a cube by a given amount to sub-pixel accuracy
@name sinfo_align_cube_to_reference()
@param cube: cube containing images to shift in spectral direction
@param refIm: reference image (OH spectrum) 
@param order: order of polynomial interpolation for the resampling
@param shift_indicator: indicator for the polynomial interpolation (0)
                        or cubic spline interpolation shift (1).
@return shifted cube 
*/

cpl_imagelist * sinfo_align_cube_to_reference (cpl_imagelist * cube, 
                                 cpl_image * refIm,
                                 int order,
                                 int shift_indicator )
{
    cpl_imagelist * retCube=NULL ;
    cpl_image * shiftedIm=NULL ;
    cpl_image * fineShiftedIm=NULL ;
    double shift=0 ;
    double  sub_shift=0 ;
    int z=0;
    double * ker=NULL ;
    cpl_image* img=NULL;

    if (cube == NULL)
    { 
        sinfo_msg_error("no input cube given!") ;
        return NULL ;
    }
    if (refIm == NULL)
    { 
        sinfo_msg_error("no input ref. image given!") ;
        return NULL ;
    }

    /* allocation for a cube structure without the image planes  */
    retCube = cpl_imagelist_new() ;

    if ( shift_indicator != 0 && shift_indicator != 1 )
    {
        ker = sinfo_new_generate_interpolation_kernel("tanh") ;
        if (ker == NULL)
        {
            sinfo_msg_error("kernel generation failure: aborting resampling") ;
            return NULL ;
        }
    }
    
    for ( z = 0 ; z < cpl_imagelist_get_size(cube) ; z++ )
    {
      /* first determine the shift by correlation with the reference image */
      img=cpl_imagelist_get(cube,z);
      if (isnan( shift=sinfo_new_determine_shift_by_correlation(refIm,img)))
        { 
            sinfo_msg_error("error in sinfo_determineShiftByCorrelation()!") ;
            return NULL ;
        }

        if ( NULL == (shiftedIm = sinfo_new_shift_image_in_spec(img,shift,
                                                                &sub_shift)) ) 
        { 
            sinfo_msg_error("error in sinfo_shiftImageInSpec()!") ;
            return NULL ;
        }
        if ( shift_indicator == 0 )
        {
            if ( NULL == (fineShiftedIm = 
                 sinfo_new_fine_shift_image_in_spec_poly (shiftedIm, 
                                                          sub_shift, order)))
            {
                sinfo_msg_error("error in sinfo_fineShiftImageInSpecPoly()!") ;
                return NULL ;
            }
        }
        else if ( shift_indicator == 1)
        {
          if ( NULL == (fineShiftedIm = 
               sinfo_new_fine_shift_image_in_spec_cubic_spline (shiftedIm, 
                                                                  sub_shift)))
          {
            sinfo_msg_error("error in fineShiftImageInSpecCubicspline()!") ;
            return NULL ;
          }
        }
        
    else
    {
      if ( NULL == (fineShiftedIm = 
               sinfo_new_shift_image(shiftedIm,0.,sub_shift, ker ) ) )
          {
           sinfo_msg_error("error in fineShiftImageInSpecCubicspline()!") ;
           return NULL ;
          }     
    }
    cpl_imagelist_set(retCube,fineShiftedIm,z);
        cpl_image_delete (shiftedIm) ;
        cpl_image_delete (fineShiftedIm) ;
    }
    if ( shift_indicator != 0 && shift_indicator != 1 ) cpl_free(ker) ;
    return retCube ;
}

/*--------------------------------------------------------------------------*/
/**@}*/
