/*                                                                            *
 *   This file is part of the ESO X-shooter Pipeline                          *
 *   Copyright (C) 2006 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: 2012-12-18 14:15:45 $
 * $Revision: 1.64 $
 */

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

#define CPL_MODE 0
/*----------------------------------------------------------------------------*/
/**
 * @defgroup xsh_localize_obj  Localize objects (xsh_localize_obj)
 * @ingroup drl_functions
 *
 * Function ...
 */
/*----------------------------------------------------------------------------*/
/**@{*/

/*-----------------------------------------------------------------------------
  Includes
  -----------------------------------------------------------------------------*/

#include <math.h>
#include <xsh_drl.h>

#include <xsh_utils_table.h>
#include <xsh_badpixelmap.h>
#include <xsh_utils_wrappers.h>
#include <xsh_data_pre.h>
#include <xsh_dfs.h>
#include <xsh_pfits.h>
#include <xsh_error.h>
#include <xsh_msg.h>
#include <xsh_fit.h>
#include <xsh_data_instrument.h>
#include <xsh_data_localization.h>
#include <xsh_data_spectrum.h>
#include <xsh_data_rec.h>
#include <xsh_utils_ifu.h>
#include <xsh_ifu_defs.h>
#include <xsh_parameters.h>
#include <cpl.h>

/*-----------------------------------------------------------------------------
  Functions prototypes
  -----------------------------------------------------------------------------*/
static void chunk_coadd( double* data, double* flux, int* qual, int nx, int ny,
  int ifirst, int ilast, const int decode_bp, int *skymask);

static void tab_minmax_get_index( double* data, int size,
				 xsh_slit_limit_param * slit_limit_par,
				 int* min, int* max);

static xsh_localization* xsh_localize_obj_auto( cpl_frame * rec_frame,
  cpl_frame *skymask_frame, 
  xsh_instrument* instrument, xsh_localize_obj_param * loc_obj_par,
  xsh_slit_limit_param * slit_limit_par,const int decode_bp);

static xsh_localization* xsh_localize_obj_manual(
  xsh_instrument *instrument, xsh_localize_obj_param *loc_obj_par);
/*****************************************************************************/


/* not used
static void mask_badpixels( double *errs, int *qual, int nx, int ny)
{
  int i,j;

  XSH_ASSURE_NOT_NULL( errs);
  XSH_ASSURE_NOT_NULL( qual);

  for( j=0 ; j<ny ; j++) {
    for(i=0; i<nx; i++){
      if ( errs[i+j*nx] > 200){
        qual[i+j*nx] = XSH_BAD_PIXEL;
      }
    }
  }

  cleanup:
    return;
}
*/
static void chunk_coadd( double* data, double *flux, int* qual, int nx, int ny,
			 int ifirst, int ilast, const int decode_bp,int *skymask)
{
  int i,j;
  int* nbgoodpixels = NULL;

  XSH_ASSURE_NOT_NULL(data);
  XSH_ASSURE_NOT_NULL(flux);
  XSH_ASSURE_NOT_NULL(qual);
  XSH_ASSURE_NOT_NULL(skymask);
  XSH_ASSURE_NOT_ILLEGAL(ifirst >= 0 && ifirst <= ilast && ilast < nx);

  XSH_CALLOC( nbgoodpixels, int, ny);
  /* initialise data */
  for(j=0; j<ny; j++){
    data[j] = 0;
  }
  /* Note that here mask is a vector sampling wavelength direction,
   * used to mask occurrence of sky lines on the re-sampled frame.
   * For this reason we loop first along X (i) then on Y (j)
   * */
  int j_nx=0;
  for(i=ifirst; i<=ilast; i++){
    if (skymask[i] == 0){
      for( j=0 ; j<ny ; j++) {
        j_nx=i+j*nx;
        if ( (qual[j_nx] & decode_bp) == 0 ){
          nbgoodpixels[j]++;
          data[j] +=flux[j_nx];
        }
        /*
        else{
          xsh_msg_dbg_high("bad pixel at %d %d",i,j);
        }
        */
      }
    }
    else{
      xsh_msg_dbg_medium("Mask by sky lines : column %d", i);
    }
  }

  for(j=0; j<ny; j++){
    if ( nbgoodpixels[j] != 0 ) {
      data[j] = data[j] / (float)(nbgoodpixels[j]);
    }
    else data[j] = 0. ;
  }
 
  cleanup:
    XSH_FREE(nbgoodpixels);
    return;
}

static void tab_minmax_get_index( double* data, int size,
				 xsh_slit_limit_param * slit_limit_par,
				 int* min, int* max)
{
  int i, maxi, mini ;
  int slit0, slit1 ;

  XSH_ASSURE_NOT_NULL(data);
  XSH_ASSURE_NOT_NULL(min);
  XSH_ASSURE_NOT_NULL(max);

  if ( slit_limit_par == NULL ) {
    slit0 = 0 ;
    slit1 = size ;
  }
  else {
    slit0 = slit_limit_par->min_slit_idx + 1 ;
    slit1 = slit_limit_par->max_slit_idx ;
  }
  maxi = 0 ;
  mini = 0 ;

  for(i = slit0; i<slit1; i++) {
    if (data[i]>data[maxi]){
      maxi = i;
    }
    else if (data[i]<data[mini]){
      mini = i;
    }
  }
  *min = mini;
  *max = maxi;

  cleanup:
    return;
}

/*-----------------------------------------------------------------------------
  Implementation
  -----------------------------------------------------------------------------*/

/*****************************************************************************/
/** 
  @brief 
    Build the localization table
  @param[in] rec_frame 
    Rectified frame (from xsh_rectify) or NULL in MANUAL mode
  @param[in] skymask_frame 
    sky mask frame
  @param[in] instrument
    Pointer to instrument description structure
  @param[in] loc_obj_par 
    Parameters for localization
  @param[in] slitlimit_par
    Parameters defining slit limits
  @param[in] fname
    output filename
   @return Localization Table
*/
/*****************************************************************************/
cpl_frame* xsh_localize_obj (cpl_frame *rec_frame, 
                             cpl_frame *skymask_frame,
                             xsh_instrument* instrument,
			     xsh_localize_obj_param * loc_obj_par,
			     xsh_slit_limit_param * slitlimit_par,
			     const char * fname)
{
  cpl_frame *merge_frame = NULL;
  cpl_frame *res_frame = NULL;
  xsh_localization *loc_list = NULL;
  int merge_par = 0;
  const char* rec_prefix = "LOCALIZE";
  char filename[256];

  /* Check parameters */
  
  XSH_ASSURE_NOT_NULL( loc_obj_par);
  XSH_ASSURE_NOT_NULL( instrument);

  xsh_msg_dbg_medium( "Entering xsh_localize_obj") ;
  
  if ( rec_frame != NULL){
    const char* filename = NULL;
    check( merge_frame =  xsh_merge_ord( rec_frame, instrument, merge_par, 
      rec_prefix));
    check( filename = cpl_frame_get_filename( merge_frame));
    check( cpl_frame_set_level( merge_frame, CPL_FRAME_LEVEL_TEMPORARY));
    xsh_add_temporary_file( filename);
  }
 
  /* parameters */
  xsh_msg_dbg_medium("method %s",LOCALIZE_METHOD_PRINT( loc_obj_par->method));

  if ( loc_obj_par->method == LOC_MANUAL_METHOD){
    xsh_msg_dbg_medium("slit position %f slit half height %f",
      loc_obj_par->slit_position, loc_obj_par->slit_hheight);
    check( loc_list = xsh_localize_obj_manual( instrument, loc_obj_par));
  }
  else{
    check( loc_list = xsh_localize_obj_auto( merge_frame, skymask_frame, 
      instrument,
      loc_obj_par, slitlimit_par,instrument->decode_bp));
  }

  xsh_msg_dbg_low( "Saving Localization Table" ) ;
  if ( fname == NULL ) {
    /*
    fname = "LOCALIZATION_TABLE_" || 
            xsh_instrument_arm_tostring(instrument) || ".fits";
    */
    sprintf(filename,"LOCALIZATION_TABLE_%s.fits", 
            xsh_instrument_arm_tostring(instrument));
    xsh_add_temporary_file(filename);
  }  else {  
    sprintf(filename,fname);
  }
  check( res_frame = xsh_localization_save( loc_list, filename, instrument));

  cleanup:
    xsh_localization_free( &loc_list);
    xsh_free_frame( &merge_frame);
  
    return res_frame ;
}
/*****************************************************************************/

