/*                                                                           *
 *   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: 2013-03-18 11:39:42 $
 * $Revision: 1.96 $
 */

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

/*---------------------------------------------------------------------------*/
/**
 * @defgroup xsh_detect_continuum      Continuum Detection (xsh_detect_continuum)
 * @ingroup drl_functions
 *
 * Functions used to detect orders and compute polynomial description of
 * min and max edges (in recipe xsh_orderpos)
 */
/*---------------------------------------------------------------------------*/
/**@{*/

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

#include <math.h>
#include <xsh_drl.h>
#include <xsh_data_order.h>
#include <xsh_error.h>
#include <xsh_utils_wrappers.h>
#include <xsh_utils.h>
#include <xsh_msg.h>
#include <xsh_data_pre.h>
#include <xsh_pfits.h>
#include <xsh_parameters.h>
#include <xsh_data_order_resid_tab.h>
#include <cpl.h>

/*---------------------------------------------------------------------------
  Typedefs
  ---------------------------------------------------------------------------*/

struct gauss_res {
  double centroid,
    sigma,
    area,
    offset,
    mse ;
} ;

typedef struct {
  int order_idx ;
  int absorder ;
  int flag;     // flag outliers
  double position ;	/**< Pos. in Y */
  double pos_x ;		/**< initial position in X */
  double centervalue ;  /**< Pos in X after gaussian fit */
  double sigmavalue ;	/**< Sigma of the fit */
  double final_x ;		/**< X position from final poly */
} CENTROIDS_STRUCT ;

/*---------------------------------------------------------------------------
  Functions prototypes
  ---------------------------------------------------------------------------*/
static cpl_error_code 
xsh_fit_gaussian( xsh_detect_continuum_param * detect_param,
		  int x, int y, int nx,
		  float *data, float *p_errs,
		  struct gauss_res *result,
		  xsh_instrument *instrument,
		  int order, double threshold ) ;

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

static int Nb_noflux, Nb_nofit ;

/* The BAD_POSITION_FLAG is used to flag the positions where the
   gaussian fit failed.
   Used in the sigma clipping
*/
/*
  TBD: even with a rather large window, even if the maximum is inside the
  window, sometimes the fit does not find correctly the maximum.
  Probably because the maximum is too close to the edge of the window.
  A possible solution would be to use 2 windows:
  a "large" one to find the maximum.
  a smaller one, centered on the maximum, to make the fit.
*/
#define BAD_POSITION_FLAG 999999
#define THRESH_POSITION_FLAG 9999999