static xsh_localization* xsh_localize_obj_manual( 
  xsh_instrument *instrument, xsh_localize_obj_param *loc_obj_par)
{
  xsh_localization *loc_list = NULL;
  int deg_poly = 0;
  double slit_cen, slit_up, slit_low;
  cpl_size pows = 0;

  /* Check parameters */
  XSH_ASSURE_NOT_NULL( loc_obj_par);
  XSH_ASSURE_NOT_NULL( instrument);

  /* Parameters */
  xsh_msg_dbg_medium("slit position %f slit half height %f",
    loc_obj_par->slit_position, loc_obj_par->slit_hheight);

  check( loc_list = xsh_localization_create());

  slit_cen = loc_obj_par->slit_position;
  slit_up = slit_cen + loc_obj_par->slit_hheight;
  slit_low = slit_cen - loc_obj_par->slit_hheight;

  loc_list->pol_degree = deg_poly;
  check( loc_list->cenpoly = cpl_polynomial_new( 1)); 
  check( loc_list->edglopoly = cpl_polynomial_new( 1));
  check( loc_list->edguppoly = cpl_polynomial_new( 1));
  check( cpl_polynomial_set_coeff( loc_list->cenpoly,
    &pows, slit_cen));
  check( cpl_polynomial_set_coeff( loc_list->edglopoly,
    &pows, slit_low));
  check( cpl_polynomial_set_coeff( loc_list->edguppoly,
    &pows, slit_up));
  
  cleanup:
    return loc_list;
}

/*****************************************************************************/
/** 
  @brief 
    Build localization table frame from the rectified table frame
  @param[in] merge_frame 
    Merge frame (from xsh_merge_ord) or NULL in MANUAL mode
  @param[in] skymask_frame
    Sky mask frame
  @param[in] instrument
    Pointer to instrument description structure
  @param[in] loc_obj_par 
    Parameters for localization
  @params[in] slitlimit_par
    Parameters to define slit limits
  @params[in] decode_bp
    Bad pixel code
   @return Localization Table
*/
/*****************************************************************************/
static xsh_localization* 
xsh_localize_obj_auto( cpl_frame *merge_frame,
		       cpl_frame *skymask_frame,
		       xsh_instrument *instrument, 
		       xsh_localize_obj_param *loc_obj_par,
		       xsh_slit_limit_param *slitlimit_par,const int decode_bp)
{
  xsh_localization *loc_list = NULL;
  xsh_spectrum *spectrum = NULL;
  double* slit_center = NULL;
  double* slit_upper = NULL;
  double* slit_lower = NULL;
  double* chunk_center = NULL;
  cpl_vector* slit_center_v = NULL;
  cpl_vector* slit_upper_v = NULL;
  cpl_vector* slit_lower_v = NULL;  
  cpl_vector* chunk_center_v = NULL;

#if 0
  int i = 0;
  int skip_order_nb = 0;
  int first_order_index = 0;
  int slit_nod, slit_lim ;
#endif

  int level;

  int nslit, nlambda, chunk_size;
  double* slit_tab = NULL;
  double *flux = NULL;
  //double *errs = NULL;
  int *qual = NULL;
  int ichunk, skip_chunk=0, nb_chunk;
  cpl_polynomial *cen_poly = NULL;
  int loc_degree;
  double lambda_min, lambda_step;
  double slit_min, slit_step, kappa=2.0;
  int islit, iter, niter=5, nbrej=-1;
  cpl_vector *dispslit = NULL;
  cpl_vector *gausspos = NULL;
  cpl_vector *gaussval = NULL;
  int *sky_mask = NULL;
  cpl_table *skymask_table = NULL;
  const char* skymask_name = NULL;

  /* Check parameters */
  XSH_ASSURE_NOT_NULL( loc_obj_par);
  XSH_ASSURE_NOT_NULL( instrument);
  XSH_ASSURE_NOT_NULL( merge_frame);

  /* parameters */
  kappa = loc_obj_par->kappa;
  niter = loc_obj_par->niter;
  loc_degree = loc_obj_par->loc_deg_poly;
  xsh_msg_dbg_medium( "Entering xsh_localize_obj_auto") ;
  xsh_msg_dbg_medium("Localize deg_poly %d chunk %d Thresh %f kappa %f niter %d",
    loc_degree, loc_obj_par->loc_chunk_nb, loc_obj_par->loc_thresh, 
    kappa, niter);
  if ( loc_obj_par->use_skymask){
    XSH_ASSURE_NOT_NULL( skymask_frame);
    check( skymask_name = cpl_frame_get_filename( skymask_frame));
    xsh_msg_dbg_medium("Sky mask %s", skymask_name);
  }

  level = xsh_debug_level_get();

  /* Load merge spectrum 2D */
  check( spectrum = xsh_spectrum_load( merge_frame));
  check( nslit = xsh_spectrum_get_size_slit( spectrum));
  check( nlambda = xsh_spectrum_get_size_lambda( spectrum));
  lambda_min = spectrum->lambda_min;
  lambda_step = spectrum->lambda_step;
  slit_min = spectrum->slit_min;
  slit_step = spectrum->slit_step;

  check( flux = cpl_image_get_data_double( spectrum->flux));
  //check( errs = cpl_image_get_data_double( spectrum->errs));
  check( qual = cpl_image_get_data_int( spectrum->qual));

  /* allocate memory */
  XSH_CALLOC( chunk_center, double, loc_obj_par->loc_chunk_nb);
  XSH_CALLOC( slit_center, double, loc_obj_par->loc_chunk_nb);
  XSH_CALLOC( slit_upper, double, loc_obj_par->loc_chunk_nb);
  XSH_CALLOC( slit_lower, double, loc_obj_par->loc_chunk_nb);

  /* create sky mask */
  XSH_CALLOC( sky_mask, int, nlambda);

  if ( loc_obj_par->use_skymask){
    float *skymask_data = NULL;
    int irow, nrow;
    double fwhm =0.0, sky_min, sky_max;
    int isky_min, isky_max, imask;
    double width, resolution;

    XSH_TABLE_LOAD( skymask_table, skymask_name);
    /* sort wavelength */
    check( xsh_sort_table_1( skymask_table, "WAVELENGTH", CPL_FALSE));
    check( skymask_data = cpl_table_get_data_float( skymask_table, 
      "WAVELENGTH"));
    check( nrow = cpl_table_get_nrow( skymask_table));

    xsh_msg_dbg_low("lambda min %f, step %f", lambda_min, lambda_step);

    for( irow=0; irow < nrow; irow++){
      check( width = xsh_pfits_get_slit_width( spectrum->flux_header, instrument));
      resolution = xsh_resolution_get( instrument, width);
      fwhm = skymask_data[irow]/resolution;
      sky_min = skymask_data[irow]-fwhm;
      sky_max = skymask_data[irow]+fwhm;
      isky_min =  (int)xsh_round_double((sky_min-lambda_min)/lambda_step);
      isky_max = (int)xsh_round_double((sky_max-lambda_min)/lambda_step);
      for( imask=isky_min; imask <=isky_max; imask++){
        sky_mask[imask] = 1;
      }
    }
    if (level  >= XSH_DEBUG_LEVEL_MEDIUM){
      FILE *mask_file = NULL;
      char mask_name[256];
      int idbg=0;

      sprintf( mask_name, "skymask.reg");
      mask_file = fopen( mask_name, "w");

      fprintf( mask_file,"# Region file format: DS9 version 4.1\n");
      fprintf( mask_file,"global color=green dashlist=8 3 width=1 font=\"helvetica 10 normal\"\
        select=1 highlite=1 dash=0 fixed=0 edit=1 move=1 delete=1 include=1 source=1\n");
      fprintf(mask_file,"physical\n");

      for(idbg=0; idbg< nlambda; idbg++){
        
        if (sky_mask[idbg] == 1){
          fprintf( mask_file, "line(%d,%d,%d,%d)\n",idbg+1, nslit, idbg+1, 1);
        }
      }
      fclose( mask_file);
    }
  }
  /* Create the result */ 
  check( loc_list = xsh_localization_create());

#if 0 /* REGDEBUG COMMENT for now don't what is it ?? */
  /* Calculate slit_nod from arcsec to index */ 
  {
    float arcsec ;
    switch( xsh_instrument_get_arm( instrument ) ) {
    case XSH_ARM_NIR:
      arcsec = XSH_ARCSEC_NIR ;
      break ;
    case XSH_ARM_UVB:
      arcsec = XSH_ARCSEC_UVB ;
      break ;
    case XSH_ARM_VIS:
      arcsec = XSH_ARCSEC_VIS ;
      break ;
    default:
      arcsec = 0.15 ;
      break ;
    }
    slit_nod = loc_obj_par->nod_step/arcsec ;
    xsh_msg_dbg_medium( "nod_step: %lf, slit_nod: %d",
			loc_obj_par->nod_step, slit_nod ) ;
  }
#endif
  XSH_CALLOC( slit_tab, double, nslit);
  check( gaussval = cpl_vector_wrap( nslit, slit_tab));
  check( gausspos = cpl_vector_new( nslit));
  for( islit=0; islit < nslit; islit++){
    cpl_vector_set( gausspos, islit, islit);
  }

  chunk_size = (nlambda-1) / (double)(loc_obj_par->loc_chunk_nb);

  /* mask the bad pixel */
  //check( mask_badpixels( errs, qual, nlambda, nslit));

  for( ichunk=0; ichunk< loc_obj_par->loc_chunk_nb; ichunk++){
    /*
	  If in nodding mode, yup and ylow must be relative to ymax:
	  ylow = ymax - nod_step/2
	  yup = ymax + nod_step/2
    */
    int ifirst=0,ilast=0,icenter=0;
    double slit_cen=0, slit_low=0, slit_up=0;
    double threshold = 0.0;
    int is_valid_chunk = 1;

    ifirst = ichunk*chunk_size;
    ilast = (ichunk+1)*chunk_size;
    icenter = (int)((ifirst+ilast)/2);

    xsh_msg_dbg_medium("%d-%d", ifirst,ilast);

    /* Sum by chunk the good pixels of flux image and normalize by */
    chunk_coadd( slit_tab, flux, qual, nlambda, nslit, ifirst, ilast, decode_bp, sky_mask);

    if (level  >= XSH_DEBUG_LEVEL_MEDIUM){
      FILE *coadd_file = NULL;
      char coadd_name[256];
      int icoadd=0;
      const char* filename = cpl_frame_get_filename( merge_frame);

      sprintf( coadd_name, "%s_%d_%d.dat",filename,ifirst,ilast);
      coadd_file = fopen( coadd_name, "w");
      
      for(icoadd=0; icoadd<nslit; icoadd++){
        fprintf(coadd_file, "%d %f\n",icoadd,slit_tab[icoadd]);
      }  
      fclose( coadd_file);
    }

    if ( loc_obj_par->method == LOC_GAUSSIAN_METHOD){
      double cenpos=0, sigma=0,area=0, offset=0;
      double kappa=3.0;

      xsh_msg_dbg_medium("Using GAUSSIAN method to fit chunk %d_%d", ifirst,ilast);
      cpl_vector_fit_gaussian( gausspos, NULL, gaussval, NULL, CPL_FIT_ALL, &cenpos,
                           &sigma,
                           &area, &offset, NULL,
                           NULL, NULL);
      if( cpl_error_get_code() == CPL_ERROR_CONTINUE ){
        xsh_msg_dbg_low("CONTINUE to fit %d_%d x0 %f sigma %f", ifirst,ilast,cenpos,sigma);
      }
      if( cpl_error_get_code() == CPL_ERROR_NONE ){
        slit_cen = cenpos;
        slit_low = cenpos-kappa*sigma;
        slit_up = cenpos+kappa*sigma;
        if (slit_low < 0) slit_low = 0;
        if (slit_up >= nslit) slit_up = nslit-1;
      }
      else{
        xsh_msg_dbg_low("FAILED to fit %d_%d", ifirst,ilast);
        is_valid_chunk=0;
        xsh_error_reset();
      }
    }
    else
    {
      int ymax=0, ymin=0;
      int ylow=0, yup=0;
      tab_minmax_get_index( slit_tab, nslit, slitlimit_par, &ymin, &ymax);
      if (ymin == ymax){
        xsh_msg_warning( "No maximum found in chunk (skip it)");
        is_valid_chunk=0;
      }
      else{
      

        xsh_msg_dbg_medium("ymin [%d] = %f ymax [%d] = %f",ymin,
          slit_tab[ymin], ymax, slit_tab[ymax]);
    
        threshold = slit_tab[ymin]+(slit_tab[ymax]-slit_tab[ymin])*
          loc_obj_par->loc_thresh;
        xsh_msg_dbg_medium("Threshold at %f",threshold);

        /* search upper edge */
        yup = ymax+1;
        while( yup < nslit && (slit_tab[yup] >= threshold)) {
          yup++;
        }

        /* search lower edge */
        ylow = ymax-1;
        while( ylow > 0 && (slit_tab[ylow] >= threshold)) {
          ylow--;
        }

        slit_cen = ymax;
        slit_low = ylow;
        slit_up = yup;
      }
    }
    /* put result in tab */
    if ( is_valid_chunk == 1){
      chunk_center[ichunk-skip_chunk] = lambda_min+lambda_step*icenter;
      slit_center[ichunk-skip_chunk] = slit_min+slit_step*slit_cen;
      slit_lower[ichunk-skip_chunk] = slit_min+slit_step*slit_low;
      slit_upper[ichunk-skip_chunk] = slit_min+slit_step*slit_up; 
    }
    else{
      skip_chunk++;
    }
  }

  nb_chunk = loc_obj_par->loc_chunk_nb-skip_chunk;

  XSH_ASSURE_NOT_ILLEGAL(loc_obj_par->loc_deg_poly < nb_chunk);

  check( chunk_center_v = cpl_vector_wrap( 
      nb_chunk, chunk_center));
  check( slit_center_v = cpl_vector_wrap( 
      nb_chunk, slit_center));

  check( cen_poly =
    xsh_polynomial_fit_1d_create( chunk_center_v, slit_center_v,
      loc_degree, NULL));

  /* sigma clipping */
  if ( niter > 0){
    xsh_msg_dbg_low("Doing sigma clipping");
    iter = 0;

    while ( (loc_degree < (nb_chunk-nbrej)) && iter < niter
      && nbrej != 0){
      double sigma_med;
 
      nbrej = 0; 
      xsh_msg_dbg_medium( "  *** NBITER = %d / %d ***", iter+1, niter); 
      dispslit = cpl_vector_new( nb_chunk);

      for(ichunk = 0; ichunk < nb_chunk; ichunk++){
        double slit_fit;
        double lambda;
        double slit_diff;

        lambda = chunk_center[ichunk];

        check( slit_fit = cpl_polynomial_eval_1d( cen_poly,
          lambda, NULL));

        slit_diff  = slit_center[ichunk]-slit_fit;
        xsh_msg_dbg_low("slit_center %f FIT %f, DIFF %d %f", slit_center[ichunk], slit_fit, ichunk, slit_diff);
        check( cpl_vector_set( dispslit, ichunk, slit_diff));
      }

      check( sigma_med = cpl_vector_get_stdev( dispslit));
      xsh_msg_dbg_medium("    kappa %f SIGMA MEDIAN = %f", kappa, sigma_med);

      for(ichunk = 0; ichunk < nb_chunk; ichunk++){
        if ( fabs(cpl_vector_get( dispslit, ichunk)) > (kappa * sigma_med) ){
          nbrej++;
        }
        else{
          chunk_center[ichunk-nbrej] = chunk_center[ichunk];
          slit_center[ichunk-nbrej] = slit_center[ichunk];
          slit_lower[ichunk-nbrej] = slit_lower[ichunk];
          slit_upper[ichunk-nbrej] = slit_upper[ichunk];
        }
      }

      xsh_msg_dbg_medium(" Removed %d points", nbrej);

      nb_chunk -= nbrej;

      /* Redo the fit */
      xsh_unwrap_vector( &chunk_center_v);
      xsh_unwrap_vector( &slit_center_v);
      xsh_free_polynomial( &cen_poly);

      check( chunk_center_v = cpl_vector_wrap(
        nb_chunk, chunk_center));
      check( slit_center_v = cpl_vector_wrap(
        nb_chunk, slit_center));

      check( cen_poly =
        xsh_polynomial_fit_1d_create( chunk_center_v, slit_center_v,
          loc_degree, NULL));
      xsh_free_vector( &dispslit);
      iter++;
    }
  }

  if (level  >= XSH_DEBUG_LEVEL_MEDIUM){
    FILE *debug_file = NULL;
    char debug_name[256];
    int idebug=0;
    const char* filename = cpl_frame_get_filename( merge_frame);

    sprintf( debug_name, "%s_points.dat",filename);
    debug_file = fopen( debug_name, "w");

    fprintf( debug_file,"#chunk_pos slit_low slit_cen slit_up\n");

    for(idebug=0; idebug<nb_chunk; idebug++){
      fprintf( debug_file, "%f %f %f %f\n",chunk_center[idebug],
        slit_lower[idebug], slit_center[idebug], slit_upper[idebug]);
    }
    fclose( debug_file);
  }

  /* create edges polynomials */
  check( slit_lower_v = cpl_vector_wrap( 
     nb_chunk, slit_lower));
  check( slit_upper_v = cpl_vector_wrap( 
    nb_chunk, slit_upper));
  loc_list->pol_degree = loc_degree;

  check(loc_list->cenpoly = cpl_polynomial_duplicate( cen_poly));
  check(loc_list->edglopoly =
    xsh_polynomial_fit_1d_create( chunk_center_v, slit_lower_v,
        loc_degree, NULL));
  check(loc_list->edguppoly =
    xsh_polynomial_fit_1d_create( chunk_center_v, slit_upper_v,
      loc_degree, NULL));

  cleanup:
    xsh_unwrap_vector( &chunk_center_v);
    xsh_unwrap_vector( &slit_center_v);
    xsh_unwrap_vector( &slit_upper_v);
    xsh_unwrap_vector( &slit_lower_v);
    xsh_unwrap_vector( &gaussval);
    xsh_free_vector( &gausspos);
    XSH_FREE( chunk_center);
    XSH_FREE( slit_center);
    XSH_FREE( slit_upper);
    XSH_FREE( slit_lower);
    XSH_FREE( slit_tab);
    XSH_FREE( sky_mask);
    xsh_free_table( &skymask_table);
    xsh_free_polynomial( &cen_poly);
    xsh_spectrum_free( &spectrum);
    return loc_list;

}
/*****************************************************************************/