/* Using  FITS pixel convention */
static cpl_error_code 
xsh_fit_gaussian( xsh_detect_continuum_param * detect_param,
		  int x, int y, int nx,
		  float *data, float *p_errs,
		  struct gauss_res * result,
		  xsh_instrument *instrument,
		  int order, double threshold )
{
  cpl_vector *v_pos = NULL ;
  cpl_vector *v_values = NULL ;
  cpl_error_code err = CPL_ERROR_NONE ;
  int l, nelem ;
  int x0 ;
  double flux=0.0, min_flux=0.0;
  int search_window ;
  int running_window ;
  int fit_window ;
  int x1, x2 ;
  double *warray = NULL ;
  int pix_shift=(y-1)*nx-1;

  /* check input parameters */
  XSH_ASSURE_NOT_NULL( detect_param);
  XSH_ASSURE_NOT_NULL( data);
  XSH_ASSURE_NOT_NULL( p_errs);
  XSH_ASSURE_NOT_NULL( result);
  XSH_ASSURE_NOT_NULL( instrument);

  search_window = detect_param->search_window ;
  fit_window = detect_param->fit_window ;
  running_window = detect_param->running_window ;

  /* Create temporary vectors */
  //  xsh_msg_dbg_high( "Entering xsh_fit_gaussian: X,Y = %d,%d (%d elements)",
  //	 x, y, nelem ) ;
  x0 = x - search_window;
  x2 = x + search_window;
  if ( x0 < 1 ) x0 = 1;
  if ( x2 > nx ) x2 = nx;
  nelem = x2 - x0 +1 ;
  x1 = x0 ;
  /*
    Find maximum flux in large window using the running window:
    ==> the maximum is the maximum of the medians in all the running windows.
  */
  if ( running_window != 0 ) {
    XSH_MALLOC( warray, double, nelem);
    for( l = 0 ; l<nelem ; l++, x0++ )
      warray[l] = data[x0+pix_shift];

    // Center of the fit window is at the maximum found
    x0 = x1 + xsh_tools_running_median_1d_get_max( warray, nelem, 
						   running_window);
    xsh_msg_dbg_high( "   Found running maximum at %d,%d (was %d)",
      x0, y, x );
    XSH_FREE(warray) ;

    /* build vector centered on the maximum, using a smaller window */
    nelem = (fit_window*2)+1 ;
    x0 -= fit_window ;
  }


  xsh_msg_dbg_high( " Final fit window: %d,%d %d elements", x0, y, nelem ) ;
  check( v_pos = cpl_vector_new( nelem ) ) ;
  check( v_values = cpl_vector_new( nelem ) ) ;

  /* Fill vectors */
  for( l = 0 ; l<nelem ; l++, x0++ ) {
    cpl_vector_set( v_pos, l, (double)x0 ) ;
    cpl_vector_set( v_values, l, (double)data[x0+pix_shift] ) ;
  }

  err = cpl_vector_fit_gaussian( v_pos, NULL, v_values, NULL,
				 CPL_FIT_ALL,
				 &result->centroid, &result->sigma,
				 &result->area, &result->offset,
				 &result->mse, NULL, NULL ) ;
  if ( err == CPL_ERROR_NONE ) {
    x0 = floor( result->centroid +0.5);
    flux = (double)data[x0+pix_shift] ;
    //xsh_msg( "    Fitted position: %d, Flux: %lf", x0, flux ) ;
    min_flux = (double)p_errs[x0+pix_shift] * threshold ;
    if ( flux < min_flux ) {
      err = CPL_ERROR_UNSPECIFIED ;
    }
  }

  if ( err != CPL_ERROR_NONE ) {
    int i ;

    if ( err == CPL_ERROR_UNSPECIFIED ) {
      xsh_msg_dbg_medium( "  NOT ENOUGH FLUX at X,Y = %d,%d (%lf < %lf)",
			x, y, flux, min_flux ) ;
      Nb_noflux++ ;
    }
    else {
      cpl_error_reset() ;
      /* Dump values */
      xsh_msg_dbg_medium( "    CPL_FIT_GAUSSIAN ERROR at X,Y = %d,%d", x, y ) ;
      Nb_nofit++ ;
      for( i = 0 ; i<nelem ; i++ ) {
	xsh_msg_dbg_high( "         X = %.0lf, Value = %lf",
			  cpl_vector_get( v_pos, i ),
			  cpl_vector_get( v_values, i ) ) ;
      }
    }
  }
  else if ( xsh_debug_level_get() >= XSH_DEBUG_LEVEL_MEDIUM &&
	    (y % 10) == 0 ) {
    /* Save data to file (only 1/10 of Y points) */
    char dirname1[128], dirname2[128], fname[256], cmd[256] ;
    FILE *fout ;

    sprintf( dirname1, "Ord_%s", xsh_instrument_arm_tostring( instrument ) ) ;
    if ( access( dirname1, 0 ) != 0 ) {
      sprintf( cmd, "mkdir %s", dirname1 ) ;
      system( cmd ) ;
    }
    sprintf( dirname2, "%s/O_%02d", dirname1, order ) ;
    if ( access( dirname2, 0 ) != 0 ) {
      sprintf( cmd, "mkdir %s", dirname2 ) ;
      system( cmd ) ;
    }

    sprintf( fname, "%s/fit_%04d", dirname2, y ) ;
    fout = fopen( fname, "w" ) ;
    for( l = 0 ; l<nelem ; l++ ) {
      fprintf( fout, "%lf %lf %lf %lf\n", cpl_vector_get( v_pos, l ),
	       cpl_vector_get( v_values, l ),
	       result->centroid, result->sigma ) ;
    }
    fclose( fout ) ;
  }

  cleanup:
    xsh_free_vector( &v_pos ) ;
    xsh_free_vector( &v_values ) ;
    return err ;
}

static double * Deltas = NULL, * PosY = NULL, * PosX = NULL,
  * XorderPos = NULL ;
static int * Orders = NULL ;
static int DeltaSize = 0 ;
static int DeltaPoints = 0 ;

static cpl_frame* 
create_resid_tab( xsh_instrument * instrument,
			      ORDERPOS_QC_PARAM * qcparam )
{
  int dbg_lvl ;
  FILE * flog = NULL ;
  char logname[256];
  int n ;
  cpl_frame * resid_frame = NULL ;
  xsh_resid_order_tab * resid_tab = NULL ;
  char frame_name[256] ;
  char tag[256];

  XSH_ASSURE_NOT_NULL( instrument ) ;

  if ( (dbg_lvl = xsh_debug_level_get()) >= XSH_DEBUG_LEVEL_LOW  ) {
    // Output ASCII file
    // First open output file
     sprintf(logname,"orderpos_%s.dat",xsh_instrument_arm_tostring(instrument));
    if ( (flog = fopen( logname, "w" ) ) == NULL )
      xsh_msg( "Cant open log file \"%s\"", logname ) ;
    else xsh_msg( " ASCII File '%s'", logname ) ;
  }

  for ( n = 0 ; n<DeltaPoints ; n++ ) {
    if ( dbg_lvl >= XSH_DEBUG_LEVEL_LOW && flog != NULL )
      fprintf( flog, "%d %lf %lf %lf %lf\n",
	       *(Orders+n), *(PosY+n), *(PosX+n),
	       *(XorderPos+n), *(Deltas+n) ) ;
  }

  check( resid_tab = xsh_resid_order_create( DeltaPoints, Orders, PosX, PosY,
					     Deltas, XorderPos ) ) ;
  resid_tab->residmin = qcparam->residmin ;
  resid_tab->residmax = qcparam->residmax ;
  resid_tab->residavg = qcparam->residavg ;
  resid_tab->residrms = qcparam->residrms ;

  sprintf(tag,"%s%s%s",XSH_ORDERPOS_RESID_TAB,"_",
			 xsh_instrument_arm_tostring( instrument ));
  sprintf(frame_name,"%s.fits",tag);

  check( resid_frame = xsh_resid_order_save( resid_tab, frame_name,
					     instrument,qcparam,tag ) ) ;

 cleanup:
  xsh_resid_order_free(&resid_tab);
 
  if(cpl_error_get_code() != CPL_ERROR_NONE) {
    xsh_free_frame(&resid_frame);
  }
  if(flog != NULL) {
    fclose(flog);
  }
  return resid_frame;

}