/*
static int comp_lambda( const void * one, const void * two )
{
  float * un = (float *)one, * deux = (float *)two ;

  if ( *un < *deux ) return -1 ;
  else if ( *un == *deux ) return 0 ;
  else return 1 ;
}


static cpl_frame * concat_rec( cpl_frame * rect_frame,
			       xsh_instrument * instrument,
			       int slitlet, int * nb_orders )
{
  cpl_frame * result = NULL ;
  xsh_rec_list * res_list = NULL, * rect_list = NULL ;
  int norders, i, ns, nlambda, depth ;
  float * res_slit = NULL, * rect_slit = NULL ;
  double * res_lambda = NULL, * rect_lambda = NULL, * plambda ;
  const char * tag = "ORDER2D_DOWN_IFU_VIS" ;
  char * fname = NULL ;
  float * res_data = NULL, * res_errs = NULL ;
  int * res_qual = NULL ;
  float * pdata, * perrs, * pdata1, * perrs1 ;
  int * pqual, * pqual1 ;
  float * rect_data = NULL, * rect_errs = NULL ;
  int * rect_qual = NULL ;

  XSH_ASSURE_NOT_NULL( rect_frame ) ;

  check( rect_list = xsh_rec_list_load( rect_frame, instrument ) ) ;
  // Create the list 
  check( res_list = xsh_rec_list_create_with_size( 1, instrument ) ) ;
  // Populate with rect_frame 
  ns = xsh_rec_list_get_nslit( rect_list, 0);
  norders = rect_list->size ;
  *nb_orders = norders ;
  for( i = 0, nlambda = 0 ; i<norders ; i++ ) {
    nlambda += xsh_rec_list_get_nlambda( rect_list, i);
    xsh_msg_dbg_medium( "Nlambda[%d] = %d, sum = %d", ns,
	     rect_list->list[i].nlambda, nlambda);
  }
  xsh_msg_dbg_medium( "Single Rectify list: ns = %d, Nlambda = %d",
		      ns, nlambda ) ;
  // Now fill the list 
  // Prepare the entry 
  check( xsh_rec_list_set_data_size( res_list, 0, 16, nlambda, ns ) ) ;
  // Fill slit array 
  res_slit = xsh_rec_list_get_slit( res_list, 0 ) ;
  rect_slit = xsh_rec_list_get_slit( rect_list, 0 ) ;
  memcpy( res_slit, rect_slit, ns*sizeof( float ) ) ;

  // Fill Lambd array 
  res_lambda = xsh_rec_list_get_lambda( res_list, 0 ) ;
  plambda = res_lambda ;
  for( i = 0 ; i<norders ; i++ ) {
    int nl ;

    rect_lambda = xsh_rec_list_get_lambda( rect_list, i ) ;
    nl = xsh_rec_list_get_nlambda( rect_list, i) ;
    memcpy( plambda, rect_lambda, nl*sizeof(double) ) ;;
    plambda += nl ;
  }

  // Now fill data, errs and qual 
  depth = ns*nlambda ;
  res_data = xsh_rec_list_get_data1( res_list, 0 ) ;
  pdata = res_data ;
  pdata1 = res_data ;
  res_errs = xsh_rec_list_get_errs1( res_list, 0 ) ;
  perrs = res_errs ;
  perrs1 = res_errs ;
  res_qual = xsh_rec_list_get_qual1( res_list, 0 ) ;
  pqual = res_qual ;
  pqual1 = pqual ;
  for( i = 0 ; i<norders ; i++ ) {
    int nl ;
    float * rdata, * pdata2 ;
    float * rerrs, * perrs2 ;
    int * rqual, * pqual2 ;
    int ks, kl ;

    check( rect_data = xsh_rec_list_get_data1( rect_list, i ) ) ;
    check( rect_errs = xsh_rec_list_get_errs1( rect_list, i ) ) ;
    check( rect_qual = xsh_rec_list_get_qual1( rect_list, i ) ) ;
    nl = xsh_rec_list_get_nlambda( rect_list, i) ;
    xsh_msg_dbg_medium( "Order %d, nlambdas: %d [%d]", i, nl, nlambda ) ;
    rdata = rect_data ;
    pdata2 = pdata1 ;
    rerrs = rect_errs ;
    perrs2 = perrs1 ;
    rqual = rect_qual ;
    pqual2 = pqual1 ;
    for ( ks = 0 ; ks < ns ; ks++ ) {
      for ( kl = 0 ; kl < nl ; kl++ ) {
	*pdata++ = *rdata++ ;
	*perrs++ = *rerrs++ ;
	*pqual++ = *rqual++ ;
      }
      pdata2 += nlambda ;
      pdata = pdata2 ;
      perrs2 += nlambda ;
      perrs = perrs2 ;
      pqual2 += nlambda ;
      pqual = pqual2 ;
    }
    pdata1 += nl ;
    pdata = pdata1 ;
    perrs1 += nl ;
    perrs = perrs1 ;
    pqual1 += nl ;
    pqual = pqual1 ;
  }

  // Sort by Lambdas (lambda array, data, errs and qual !) 
  {
    int * idx ;
    int ks ;

    idx = xsh_sort( res_lambda, nlambda, sizeof( double ), comp_lambda ) ;
    pdata = res_data ;
    perrs = res_errs ;
    pqual = res_qual ;
    for( ks = 0 ; ks < ns ; ks++ ) {
      xsh_reindex_float( pdata, idx, nlambda ) ;
      pdata += nlambda ;
      xsh_reindex_float( perrs, idx, nlambda ) ;
      perrs += nlambda ;
      xsh_reindex_int( pqual, idx, nlambda ) ;
      pqual += nlambda ;
    }
  }

  fname = xsh_stringcat_any( "SINGLE_RECTIFY_", SlitletName[slitlet],
			     ".fits", (void*)NULL ) ;
  check( result = xsh_rec_list_save( res_list, fname, tag, 1 ) ) ;
  XSH_FREE( fname ) ;
  fname = xsh_stringcat_any( "MULTIPLE_RECTIFY_", SlitletName[slitlet],
			     ".fits", (void*)NULL ) ;
  check( rect_frame = xsh_rec_list_save( rect_list,fname, tag, 1 ) ) ;
 cleanup:
  xsh_rec_list_free( &res_list ) ;
  xsh_rec_list_free( &rect_list ) ;
  return result ;
}
*/

/*****************************************************************************/
/*****************************************************************************/
cpl_frameset * xsh_localize_obj_ifu( cpl_frameset *rec_frameset,
                                     cpl_frame *skymask_frame,
				     xsh_instrument * instrument,
				     xsh_localize_obj_param * locobj_par,
				     xsh_slit_limit_param *slitlimit_par)
{
  int i, slitlet;
  cpl_frameset *result_frameset = NULL;
  char fname[256];

  XSH_ASSURE_NOT_NULL( rec_frameset);
  XSH_ASSURE_NOT_NULL( instrument);
  XSH_ASSURE_NOT_NULL( locobj_par);

  check( result_frameset = cpl_frameset_new());

  for( i = 0, slitlet = LOWER_IFU_SLITLET ; i < 3 ; i++, slitlet++ ) {
    cpl_frame * loc_frame = NULL;
    cpl_frame *rec_frame = NULL;

    sprintf( fname ,"LOCALIZE_TABLE_%s_IFU_%s.fits", SlitletName[slitlet],
      xsh_instrument_arm_tostring( instrument));

    xsh_msg( "Localizing slitlet %s, frame '%s'", SlitletName[slitlet], fname);

    check( rec_frame = cpl_frameset_get_frame( rec_frameset, i));

    check( loc_frame = xsh_localize_obj( rec_frame, skymask_frame, instrument, 
      locobj_par,slitlimit_par, fname));
    check( cpl_frameset_insert( result_frameset, loc_frame));
  }

 cleanup:
  if ( cpl_error_get_code() != CPL_ERROR_NONE ) {
    xsh_free_frameset( &result_frameset);
  }
  return result_frameset;
}
/*--------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*/
/**
   @brief Convert seeing keywork in mu sigma
   @param[in] Object frame
   @return mu sigma
*/
double xsh_convert_seeing( cpl_frame* frame)
{
  double mu=-1.0;
  double avg_seeing, seeing_start, seeing_end;
  double avg_airmass;
  double k;
  const char* filename = NULL;
  cpl_propertylist *header = NULL;
  
  XSH_ASSURE_NOT_NULL( frame);

  check( filename = cpl_frame_get_filename( frame));
  check( header = cpl_propertylist_load( filename, 0)); 
  check( avg_airmass = xsh_pfits_get_airm_mean( header)); 
  check( seeing_start = xsh_pfits_get_seeing_start( header));
  check( seeing_end = xsh_pfits_get_seeing_end( header));
  avg_seeing = 0.5*(seeing_start+seeing_end);
  
  k = sqrt( 1.0-78.08*pow(XSH_LAMBDA_DIMM,0.4)*
    pow(avg_airmass,-0.6)*pow(avg_seeing,-1.0/3.0));

  xsh_msg("K %f", k);

  mu = 1.0/CPL_MATH_FWHM_SIG*avg_seeing*pow(XSH_LAMBDA_DIMM,0.2)*
    pow(avg_airmass,0.6)*k;

  xsh_msg("Mu %f", mu);

  cleanup:
  if ( cpl_error_get_code() != CPL_ERROR_NONE ) {
    mu =-1;
  }
  xsh_free_propertylist( &header);
  return mu;
}
/*--------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*/
/**
   @brief Localize center of object on a merge 2D IFU slitlet
   @param[in] merge2d_slitlet merge 2D IFU slitlet
   @param[in] smooth_hsize Half size of median filter using for smoothing
   @param[in] nscales nscales use for atrous decomposition
   @param[in] HF_skip Number of skipping High Frequency used to reconstruct 
              signal
   @param[in] resname Name of result table
   @param[in] cut_sigma_low Gaussian fits of the cross-dispersion profile whose FWHM is lower than this value are rejected
   @param[in] cut_sigma_up Gaussian fits of the cross-dispersion profile whose FWHM is upper than this value are rejected
   @param[in] cut_snr_low Gaussian fits of the cross-dispersion profile whose SNR is lower than this value are rejected
   @param[in] cut_snr_up Gaussian fits of the cross-dispersion profile whose SNR is upper than this value are rejected
   @param[in] box_hsize Half size of running chunk box
   @param[in] slit_min min slit value
   @param[in] slit_max max slit value
   @param[in] deg poly degree
   @param[in] box_hsize half size of box
   @param[in] instrument arm setting
   @return frame containing for Wavelength the position of object center 
           on the slit
*/
cpl_frame* xsh_localize_ifu_slitlet( cpl_frame *merge2d_slitlet, 
  cpl_frame *skymask_frame, int smooth_hsize,
  int nscales, int HF_skip, const char* resname, double cut_sigma_low, 
  double cut_sigma_up, double cut_snr_low, double cut_snr_up,
  double slit_min, double slit_max, int deg, int box_hsize,
  xsh_instrument *instrument)