/* the following generate a leak */
static void cumulate_qc_parameter( int order, xsh_order_list * list,
				   CENTROIDS_STRUCT * pcent, int npts )
{
  int j, absorder ;
  double diff ;
  double * Delta0 ;
  int prevSize ;

  absorder = list->list[order].absorder ;
  xsh_msg_dbg_low( " Cumulate: Order %d, Nb of Points: %d", absorder, npts ) ;
  prevSize = DeltaSize ;

  DeltaSize += npts ;
  if ( order == 0 ) {
    XSH_CALLOC( Deltas, double, DeltaSize ) ;
    XSH_CALLOC( PosX, double, DeltaSize ) ;
    XSH_CALLOC( PosY, double, DeltaSize ) ;
    XSH_CALLOC( Orders, int, DeltaSize ) ;
    XSH_CALLOC( XorderPos, double, DeltaSize ) ;
  }
  else {
    //XSH_REALLOC( Deltas, double, DeltaSize ) ;
    Deltas = cpl_realloc( Deltas, DeltaSize*sizeof(double) ) ;
    PosX = cpl_realloc( PosX, DeltaSize*sizeof(double) ) ;
    PosY = cpl_realloc( PosY, DeltaSize*sizeof(double) ) ;
    Orders = cpl_realloc( Orders, DeltaSize*sizeof(int) ) ;
    XorderPos = cpl_realloc( XorderPos, DeltaSize*sizeof(double) ) ;
  }
  xsh_msg_dbg_low( "Cumulated points: %d", DeltaSize ) ;
  Delta0 = Deltas + prevSize ;


  for( j = 0 ; j<npts ; j++, pcent++ ) {
    double x1 ;
    double y ;

    y = pcent->position ;
    x1 = cpl_polynomial_eval_1d( list->list[order].cenpoly, y, NULL ) ;
    diff= ( x1 - pcent->centervalue ) ;

    xsh_msg_dbg_medium( "    ---- Y = %lf, x0 = %lf, x1 = %lf, diff = %lf", y,
		     pcent->centervalue, x1, diff ) ;
    *(Deltas+DeltaPoints) = diff ;
    *(PosX+DeltaPoints) = pcent->centervalue ;
    *(PosY+DeltaPoints) = y ;
    *(Orders+DeltaPoints) = absorder ;
    *(XorderPos+DeltaPoints) = x1 ;
    DeltaPoints++ ;
  }
  {
    // Residuals per order
    cpl_vector *v_avg = NULL ;
    double residavg, residmax, residmin, residrms ;
    v_avg = cpl_vector_wrap( npts, Delta0 ) ;

    residavg = cpl_vector_get_mean( v_avg ) ;
    residmax = cpl_vector_get_max( v_avg ) ;
    residmin = cpl_vector_get_min( v_avg ) ;
    residrms = cpl_vector_get_stdev( v_avg ) ;
    xsh_msg_dbg_low( " Residuals for Order %d:", absorder ) ;
    xsh_msg_dbg_low( "     Mean: %lf, Max: %lf, Min: %lf, Rms: %lf",
		     residavg, residmax, residmin, residrms ) ;
    xsh_msg_dbg_low( "   StartY: %d, Endy: %d", list->list[order].starty,
		     list->list[order].endy ) ;
    cpl_vector_unwrap( v_avg ) ;
  }

 cleanup:
  return ;
}

static cpl_frame* 
calculate_qc_parameters( ORDERPOS_QC_PARAM * qcparam,
				     xsh_instrument * instrument )
{
  cpl_vector *v_avg = NULL ;
  cpl_frame* resid_frame=NULL;

  check(v_avg = cpl_vector_wrap( DeltaPoints, Deltas )) ;
  assure( v_avg != NULL, cpl_error_get_code(),
	  "Cant wrap the v_avg vector" ) ;

  qcparam->residavg = cpl_vector_get_mean( v_avg ) ;
  qcparam->residmax = cpl_vector_get_max( v_avg ) ;
  qcparam->residmin = cpl_vector_get_min( v_avg ) ;
  qcparam->residrms = cpl_vector_get_stdev( v_avg ) ;
  check(resid_frame=create_resid_tab( instrument, qcparam ) );

 cleanup:
  cpl_vector_unwrap( v_avg ) ;
  XSH_FREE( Deltas ) ;
  XSH_FREE( PosX ) ;
  XSH_FREE( PosY ) ;
  XSH_FREE( Orders ) ;
  XSH_FREE( XorderPos ) ;
  if(cpl_error_get_code() != CPL_ERROR_NONE) {
    xsh_free_frame(&resid_frame);
  }
  return resid_frame;

}

static int comp_pos( const void * un, const void * deux )
{
  CENTROIDS_STRUCT * one = (CENTROIDS_STRUCT *)un ;
  CENTROIDS_STRUCT * two = (CENTROIDS_STRUCT *)deux ;

  if ( one->position < two->position ) return -1 ;
  else if ( one->position == two->position ) return 0 ;
  else return 1 ;
}

static void set_qc_parameters( xsh_order_list * list, ORDERPOS_QC_PARAM *pqc,
			       xsh_instrument * instrument )
{
  cpl_propertylist * header=NULL ;

  XSH_ASSURE_NOT_NULL(list);
  XSH_ASSURE_NOT_NULL(pqc);

  header = xsh_order_list_get_header( list ) ;
  XSH_ASSURE_NOT_NULL( header );

  check( xsh_pfits_set_qc( header, &pqc->residmin,
			   QC_ORD_ORDERPOS_RESIDMIN,
			   instrument ) ) ;
  check( xsh_pfits_set_qc( header, &pqc->residmax,
			   QC_ORD_ORDERPOS_RESIDMAX,
			   instrument ) ) ;
  check( xsh_pfits_set_qc( header, &pqc->residavg,
			   QC_ORD_ORDERPOS_RESIDAVG,
			   instrument ) ) ;
  check( xsh_pfits_set_qc( header, &pqc->residrms,
			   QC_ORD_ORDERPOS_RESIDRMS,
			   instrument ) ) ;
  check( xsh_pfits_set_qc( header, &pqc->ndet,
			   QC_ORD_ORDERPOS_NDET,
			   instrument ) ) ;
  check( xsh_pfits_set_qc( header, &pqc->npred,
			   QC_ORD_ORDERPOS_NPRED,
			   instrument ) ) ;
  /*
    check( xsh_pfits_set_qc( header, &pqc->max_pred,
    QC_ORD_ORDERPOS_MAX_PRED,
    instrument ) ) ;
    check( xsh_pfits_set_qc( header, &pqc->min_pred,
    QC_ORD_ORDERPOS_MIN_PRED,
    instrument ) ) ;
    check( xsh_pfits_set_qc( header, &pqc->nposall,
    QC_ORD_ORDERPOS_NPOSALL,
    instrument ) ) ;
    check( xsh_pfits_set_qc( header, &pqc->npossel,
    QC_ORD_ORDERPOS_NPOSSEL,
    instrument ) ) ;
  */


 cleanup:
 
  return ;
}