{
  cpl_frame *result = NULL;
  double wmin, wstep; 
  //double wmax;
  double smin, sstep;
  //double smax;
  int wsize, ssize;
  xsh_spectrum *spectrum2d = NULL;
  double *flux = NULL;
  double *errs = NULL;
  int *qual = NULL;
  int i, j, k;
  int level;
  double *slit_vect_data = NULL;
  double *sliterr_vect_data = NULL;
  double *slit_pos_data = NULL;
  int slit_vect_size;
  cpl_vector *slit_vect = NULL;
  cpl_vector *sliterr_vect = NULL;
  cpl_vector *slit_pos = NULL;
  cpl_vector *smooth_vect = NULL;
  double *spos_data = NULL;
  double *errpos_data = NULL;
  double *wpos_data = NULL;
  double *sigma_data = NULL;
  double *snr_data = NULL;
  int data_size=0, ndata_size=0;
  cpl_vector *spos_vect = NULL;
  cpl_vector *wpos_vect = NULL;
  cpl_vector *sigma_vect = NULL;
  cpl_vector *snr_vect = NULL;
  cpl_vector *sigma_sort_vect = NULL;
  cpl_vector *snr_sort_vect = NULL;
  double *sposg_data = NULL;
  double *wposg_data = NULL;
  cpl_matrix *decomp = NULL;
  int nb_scales;
  cpl_table *table = NULL;
  char tablename[256];
  cpl_propertylist *header = NULL;
  double n05, n95, snr_05, snr_95, sigma_05, sigma_95;
  int jmin, jmax;
  /* sky mask */
  int *sky_mask = NULL;
  cpl_table *skymask_table = NULL;
  const char* skymask_name = NULL;

  XSH_ASSURE_NOT_NULL( merge2d_slitlet);
  XSH_ASSURE_NOT_ILLEGAL( smooth_hsize >= 0);
  XSH_ASSURE_NOT_ILLEGAL( nscales > 0);
  XSH_ASSURE_NOT_ILLEGAL( nscales > HF_skip);
  XSH_ASSURE_NOT_ILLEGAL( box_hsize >= 0);
  /*
  xsh_msg_dbg_medium("PARAMS merge2d_slitlet %s",cpl_frame_get_filename(merge2d_slitlet));
  if (skymask_frame != NULL){
    xsh_msg_dbg_medium("PARAMS skymask_frame %s",cpl_frame_get_filename(skymask_frame));
  }
  
  xsh_msg_dbg_medium("PARAMS smooth_hsize %d", smooth_hsize);
  xsh_msg_dbg_medium("PARAMS nscales %d", nscales);
  xsh_msg_dbg_medium("PARAMS HF_skip %d", HF_skip);
  xsh_msg_dbg_medium("PARAMS res_name %s", resname);
  xsh_msg_dbg_medium("PARAMS cut sigma %f %f", cut_sigma_low, cut_sigma_up);
  xsh_msg_dbg_medium("PARAMS cut snr %f %f", cut_snr_low, cut_snr_up);
  xsh_msg_dbg_medium("PARAMS slit %f %f", slit_min, slit_max);
  xsh_msg_dbg_medium("PARAMS deg %d", deg); 
  */
  level = xsh_debug_level_get();
  check( spectrum2d = xsh_spectrum_load( merge2d_slitlet));
  check( wmin = xsh_spectrum_get_lambda_min( spectrum2d));
  //check( wmax = xsh_spectrum_get_lambda_max( spectrum2d));
  check( wstep = xsh_spectrum_get_lambda_step( spectrum2d));

if ( skymask_frame != NULL){
    check( skymask_name = cpl_frame_get_filename( skymask_frame));
    xsh_msg_dbg_medium("Sky mask %s", skymask_name);
  }
  wsize = spectrum2d->size_lambda;


  smin = spectrum2d->slit_min;
  //smax = spectrum2d->slit_max;
  sstep = spectrum2d->slit_step;
  ssize = spectrum2d->size_slit;

  jmin = 0;
  jmax = ssize;
  
  
  while( (smin+jmin*sstep) < slit_min){
    jmin++;
  }
  while( (smin+jmax*sstep) > slit_max){
    jmax--;
  }

  /* in case of wrong value put default */
  if ( jmin > jmax || jmax <= 0 || jmin >= ssize){
    jmin = 0;
    jmax = ssize;
  }
  xsh_msg_dbg_medium( "Use [%d-%d] from [%d %d] slitlet",
    jmin, jmax, 0, ssize);

  check( flux = xsh_spectrum_get_flux( spectrum2d));
  check( errs = xsh_spectrum_get_errs( spectrum2d));
  check( qual = xsh_spectrum_get_qual( spectrum2d));

  XSH_MALLOC( slit_vect_data, double , ssize);
  XSH_MALLOC( sliterr_vect_data, double, ssize);
  XSH_MALLOC( slit_pos_data, double , ssize);

  XSH_MALLOC( spos_data, double , wsize);
  XSH_MALLOC( errpos_data, double, wsize);
  XSH_MALLOC( wpos_data, double , wsize);

  XSH_MALLOC( sigma_data, double , wsize);
  XSH_MALLOC( snr_data, double , wsize);

/* create sky mask */
  XSH_CALLOC( sky_mask, int, wsize);

  if ( skymask_frame != NULL){
    float *skymask_data = NULL;
    int irow, nrow;
    double fwhm =0.0, sky_min, sky_max;
    int isky_min, isky_max, imask;
    double width, resolution;

    XSH_TABLE_LOAD( skymask_table, skymask_name);
    /* sort wavelength */
    check( xsh_sort_table_1( skymask_table, "WAVELENGTH", CPL_FALSE));
    check( skymask_data = cpl_table_get_data_float( skymask_table, 
      "WAVELENGTH"));
    check( nrow = cpl_table_get_nrow( skymask_table));

    for( irow=0; irow < nrow; irow++){
      check( width = xsh_pfits_get_slit_width( spectrum2d->flux_header, instrument));
      resolution = xsh_resolution_get( instrument, width);
      fwhm = skymask_data[irow]/resolution;
      sky_min = skymask_data[irow]-fwhm;
      sky_max = skymask_data[irow]+fwhm;
      isky_min =  (int)xsh_round_double((sky_min-wmin)/wstep);
      if (isky_min < 0){
        isky_min=0;
      }
      isky_max = (int)xsh_round_double((sky_max-wmin)/wstep);
      if ( isky_max >= wsize){
        isky_max = wsize-1;
      }
      for( imask=isky_min; imask <=isky_max; imask++){
        sky_mask[imask] = 1;
      }
    }

    if (level  >= XSH_DEBUG_LEVEL_MEDIUM){
      FILE *mask_file = NULL;
      char mask_name[256];
      int idbg=0;

      sprintf( mask_name, "skymask.reg");
      mask_file = fopen( mask_name, "w");

      fprintf( mask_file,"# Region file format: DS9 version 4.1\n");
      fprintf( mask_file,"global color=green dashlist=8 3 width=1 font=\"helvetica 10 normal\"\
        select=1 highlite=1 dash=0 fixed=0 edit=1 move=1 delete=1 include=1 source=1\n");
      fprintf(mask_file,"physical\n");

      for(idbg=0; idbg< wsize; idbg++){
        
        if (sky_mask[idbg] == 1){
          fprintf( mask_file, "line(%d,%d,%d,%d)\n",idbg+1, ssize, idbg+1, 1);
        }
      }
      fclose( mask_file);
    }
  }


  data_size = 0;

  for (i = 0; i < wsize; i++) {
    double slitcen = 0;
    double sigma = 0;
    double area = 0;
    double offset = 0;
    double a = 0, b = 0, c = 0;
    double frac_rej = 0;

    cpl_matrix *covariance = NULL;
    double fit_err;
    int good_fit = 0;

    slit_vect_size = 0;
    
    /* get slit data */
    for (j = jmin; j < jmax; j++) {
      double val = 0, err = 0;
      int start, end;
      int ngood = 0, nbad = 0;

      /* running box hsize */
      start = i - box_hsize;
      end = i + box_hsize;

      if (start < 0) {
        start = 0;
      }
      if (end >= wsize) {
        end = wsize - 1;
      }

      for (k = start; k <= end; k++) {
        if ( (qual[k + j * wsize] & instrument->decode_bp) == 0 ) {
          val += flux[k + j * wsize];
          ngood++;
        } else {
          nbad++;
        }

      }
      
      val += flux[i+j*wsize];
      err = errs[i+j*wsize];
      
      if ( ngood > 0){ 
        val *= (double)(nbad+ngood)/(double)ngood;
	 
        slit_pos_data[slit_vect_size] = j;
        sliterr_vect_data[slit_vect_size] = err;
        slit_vect_data[slit_vect_size] = val;
       
        slit_vect_size++;
      }
    }

    frac_rej = 1-((double)slit_vect_size/(double)ssize);
    if ( frac_rej < 0.5){
      check( slit_pos = cpl_vector_wrap( slit_vect_size, slit_pos_data));
      check( slit_vect = cpl_vector_wrap( slit_vect_size, slit_vect_data));
      check( sliterr_vect = cpl_vector_wrap( slit_vect_size, sliterr_vect_data));
      /* smooth with median filter */
      check( smooth_vect = cpl_vector_filter_median_create( slit_vect, 
        smooth_hsize));

      /* gaussian fit */
#if CPL_MODE
      cpl_vector_fit_gaussian( slit_pos, NULL, slit_vect, sliterr_vect,
        CPL_FIT_ALL, &slitcen, &sigma, &area, &offset, &mse, NULL, &covariance);
      a = offset;
      fit_err = sqrt(cpl_matrix_get(covariance,0,0))*sstep;
      good_fit = (cpl_error_get_code() == CPL_ERROR_NONE);
#else
    {
      double init_par[6];
      double fit_errs[6];
      int status = 0;
      xsh_gsl_init_gaussian_fit( slit_pos, slit_vect, init_par);
      xsh_gsl_fit_gaussian( slit_pos, slit_vect, deg, init_par, fit_errs, &status);
      area = init_par[0];
      a = init_par[1];
      b = init_par[2];
      c = init_par[3];
      slitcen = init_par[4];
      sigma = init_par[5];
      offset = a+b*slitcen+c*slitcen*slitcen;
      fit_err = fit_errs[4]*sstep;
      good_fit= (status == 0);
    }
#endif

      if ( good_fit){
        double height=0;
        double snr=0;

        height = area / sqrt(2*M_PI*sigma*sigma);
        snr = (height+offset)/offset;

        /* clean bad fit */
        if ( (area > 0) && (slitcen > 0) && (slitcen < ssize) 
         && (sigma < ssize) && (snr > 0)){
            spos_data[data_size] = smin+slitcen*sstep;
            errpos_data[data_size] = fit_err;
            wpos_data[data_size] = wmin+i*wstep;
            sigma_data[data_size] = sigma*sstep;
            snr_data[data_size] = snr;
#if 0
	    /* DEBUG only */
	    if ( (wpos_data[data_size] >= (1500-wstep)) && (wpos_data[data_size] <= (1500+wstep))){
	      char test_name[256];
	      FILE* test_file = NULL;
	      int itest;
	      double dtest;
		 
	      sprintf( test_name, "data_w%f_%s.dat", wpos_data[data_size],resname);
	      XSH_REGDEBUG("Produce test file %s", test_name);
	      test_file = fopen( test_name, "w+");
	      fprintf( test_file, "# pos slit\n");
	      
	      for( itest=0; itest < slit_vect_size; itest++){
	        fprintf( test_file, "%f %f\n", cpl_vector_get( slit_pos, itest), cpl_vector_get( slit_vect, itest));
	      }
	      
	      fclose( test_file);
	      
	      sprintf( test_name, "gauss_w%f_%s.dat", wpos_data[data_size],resname);
	      XSH_REGDEBUG("Produce test file %s", test_name);
	      
	      test_file = fopen( test_name, "w+");
	      fprintf( test_file, "# pos gauss offset x0=%f_sig=%f_area=%f_offset=%f\n", slitcen, sigma, area, offset);	      

	      for( dtest=0; dtest < slit_vect_size; dtest+=0.1){
	        double z, gauss;
		double off;
		
		off = a+b*dtest+c*dtest*dtest;
		z = ( dtest-slitcen)/(sigma*XSH_MATH_SQRT_2);
		gauss = height*exp(-(z*z))+off;
		
	        fprintf( test_file, "%f %f %f\n", dtest, gauss, off);
	      }
	      
	      fclose( test_file);
	    }
#endif
	    data_size++;
        }
      }
      else{
        xsh_error_reset();
      }
      xsh_free_matrix( &covariance);
      xsh_free_vector( &smooth_vect);
      xsh_unwrap_vector( &slit_pos);
      xsh_unwrap_vector( &slit_vect);
      xsh_unwrap_vector( &sliterr_vect);
    }
  }
  

  /* sort sigma and snr */
  check( sigma_vect = cpl_vector_wrap( data_size, sigma_data));
  check( snr_vect = cpl_vector_wrap( data_size, snr_data));
  check( sigma_sort_vect = cpl_vector_duplicate( sigma_vect));
  check( snr_sort_vect = cpl_vector_duplicate( snr_vect));

  check( cpl_vector_sort( sigma_sort_vect, 1));
  check( cpl_vector_sort( snr_sort_vect, 1));

  n05 = (data_size-1)*cut_sigma_low;  
  i = ceil( n05);
  sigma_05 = cpl_vector_get( sigma_sort_vect, i);
  n05 = (data_size-1)*cut_snr_low;
  i = ceil( n05);
  snr_05 = cpl_vector_get( snr_sort_vect, i);


  n95 = (data_size-1)*cut_sigma_up;
  i = floor( n95);
  sigma_95 = cpl_vector_get( sigma_sort_vect, i);
  n95 = (data_size-1)*cut_snr_up;
  i = floor( n95);
  snr_95 = cpl_vector_get( snr_sort_vect, i);

  /* filter on snr and sigma */
  for( i=0; i< data_size; i++){
    double sigma, snr;

    sigma = sigma_data[i];
    snr = snr_data[i];
    if ( sigma_05 <= sigma && sigma <= sigma_95 && snr_05 <= snr && snr <=snr_95){
      wpos_data[ndata_size] = wpos_data[i];
      spos_data[ndata_size] = spos_data[i];
      errpos_data[ndata_size] = errpos_data[i];
      ndata_size++;
    }
  }
  xsh_msg_dbg_low( "Filtering by snr [%f,%f] and sigma [%f,%f] from %d lines to %d lines",
    sigma_05, sigma_95, snr_05, snr_95, data_size, ndata_size);

  /* REGDEBUG */
  if (level  >= XSH_DEBUG_LEVEL_MEDIUM){
    FILE *test_file = NULL;
    int itest;
    char test_name[256];
#if CPL_MODE		 
    sprintf( test_name, "cpl_gaussian_fit_%s.dat", resname);
#else
    sprintf( test_name, "gsl_gaussian_fit_%s.dat", resname);
#endif
    test_file = fopen( test_name, "w");
    fprintf( test_file, "# wavelength slit_fit fit_err sigma\n");

    for(itest=0; itest<ndata_size; itest++){
      fprintf( test_file, "%f %f %f %f\n", wpos_data[itest], spos_data[itest], errpos_data[itest], 
        sigma_data[itest]);
    }
    xsh_msg_dbg_medium( "Produce file %s", test_name);
    fclose( test_file);
  }

  check( wpos_vect = cpl_vector_new( wsize));
  check( spos_vect = cpl_vector_new( wsize));
  check( sposg_data = cpl_vector_get_data( spos_vect));
  check( wposg_data = cpl_vector_get_data( wpos_vect));
  j=0;  

  for(i=0; i< wsize; i++){
    double wave, wkeep;
    double slit;

    wave = wmin+i*wstep;
    wkeep = wpos_data[j];
    check( cpl_vector_set( wpos_vect, i, wave));
    if ( fabs(wave -wkeep) < 0.0000001){
      slit = spos_data[j];
      if (j < (ndata_size-1)){
        j++;
      }
    }
    else{
      slit = xsh_data_interpolate( wave, ndata_size, wpos_data, spos_data);
    }
    check( cpl_vector_set( spos_vect, i, slit));
  }

  check( decomp = xsh_atrous( spos_vect, nscales));

  nb_scales = nscales-HF_skip;

  /* filter High frequency */
  for( i=0; i< wsize; i++){
    sposg_data[i] = 0;
    for(j=0; j<nb_scales; j++){
      sposg_data[i] += cpl_matrix_get( decomp, j, i); 
    }
  }
  
  /* Save result in a table */
  check( table = cpl_table_new( wsize));
  XSH_TABLE_NEW_COL(table, XSH_OBJPOS_COLNAME_WAVELENGTH,
    XSH_OBJPOS_UNIT_WAVELENGTH, CPL_TYPE_DOUBLE);
  XSH_TABLE_NEW_COL(table, XSH_OBJPOS_COLNAME_SLIT,
    XSH_OBJPOS_UNIT_SLIT, CPL_TYPE_DOUBLE);

  for( i=0; i< wsize; i++){
    check( cpl_table_set_double( table, XSH_OBJPOS_COLNAME_WAVELENGTH,
                    i, wposg_data[i]));
    check( cpl_table_set_double( table, XSH_OBJPOS_COLNAME_SLIT,
                    i, sposg_data[i]));
  }
  sprintf( tablename, resname);
  header = cpl_propertylist_new();
  check( cpl_table_save( table, header, NULL, tablename, CPL_IO_DEFAULT));
  /* Create the frame */
  check(result=xsh_frame_product( tablename,
                                  "OBJPOS_TAB",
                                   CPL_FRAME_TYPE_TABLE,
                                   CPL_FRAME_GROUP_PRODUCT,
                                   CPL_FRAME_LEVEL_TEMPORARY));


  check (xsh_add_temporary_file( tablename));

  cleanup:
    XSH_TABLE_FREE( table);
    xsh_free_propertylist( &header);
    xsh_free_matrix( &decomp);
    xsh_free_vector( &spos_vect);
    xsh_free_vector( &wpos_vect);
    xsh_free_vector( &sigma_sort_vect);
    xsh_free_vector( &snr_sort_vect);
    xsh_unwrap_vector( &sigma_vect);
    xsh_unwrap_vector( &snr_vect);
    XSH_FREE( sigma_data);
    XSH_FREE( snr_data);
    XSH_FREE( spos_data);
    XSH_FREE( errpos_data);
    XSH_FREE( wpos_data);
    XSH_FREE( slit_pos_data);
    XSH_FREE( slit_vect_data);
    XSH_FREE( sliterr_vect_data);
    xsh_free_vector( &smooth_vect);
    xsh_spectrum_free( &spectrum2d);
    XSH_FREE(sky_mask);
    xsh_free_table( &skymask_table);
    return result;
}
/*---------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*/
/**
   @brief Localize center of object on a merge 2D IFU slitlet
   @param[in] merge2d_slitlet merge 2D IFU slitlet
   @param[in] smooth_hsize Half size of median filter using for smoothing
   @param[in] nscales nscales use for atrous decomposition
   @param[in] HF_skip Number of skipping High Frequency used to reconstruct 
              signal
   @param[in] prefix  of result table
   @return frame containing for Wavelength the position of object center 
           on the slit
*/
cpl_frameset* xsh_localize_ifu( cpl_frameset *merge2d_frameset, 
  cpl_frame *skymask_frame,
  xsh_localize_ifu_param * locifu_par, xsh_instrument *instrument, 
  const char* prefix)
{
  int i, slitlet;
  cpl_frameset *result_frameset = NULL;
  char fname[256];
  int smooth_hsize;
  int nscales;
  int HF_skip;
  double cut_sigma_low;
  double cut_sigma_up;
  double cut_snr_low;
  double cut_snr_up;
  double slit_min = -6.0;
  double slit_max = 6.0;
  cpl_frame *frame = NULL;
  const char *frame_name = NULL;
  cpl_propertylist *header = NULL;
  int deg = 2, skymask=0;
  int box_hsize;
  cpl_frame *mask_frame = NULL;

  XSH_ASSURE_NOT_NULL( merge2d_frameset);
  XSH_ASSURE_NOT_NULL( instrument);
  XSH_ASSURE_NOT_NULL( locifu_par);

  smooth_hsize = locifu_par->smooth_hsize;
  nscales = locifu_par->nscales;
  HF_skip = locifu_par->HF_skip;
  cut_sigma_low = locifu_par->cut_sigma_low;
  cut_sigma_up = locifu_par->cut_sigma_up;
  cut_snr_low = locifu_par->cut_snr_low;
  cut_snr_up = locifu_par->cut_snr_up;
  skymask = locifu_par->use_skymask;
  box_hsize = locifu_par->box_hsize;
  /* get slit borders */
  if (skymask){
    mask_frame = skymask_frame;
  }
  
  check( frame = cpl_frameset_get_frame( merge2d_frameset, 0));
  check( frame_name = cpl_frame_get_filename( frame));
  check( header = cpl_propertylist_load( frame_name, 0));
  check( slit_min = xsh_pfits_get_rectify_space_min( header));
  xsh_free_propertylist( &header);

  check( frame = cpl_frameset_get_frame( merge2d_frameset, 2));
  check( frame_name = cpl_frame_get_filename( frame));
  check( header = cpl_propertylist_load( frame_name, 0));
  check( slit_max = xsh_pfits_get_rectify_space_max( header));
  xsh_free_propertylist( &header);

  slit_min += locifu_par->slitlow_edges_mask;
  slit_max -= locifu_par->slitup_edges_mask;

  deg = locifu_par->bckg_deg;
  
  check( result_frameset = cpl_frameset_new());

  for( i = 0, slitlet = LOWER_IFU_SLITLET ; i < 3 ; i++, slitlet++ ) {
    cpl_frame * loc_frame = NULL;
    cpl_frame *merge2d_frame = NULL;

    sprintf( fname ,"%s_LOCIFU_%s_%s.fits", prefix, SlitletName[slitlet],
      xsh_instrument_arm_tostring( instrument));

    xsh_msg( "Localizing IFU in [%f,%f] slitlet %s, frame '%s'", slit_min, slit_max,
      SlitletName[slitlet], fname);

    check( merge2d_frame = cpl_frameset_get_frame( merge2d_frameset, i));


    check( loc_frame = xsh_localize_ifu_slitlet( merge2d_frame, mask_frame,
      smooth_hsize, nscales, 
      HF_skip, fname, cut_sigma_low, cut_sigma_up, cut_snr_low, cut_snr_up, 
      slit_min, slit_max, deg, box_hsize, instrument));

    check( cpl_frameset_insert( result_frameset, loc_frame));
  }

 cleanup:
  if ( cpl_error_get_code() != CPL_ERROR_NONE ) {
    xsh_free_frameset( &result_frameset);
    xsh_free_propertylist( &header);
  }
  return result_frameset;
}
/*---------------------------------------------------------------------------*/
/**@}*/