/** 
    @brief
    Detect order and compute polynomial description of ordermin and 
    order max. Uses a guess order table if available. If not, tries to compute
    one (see function xsh_order_table_from_fmtchk).
 
    @param[in] frame
    The frame which contains data
    @param[in] order_table
    The table data with theorical description of polynomial coefficient of 
    centroid order or NULL
    @param[in] spectral_frame
    The spectral format table frame or NULL
    @param[in] detect_param
    The detect continuum parameters
    @param[in] dcn_clipping 
    Pointer to sigma clipping parameters
    @param[in] instr 
    The instrument settings
    @param[out] resid_frame table frame with measured residuals
    @return
    Frame containing order table
*/
/*---------------------------------------------------------------------------*/
cpl_frame* xsh_detect_continuum( cpl_frame *frame, cpl_frame *order_table,
				 cpl_frame *spectral_frame,
				 xsh_detect_continuum_param *detect_param,
				 xsh_clipping_param *dcn_clipping,
				 xsh_instrument *instr, 
                                 cpl_frame** resid_frame)
{
  /* MUST BE DEALLOCATED in caller */
  cpl_frame *result = NULL;
  /* ALLOCATED locally */
  xsh_pre *pre = NULL;		/**< PRE structure loaded from input frame */
  xsh_order_list *list = NULL;
  xsh_order_list *guess_list = NULL;
  CENTROIDS_STRUCT *Centroids = NULL;
  cpl_vector *centers = NULL ;
  cpl_vector *xpos = NULL ;
 
  /* Others */
  float *data = NULL;		/**< Array of pixel values input image */
  float *p_errs = NULL;	/**< Array of pixel errors input image */
  int y = 0;
  int i = 0;
  int ndetected = 0 ;		/**< Nb of detected orders */
  ORDERPOS_QC_PARAM ord_qc_param ;
  static FILE *flog = NULL ;
  int step, degree;
  double fit_threshold ;

  /* check input */
  XSH_ASSURE_NOT_NULL( frame);
  XSH_ASSURE_NOT_NULL( order_table);
  XSH_ASSURE_NOT_NULL( detect_param);
  XSH_ASSURE_NOT_NULL( dcn_clipping);
  XSH_ASSURE_NOT_NULL( instr);

  /* Running window may be zero */
  assure(detect_param->search_window > 0 &&
	 detect_param->fit_window > 0, CPL_ERROR_NULL_INPUT,
	 "Windows must be > 0" ) ;

  /* parameters */
  fit_threshold = detect_param->fit_threshold ;
  degree = detect_param->poly_degree ;

  /* Showing parameters */
  xsh_msg_dbg_medium("Clipping parameters :");
  xsh_msg_dbg_medium("  Sigma %f Niter %d Frac %f", dcn_clipping->sigma,
    dcn_clipping->niter, dcn_clipping->frac);

  /* load the frames */
  check( pre = xsh_pre_load( frame, instr));
  check( list = xsh_order_list_load (order_table, instr));

  assure(detect_param->search_window > 0 &&
         detect_param->search_window < (pre->nx/list->size), 
         CPL_ERROR_ILLEGAL_INPUT,
	 "Search windows half size %d must be > 0  and < %d change value of "
         "parameter detectcontinuum-search-win-hsize", 
         detect_param->search_window ,(pre->nx/list->size)) ;

  check( xsh_order_list_verify( list, xsh_pre_get_ny( pre)));
  /* Save the guessed order list (used to calculate QC parameters) */
  check( guess_list = xsh_order_list_load( order_table, instr));

  /* clean header */
  xsh_free_propertylist( &(list->header)); 
  XSH_NEW_PROPERTYLIST( list->header); 

  XSH_ASSURE_NOT_NULL( guess_list);

  /* get the data */ 
  check( data = cpl_image_get_data_float(pre->data));
  check( p_errs = cpl_image_get_data_float( pre->errs));

  /* Create/initialize temporary array (maximum size) */
  XSH_MALLOC( Centroids, CENTROIDS_STRUCT, pre->ny);

  /* detect order continuum */
  /* Loop over all orders */
  xsh_msg_dbg_high( "Loop over %d Orders", list->size);

  step = detect_param->poly_step ;


  for(i=0; i< list->size; i++){
    int x = 0 ;
    double fx ;
    int npts = 0, totpts = 0, npos, nn, mm, niter, totnbad = 0 ;
    cpl_error_code err ;
    int low_y=0, up_y=0;
    int ycenter ;
    int absorder ;

    Nb_nofit = 0 ;
    Nb_noflux = 0 ;
    absorder = list->list[i].absorder ;
    /* Calculate at the center */
    ycenter = pre->ny/2;

    if ( ycenter < list->list[i].starty ) ycenter = list->list[i].starty;
    /* Default position of the center from input order table */
    check( fx = cpl_polynomial_eval_1d(list->list[i].cenpoly,
				       (double)ycenter,NULL));
    /* rounding x value */
    x = floor( fx+0.5);

    /* X, Y using FITS convention */
     
    xsh_msg_dbg_medium( ">>>> Order #%d, At Center X,Y: %d,%d",
	     absorder, x, ycenter ) ;

    /* VALID X,Y coordinate*/
    if ( x >= 1 && x <= pre->nx) {
      struct gauss_res fit_result ;

      /* Fit Gaussian */
      err = xsh_fit_gaussian( detect_param, x, ycenter,
			      pre->nx, data, p_errs,
			      &fit_result,
			      instr, absorder,
			      fit_threshold );

      if ( err == CPL_ERROR_NONE ) {
	/* And now store the result of the fit somewhere (?) */
	(Centroids+npts)->position = (double) ycenter ;
	(Centroids+npts)->pos_x = (double) x;
	(Centroids+npts)->centervalue = fit_result.centroid ;
	(Centroids+npts)->sigmavalue = fit_result.sigma ;
	(Centroids+npts)->absorder = absorder ;
	npts++ ;
      }
      totpts++ ;
    }

    /* Loop over all Y pixels starting from center to starty */
    xsh_msg_dbg_medium( "    Down Y from %d to %d",
	     ycenter -1, list->list[i].starty ) ;
    low_y = ycenter ;
    for( y = ycenter - 1; y >= list->list[i].starty ; y-=step ) {

      check( fx=cpl_polynomial_eval_1d(list->list[i].cenpoly,
                                           (double)y,NULL));
      x = floor( fx+0.5) ;
      /* VALID X,Y coordinate*/
      if ( x >= 1 && x <= pre->nx) {
	struct gauss_res fit_result ;

	/* Fit Gaussian */
	check_msg(err = xsh_fit_gaussian( detect_param, x, y,
				pre->nx, data, p_errs,
				&fit_result,
				instr, list->list[i].absorder,
                                          fit_threshold ),"Try to increase detectcontinuum-fit-win-hsize") ;
	if ( err == CPL_ERROR_NONE ) {
	  /* And now store the result of the fit somewhere (?) */
	  (Centroids+npts)->position = (double)y ;
	  (Centroids+npts)->pos_x = (double)x;
	  (Centroids+npts)->centervalue = fit_result.centroid ;
	  (Centroids+npts)->sigmavalue = fit_result.sigma ;
	  (Centroids+npts)->absorder = absorder ;
	  low_y = y ;
	  npts++ ;
	} else {
           cpl_error_reset();
        }
	totpts++ ;
      }
    }

    xsh_msg_dbg_medium( " --> Nb of points: %d", npts ) ;
    /* Loop over all Y pixels starting from center to endy */
    xsh_msg_dbg_medium( "    Up Y from %d to %d",
	     ycenter+1, list->list[i].endy ) ;
    up_y = ycenter ;
    for( y = ycenter + 1; y <= list->list[i].endy ; y+=step ) {
      check( fx=cpl_polynomial_eval_1d(list->list[i].cenpoly,
				       (double)y,NULL));
      x = floor( fx+0.5);
      /* VALID X,Y coordinate*/
      if ( x >= 1 && x <= pre->nx) {
	struct gauss_res fit_result ;

	/* Fit Gaussian */
	err = xsh_fit_gaussian( detect_param, x, y,
				pre->nx, data, p_errs,
				&fit_result,
				instr, list->list[i].absorder,
				fit_threshold ) ;
	if ( err == CPL_ERROR_NONE ) {
	  /* And now store the result of the fit somewhere */
	  (Centroids+npts)->position = (double)y ;
	  (Centroids+npts)->pos_x = (double)x ;
	  (Centroids+npts)->centervalue = fit_result.centroid ;
	  (Centroids+npts)->sigmavalue = fit_result.sigma ;
	  (Centroids+npts)->absorder = absorder ;
	  up_y = y ;
	  npts++ ;

	} else {
           cpl_error_reset();
        }
	totpts++ ;
      }
    }

    if (xsh_debug_level_get() >= XSH_DEBUG_LEVEL_HIGH) {
      FILE* debug_file = NULL;
      char debug_name[256];
      int idebug;
      
      sprintf(debug_name, "centroid_%d.reg", list->list[i].absorder);

      debug_file = fopen( debug_name, "w");
      fprintf( debug_file, "# Region file format: DS9 version 4.0\n"\
        "global color=red font=\"helvetica 4 normal\""\
        "select=1 highlite=1 edit=1 move=1 delete=1 include=1 fixed=0 "\
        "source\nimage\n");
      for( idebug=0; idebug < npts; idebug++){
        fprintf( debug_file, "point(%f,%f) #point=cross color=blue\n", 
          Centroids[idebug].pos_x, Centroids[idebug].position);
        fprintf( debug_file, "point(%f,%f) #point=cross color=red\n",
          Centroids[idebug].centervalue, Centroids[idebug].position); 
      }
      fclose( debug_file); 
    }
    xsh_msg_dbg_medium( " --> Nb of points: %d", npts ) ;
    xsh_msg_dbg_low( "      No enough flux: %d, No fit: %d", Nb_noflux,
		     Nb_nofit ) ;

    xsh_msg_dbg_high( "     Nb of correct points found: %d/%d",
			 npts, totpts ) ;
    xsh_msg_dbg_high( "   LowY: %d, UpY: %d", low_y, up_y ) ;


    assure(npts > 3,CPL_ERROR_ILLEGAL_OUTPUT,
	   "can't fit polynomial solution with nb of points %d", npts );

    /*
      Now ready to fit a polynomial.
      But first, order data by positions !
    */
    qsort( Centroids, npts, sizeof( CENTROIDS_STRUCT ), comp_pos ) ;

    /********* Sigma clipping
	       After fit polynome, suppress "bad" positions
	       then refit the polynomial
    **********/
    xsh_msg( "Sigma Clipping on residuals over %d pts", npts ) ;

    npos = npts ;
    //First remove too large outliers
    {
      double poly_rms ;
      int nthresh = 0 ;
      /* put the start and end of order in Y */
      list->list[i].starty = low_y ;//Centroids->position ;
      list->list[i].endy = up_y ; //(Centroids+npos-1)->position ;

      /* create vector from values */
      check( xpos = cpl_vector_new( npos ) ) ;
      check( centers = cpl_vector_new( npos ) ) ;
      {
	int j ;
	for( j = 0 ; j<npos ; j++ ) {
	  cpl_vector_set( xpos, j, (Centroids+j)->position ) ;
	  cpl_vector_set( centers, j, (Centroids+j)->centervalue ) ;
	}
      }

      /* create polynomial solutions */
      check(xsh_free_polynomial( &list->list[i].cenpoly));

      xsh_msg_dbg_low( "Polynomial degree: %d", degree ) ;
      check(list->list[i].cenpoly = 
	    xsh_polynomial_fit_1d_create( xpos, centers, degree, &poly_rms));
      xsh_free_vector( &centers ) ;
      xsh_free_vector( &xpos ) ;



      
      // Now sigma clipping outliers aboth fixed threshold
      
      poly_rms = sqrt( poly_rms ) ;
      xsh_msg_dbg_low( "       Sigma Clipping - Polynomial RMS: %lf",
		       poly_rms ) ;

      for( nn = 0 ; nn < npos ; nn++ ) {
	/* Compute distance between Centroids->xpos and poly value at x
	   with rms of polynomial
	*/
	double xpoly, delta ;

	xpoly = cpl_polynomial_eval_1d( list->list[i].cenpoly,
					(Centroids+nn)->position,
					NULL ) ;
	delta = fabs( xpoly - (Centroids+nn)->centervalue) ;
	if ( delta > dcn_clipping->res_max ) {
	  /* Reject this position (set the position at an 
	     impossible value) */
	  xsh_msg_dbg_high( "        Rejected at %.0lf,%.0lf (%lf > %lf)",
			    (Centroids+nn)->centervalue, 
			    (Centroids+nn)->position,
			    delta, poly_rms ) ;
	  (Centroids+nn)->position = THRESH_POSITION_FLAG ;
	  (Centroids+nn)->flag = THRESH_POSITION_FLAG ;
	  nthresh++ ;
	}
      }



      totnbad += nthresh ;
      xsh_msg_dbg_low( "       Rejected Positions: %d (%d)", nthresh, totnbad ) ;

      /*
	if too many rejected positions, discard the order
      */
      if ( ((float)(npts-totnbad)/(float)npts) < dcn_clipping->frac ) {
	/* Tag the order as undetected and break */
	list->list[i].starty = -1 ;
	list->list[i].endy = -1 ;
	xsh_msg_warning( "**** Too many rejected points: %d/%d (%.2f) Discard Order %d",
		 totnbad, npts, (float)(npts-totnbad)/(float)npts, absorder ) ;
        xsh_msg_warning("Try to increase detectcontinuum-clip-res-max and/or decrease detectcontinuum-clip-frac and/or detectcontinuum-search-win-hsize"); 
	break ;
      }

      /* Reorder Centroids (suppress bad positions) */
      for( nn = 0, mm = 0 ; nn<npos ; nn++ ) {
	if ( (Centroids+nn)->position != THRESH_POSITION_FLAG ) {
	  if ( mm != nn ) *(Centroids+mm) = *(Centroids+nn) ;
	  mm++ ;
	}
      }
      npts = mm ;
    }

    npos = npts ;
    for( niter = 0 ; niter < dcn_clipping->niter ; niter++ ) {
      double poly_rms ;
      int nbad = 0 ;

      /* put the start and end of order in Y */
      list->list[i].starty = low_y ;//Centroids->position ;
      list->list[i].endy = up_y ; //(Centroids+npos-1)->position ;

      xsh_msg_dbg_low( " Iteration %d, Starty = %d, Endy = %d, Npos = %d",
		       niter, list->list[i].starty, list->list[i].endy, npos ) ;
      /* create vector from values */
      check( xpos = cpl_vector_new( npos ) ) ;
      check( centers = cpl_vector_new( npos ) ) ;
      {
	int j ;
	for( j = 0 ; j<npos ; j++ ) {
	  cpl_vector_set( xpos, j, (Centroids+j)->position ) ;
	  cpl_vector_set( centers, j, (Centroids+j)->centervalue ) ;
	}
      }

      /* create polynomial solutions */
      check(xsh_free_polynomial( &list->list[i].cenpoly));

      xsh_msg_dbg_low( "Polynomial degree: %d", degree ) ;
      check(list->list[i].cenpoly = 
	    xsh_polynomial_fit_1d_create( xpos, centers, degree, &poly_rms));
      xsh_free_vector( &centers ) ;
      xsh_free_vector( &xpos ) ;
      {
	double coeff0, coeff1, coeff2 ;
	cpl_size icoeff = 0 ;

	coeff0 = cpl_polynomial_get_coeff( list->list[i].cenpoly, &icoeff ) ;
	icoeff = 1 ;
	coeff1 = cpl_polynomial_get_coeff( list->list[i].cenpoly, &icoeff ) ;
	icoeff = 2 ;
	coeff2 = cpl_polynomial_get_coeff( list->list[i].cenpoly, &icoeff ) ;
	xsh_msg_dbg_low( " Calculated Poly: %lf %lf %lf", coeff0, coeff1, coeff2 ) ;
	icoeff = 0 ;
	coeff0 = cpl_polynomial_get_coeff( guess_list->list[i].cenpoly, &icoeff ) ;
	icoeff = 1 ;
	coeff1 = cpl_polynomial_get_coeff( guess_list->list[i].cenpoly, &icoeff ) ;
	icoeff = 2 ;
	coeff2 = cpl_polynomial_get_coeff( guess_list->list[i].cenpoly, &icoeff ) ;
	xsh_msg_dbg_low( "    Initial Poly: %lf %lf %lf", coeff0, coeff1, coeff2 ) ;
      }
      /*
	Now sigma clipping
      */
      poly_rms = sqrt( poly_rms ) ;
      xsh_msg_dbg_low( "       Sigma Clipping - Polynomial RMS: %lf", poly_rms ) ;

      for( nn = 0 ; nn < npos ; nn++ ) {
	/* Compute distance between Centroids->xpos and poly value at x
	   with rms of polynomial
	*/
	double xpoly, delta ;

	xpoly = cpl_polynomial_eval_1d( list->list[i].cenpoly,
					(Centroids+nn)->position,
					NULL ) ;
	delta = fabs( xpoly - (Centroids+nn)->centervalue) ;
	if ( delta > poly_rms*dcn_clipping->sigma ) {
	  /* Reject this position (set the position at an 
	     impossible value) */
	  xsh_msg_dbg_high( "        Rejected at %.0lf,%.0lf (%lf > %lf)",
			    (Centroids+nn)->centervalue, 
			    (Centroids+nn)->position,
			    delta, poly_rms ) ;
	  (Centroids+nn)->position = BAD_POSITION_FLAG ;
	  (Centroids+nn)->flag = BAD_POSITION_FLAG ;
	  nbad++ ;
	}
      }
      if ( nbad == 0 ) break ;
      totnbad += nbad ;
      xsh_msg_dbg_low( "       Rejected Positions: %d (%d)", nbad, totnbad ) ;

      /*
	if too many rejected positions, discard the order
      */
      if ( ((float)(npts-totnbad)/(float)npts) < dcn_clipping->frac ) {
	/* Tag the order as undetected and break */
	list->list[i].starty = -1 ;
	list->list[i].endy = -1 ;
	xsh_msg( "**** Too many rejected points: %d/%d (%.2f) Discard Order %d",
		 totnbad, npts, (float)(npts-totnbad)/(float)npts, absorder ) ;
	break ;
      }

      /* Reorder Centroids (suppress bad positions) */
      for( nn = 0, mm = 0 ; nn<npos ; nn++ ) {
	if ( (Centroids+nn)->position != BAD_POSITION_FLAG ) {
	  if ( mm != nn ) *(Centroids+mm) = *(Centroids+nn) ;
	  mm++ ;
	}
      }
      npos = mm ;
    }

    if ( list->list[i].starty != -1 ) {
      double coeff0, coeff1, coeff2 ;
      cpl_size icoeff = 0 ;

      ndetected++ ;
      coeff0 = cpl_polynomial_get_coeff( list->list[i].cenpoly, &icoeff ) ;
      icoeff = 1 ;
      coeff1 = cpl_polynomial_get_coeff( list->list[i].cenpoly, &icoeff ) ;
      icoeff = 2 ;
      coeff2 = cpl_polynomial_get_coeff( list->list[i].cenpoly, &icoeff ) ;
      xsh_msg( "  Polynomial Coeffs: %lf %lf %lf",
	       coeff0, coeff1, coeff2 ) ;


      // Prepare to calculate QC parameters
      cumulate_qc_parameter( i, list, Centroids, npos ) ;
    }
  }

  xsh_msg( " ++ Nb of Detected Orders: %d", ndetected ) ;
  assure(ndetected > 0,CPL_ERROR_ILLEGAL_INPUT,
         "parameter detectcontinuum-search-wind-hsize may have a too large value or detectcontinuum-ordertab-deg-y or detectcontinuum-clip-sigma or detectcontinuum-clip-res-max may have a too small value");
  if(DeltaPoints>0) {
  check(*resid_frame=calculate_qc_parameters( &ord_qc_param, instr )) ;
  }
 
  ord_qc_param.ndet = ndetected ;
 
  //xsh_order_list_dump( list, "final_order.out" ) ;
  /*
    Setup more QC parameters
  */
  ord_qc_param.npred = list->size ;
  ord_qc_param.max_pred = 0;
  ord_qc_param.min_pred = 0 ;
  ord_qc_param.nposall = 0 ;
  ord_qc_param.npossel = 0 ;

  /*
    Set QC parameters
  */
  check(set_qc_parameters( list, &ord_qc_param, instr ) );

  /*
    Save table frame
  */
  {
    char frame_name[256];
    char tag[256];
    sprintf(tag,"%s",XSH_GET_TAG_FROM_ARM(XSH_ORDER_TAB_CENTR,instr));
    sprintf(frame_name,"%s%s",tag,".fits");

    check(result = xsh_order_list_save(list,instr,frame_name,tag,pre->ny ));
    //xsh_add_temporary_file( frame_name ) ;

    xsh_msg_dbg_high("%s Created", frame_name ) ;
  }



 cleanup:
  if ( flog ) fclose( flog ) ;

  xsh_free_vector( &centers ) ;
  xsh_free_vector( &xpos ) ;
  XSH_FREE( Centroids ) ;
  xsh_pre_free(&pre);
  xsh_order_list_free(&list);
  xsh_order_list_free(&guess_list);


  return result;
}

/**@}*/
