/*                                                                           *
 *   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-01 18:59:24 $
 * $Revision: 1.57 $
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#define REGDEBUG_BSPLINE 0
/*---------------------------------------------------------------------------*/
/**
 * @defgroup xsh_data_wavemap Wavemap
 * @ingroup data_handling
 */
/*---------------------------------------------------------------------------*/

/**@{*/


/*-----------------------------------------------------------------------------
  Includes
  ----------------------------------------------------------------------------*/
#include <string.h>
#include <math.h>
#include <xsh_utils_table.h>
#include <xsh_parameters.h>
#include <xsh_data_pre.h>
#include <xsh_data_order.h>
#include <xsh_data_dispersol.h>
#include <xsh_utils_wrappers.h>
#include <xsh_data_wavemap.h>
#include <xsh_error.h>
#include <xsh_msg.h>
#include <xsh_pfits.h>
#include <xsh_dfs.h>
#include <math.h>
#include <gsl/gsl_multifit.h>
#include <cpl.h>

/*----------------------------------------------------------------------------
  Function implementation
  ----------------------------------------------------------------------------*/
/** 
 * @brief Dump main info about a rec table (for each order of the list)
 * 
 * @param list pointer to table list
 * @param fname File name
 */
void 
xsh_wavemap_list_dump( xsh_wavemap_list * list,
			    const char * fname )
{
  int i ;
  FILE * fout = NULL;

  XSH_ASSURE_NOT_NULL( list ) ;
  if ( fname == NULL ) fout = stdout ;
  else fout = fopen( fname, "w" ) ;
  XSH_ASSURE_NOT_NULL( fout ) ;

  fprintf( fout, "Wavemap List. Nb of orders: %d\n", list->size ) ;
  for( i = 0 ; i<list->size ; i++ ) {
    fprintf( fout,  " Entry %2d: Order %d, Ndata: %d\n",
	     i, list->list[i].order, list->list[i].sky_size);
  }

 cleanup:
  if ( fname != NULL && fout != NULL ) fclose( fout ) ;
  return ;
}


/*---------------------------------------------------------------------------*/
/**
   @brief create an empty order list
   @param instr instrument in use
   @return the order list structure

*/
/*---------------------------------------------------------------------------*/
xsh_wavemap_list* 
xsh_wavemap_list_create( xsh_instrument* instr)
{
  xsh_wavemap_list* result = NULL;
  XSH_INSTRCONFIG * config = NULL;
  //XSH_ARM xsh_arm;
  int i, size = 0;

  /* check input parameters */
  XSH_ASSURE_NOT_NULL( instr);

  /* Get arm */
  //xsh_arm = xsh_instrument_get_arm( instr) ;
  check( config = xsh_instrument_get_config( instr));
  size = config->orders;

  XSH_CALLOC(result, xsh_wavemap_list, 1 );

  result->size = size;

  XSH_ASSURE_NOT_ILLEGAL(result->size > 0);   
  result->instrument = instr;
  XSH_CALLOC(result->list, xsh_wavemap, result->size);  
  XSH_NEW_PROPERTYLIST(result->header);

  /* Create polynomials for each order */
  for( i = 0 ; i<result->size ; i++ ) {
    result->list[i].tcheb_pol_lambda = NULL ;
  }

 cleanup:
  if (cpl_error_get_code() != CPL_ERROR_NONE) {
    xsh_wavemap_list_free(&result);
  }  
  return result;
}
/*---------------------------------------------------------------------------*/
/**
   @brief set max size of wavemap
   @param list the wavemap list
   @param idx  index to check size
   @param absorder absolute order
   @param max_size maximum size
   @return the wavemap list structure

*/
/*---------------------------------------------------------------------------*/
void 
xsh_wavemap_list_set_max_size( xsh_wavemap_list * list, int idx,
                               int absorder, int max_size)
{
  xsh_wavemap * pwavemap = NULL ;

  /* Check input parameters */
  XSH_ASSURE_NOT_NULL(list) ;
  XSH_ASSURE_NOT_ILLEGAL( idx >= 0 && idx < list->size && max_size > 0);
  pwavemap = &list->list[idx] ;
  XSH_ASSURE_NOT_NULL( pwavemap);

  pwavemap->order = absorder;
  pwavemap->sky_size = max_size;
  pwavemap->object_size = max_size;
  pwavemap->all_size = max_size;
  XSH_CALLOC( pwavemap->sky, wavemap_item, max_size);
  XSH_CALLOC( pwavemap->object, wavemap_item, max_size);
  XSH_CALLOC( pwavemap->all, wavemap_item, max_size);

  cleanup:
    return ;
}

/*---------------------------------------------------------------------------*/
/**
   @brief free memory associated to a wavemap_list
   @param list the wavemap_list to free
*/
/*---------------------------------------------------------------------------*/
void 
xsh_wavemap_list_free(xsh_wavemap_list** list)
{
  int i = 0;
  xsh_wavemap_list *plist = NULL;

  if (list != NULL && *list != NULL ) {
    plist = *list ;
    /* free the list */
    for (i = 0; i < plist->size; i++) {
      xsh_wavemap *pr = &(plist->list[i]) ;

      xsh_msg_dbg_high( "Freeing order index %d", i ) ;
      if ( pr == NULL ) continue ;
      xsh_msg_dbg_high( "     Abs Order: %d", pr->order ) ;
      cpl_free( pr->sky);
      cpl_free( pr->object);
      cpl_free( pr->all);
      if (pr->pol_lambda != NULL){
        xsh_free_polynomial( &(pr->pol_lambda));
      }
      if (pr->pol_slit != NULL){
        xsh_free_polynomial( &(pr->pol_slit));
      }
      xsh_free_polynomial( &(pr->tcheb_pol_lambda));
    }
    if ((*list)->list){
      cpl_free((*list)->list);
    }
    xsh_free_propertylist(&((*list)->header));
    cpl_free(*list);
    *list = NULL;
  }
}

static void 
lambda_fit( double * lambda, double * xpos, double * ypos,
            int size, xsh_wavemap_list * wmap, 
            xsh_dispersol_param * dispsol_param, int order)
{
  double xmin, xmax, ymin, ymax, lmin, lmax;
  double *tcheb_lambda = NULL, *tcheb_xpos = NULL, *tcheb_ypos = NULL;
  int nbcoefs, xdeg, ydeg;
  cpl_vector *tcheb_coef_x = NULL;
  cpl_vector *tcheb_coef_y = NULL;
  int i, j, k ;
  double chisq;
  gsl_matrix *X = NULL, *cov = NULL;
  gsl_vector *y = NULL, *c = NULL;
  gsl_multifit_linear_workspace *work = NULL;
  cpl_polynomial* result = NULL;
  cpl_size pows[3];

  XSH_ASSURE_NOT_NULL( lambda);
  XSH_ASSURE_NOT_NULL( xpos);
  XSH_ASSURE_NOT_NULL( ypos);
  XSH_ASSURE_NOT_NULL( wmap);
  XSH_ASSURE_NOT_NULL( dispsol_param);
  XSH_ASSURE_NOT_ILLEGAL( size > 0);
  XSH_ASSURE_NOT_ILLEGAL( order >= 0 && order < wmap->size);

  xsh_msg_dbg_low( "      Entering lambda_fit. Order %d, size: %d",
		   wmap->list[order].order, size);
  xdeg = dispsol_param->deg_x;
  ydeg = dispsol_param->deg_y;
  nbcoefs = (xdeg+1)*(ydeg+1) ;

  XSH_ASSURE_NOT_ILLEGAL( size >= nbcoefs);

  wmap->degx = xdeg ;
  wmap->degy = ydeg ;

  /* Calculate min and max (necessary to transform into Tchebitchev space) */
  check(xsh_tools_min_max(size, lambda, &lmin, &lmax));
  check(xsh_tools_min_max(size, xpos, &xmin, &xmax));
  check(xsh_tools_min_max(size, ypos, &ymin, &ymax));
  
  wmap->list[order].xmin = xmin ;
  wmap->list[order].xmax = xmax ;
  wmap->list[order].ymin = ymin ;
  wmap->list[order].ymax = ymax ;
  wmap->list[order].lambda_min = lmin ;
  wmap->list[order].lambda_max = lmax ;

  xsh_msg_dbg_medium( 
    "   Min,Max. Lambda: %.6lf,%.6lf - X: %.6lf,%.6lf - Y: %.6lf,%.6lf",
    lmin, lmax, xmin, xmax, ymin, ymax);

  /* Transform to tchebitchev space */
  XSH_CALLOC(tcheb_lambda, double, size);
  XSH_CALLOC(tcheb_xpos, double, size);
  XSH_CALLOC(tcheb_ypos, double, size);

  check(xsh_tools_tchebitchev_transform_tab(size, lambda, lmin, lmax,
					    tcheb_lambda));
  check(xsh_tools_tchebitchev_transform_tab(size, xpos, xmin, xmax,
					    tcheb_xpos));
  check(xsh_tools_tchebitchev_transform_tab(size, ypos, ymin, ymax,
					    tcheb_ypos));
  X = gsl_matrix_alloc( size, nbcoefs);
  y = gsl_vector_alloc( size);
  c = gsl_vector_alloc( nbcoefs);
  cov = gsl_matrix_alloc( nbcoefs, nbcoefs);

  for (i = 0; i < size; i++) {
    check( tcheb_coef_x = xsh_tools_tchebitchev_poly_eval(
      xdeg, tcheb_xpos[i]));
    check( tcheb_coef_y = xsh_tools_tchebitchev_poly_eval(
      ydeg, tcheb_ypos[i]));

    for(j = 0; j < dispsol_param->deg_x +1 ; j++) {
      for(k = 0; k < dispsol_param->deg_y +1 ; k++){
        double vx, vy;
	
        check( vx = cpl_vector_get( tcheb_coef_x, j));
	check( vy = cpl_vector_get( tcheb_coef_y, k));
	gsl_matrix_set (X, i, k+j*(xdeg+1), vx*vy );
      }
    }
    gsl_vector_set (y, i, tcheb_lambda[i]);
    xsh_free_vector(&tcheb_coef_x);
    xsh_free_vector(&tcheb_coef_y);

  }
  xsh_msg_dbg_medium( "    GSL Initialized" ) ;


  XSH_ASSURE_NOT_ILLEGAL_MSG( size  >nbcoefs,
			      "Not enough Points vs Number of Coeffs" ) ;
  work = gsl_multifit_linear_alloc(size, nbcoefs);
  gsl_multifit_linear(X, y, c, cov, &chisq, work);

  xsh_msg_dbg_medium( "     GSL Done" ) ;

  wmap->list[order].tcheb_pol_lambda = cpl_polynomial_new( 2);

  result = wmap->list[order].tcheb_pol_lambda ;

  for(i = 0; i < (xdeg +1) ; i++){
    for(j = 0; j < (ydeg +1) ; j++){
      pows[0] = j;
      pows[1] = i;
      cpl_polynomial_set_coeff(result, pows,
			       gsl_vector_get(c, j+i*(ydeg+1)));
    }
  }
  xsh_msg_dbg_medium( "    GSL Finished" ) ;

  gsl_multifit_linear_free (work);
  gsl_matrix_free (X);
  gsl_vector_free (y);
  gsl_vector_free (c);
  gsl_matrix_free (cov);

  cleanup:
    XSH_FREE( tcheb_lambda);
    XSH_FREE( tcheb_xpos);
    XSH_FREE( tcheb_ypos);
    return ;
}

/*---------------------------------------------------------------------------*/
/**
   @brief compute a wave-map-list
   @param  vlambda array with wavelength values
   @param  vslit   array with slit values
   @param  xpos    array with x positions
   @param  ypos    array with y positions
   @param  nitems  number of list sampling points
   @param  orders  number of orders
   @param  dispsol_param structure with parameters for dispersion solution
   @param  wmap    wavemap
   @doc For each order makes a fit of 
          lambda as a function of X,Y (tchebitchev)
          slit pos as a function of X,Y (tchebitchev)
   @return null
*/
/*---------------------------------------------------------------------------*/



void 
xsh_wavemap_list_compute_poly( double * vlambda, 
                              double * vslit,
                               double * xpos, 
                               double * ypos,
                               int nitems, 
                               double * orders,
                               xsh_dispersol_param *dispsol_param,
                               xsh_wavemap_list *wmap)
{
  /*
 *     calculer par ordre ==> separer en fonction de l'ordre (vorderdata)
 *         pour chaque ordre, fitter lambda en fonction de X et Y (tchebitchev)
 *           */
  int i, ordersize, nborder ;
  double *vx = NULL, *vy = NULL, *vl = NULL, *vs = NULL;
  

  xsh_msg( "Entering xsh_wavemap_compute" ) ;

  XSH_ASSURE_NOT_NULL( vlambda);
  XSH_ASSURE_NOT_NULL( vslit);
  XSH_ASSURE_NOT_NULL( xpos);
  XSH_ASSURE_NOT_NULL( ypos);
  XSH_ASSURE_NOT_NULL( orders);
  XSH_ASSURE_NOT_NULL( wmap);
  XSH_ASSURE_NOT_NULL( dispsol_param);
  XSH_ASSURE_NOT_ILLEGAL( nitems > 0);
  XSH_ASSURE_NOT_ILLEGAL( wmap->size);

  xsh_msg( "   X degree = %d, Y degree = %d", dispsol_param->deg_x,
           dispsol_param->deg_y) ;

  /* split by order */
  ordersize = 0;
  nborder = 0;

  wmap->degx = dispsol_param->deg_x;
  wmap->degy = dispsol_param->deg_y;

  xsh_msg("Compute POLY for lambda");
  XSH_REGDEBUG( "nitems %d ", nitems);

  for( i = 1 ; i<=nitems ; i++ ) {

    if ( i < nitems && orders[i-1] == orders[i] ) {
      ordersize++;
    }
    else {
      cpl_vector* posx = NULL;
      cpl_vector* posy = NULL;
      cpl_vector* lvalues = NULL;
      cpl_vector* svalues = NULL;
      cpl_bivector *positions = NULL;
      double mse =0.0;

      ordersize++;
      int i_minus_ordersize=i-ordersize;
      XSH_MALLOC( vx, double, ordersize);
      memcpy( vx, &(xpos[i_minus_ordersize]), ordersize*sizeof(double));

      XSH_MALLOC( vy, double, ordersize);
      memcpy( vy, &(ypos[i_minus_ordersize]), ordersize*sizeof(double));

      XSH_MALLOC( vl, double, ordersize);
      memcpy( vl, &(vlambda[i_minus_ordersize]), ordersize*sizeof(double) ) ;

      XSH_MALLOC( vs, double, ordersize);
      memcpy( vs, &( vslit[i_minus_ordersize]), ordersize*sizeof(double) ) ;
      wmap->list[nborder].sky_size = ordersize ;

      /* Now fit lambda */
      wmap->list[nborder].order = orders[i-1] ;
      xsh_msg_dbg_high( "Order: %d, Size: %d", wmap->list[nborder].order,
                        ordersize );

      posx = cpl_vector_wrap( ordersize, vx);
      posy = cpl_vector_wrap( ordersize, vy);

      positions = cpl_bivector_wrap_vectors( posx, posy);
      lvalues = cpl_vector_wrap( ordersize, vl);
      svalues = cpl_vector_wrap( ordersize, vs);
      cpl_size loc_degx=(cpl_size)dispsol_param->deg_x;
      wmap->list[nborder].pol_lambda =
        xsh_polynomial_fit_2d_create( positions, lvalues, &loc_degx,
				      &mse);

      wmap->list[nborder].pol_slit =
        xsh_polynomial_fit_2d_create( positions, svalues, &loc_degx,
				      &mse);

      cpl_bivector_unwrap_vectors( positions);

      cpl_vector_unwrap( posx);
      cpl_vector_unwrap( posy);
      cpl_vector_unwrap( lvalues);
      cpl_vector_unwrap( svalues);

      XSH_FREE( vx);
      XSH_FREE( vy);
      XSH_FREE( vl);
      XSH_FREE( vs);
      nborder++ ;
      ordersize = 0 ;

    }
  }

 cleanup:
    XSH_FREE( vx);
    XSH_FREE( vy);
    XSH_FREE( vl);
    XSH_FREE( vs);
    return ;
}

/*---------------------------------------------------------------------------*/
/**
   @brief compute a wave-map-list
   @param  vlambda array with wavelength values
   @param  xpos    array with x positions
   @param  ypos    array with y positions
   @param  nitems  number of list sampling points
   @param  orders  number of orders
   @param  dispsol_param structure with parameters for dispersion solution
   @param  wmap    wavemap
   @doc For each order makes a fit of 
          lambda as a function of X,Y (tchebitchev)
   @return null
*/
/*---------------------------------------------------------------------------*/
void 
xsh_wavemap_list_compute( double * vlambda, 
                          double * xpos, 
                          double * ypos,
			  int nitems, 
                          double * orders,
			  xsh_dispersol_param * dispsol_param,
			  xsh_wavemap_list * wmap )
{
  int i, ordersize, nborder ;
  double *vx = NULL, *vy = NULL, *vl = NULL;

  xsh_msg( "Entering xsh_wavemap_compute" ) ;

  XSH_ASSURE_NOT_NULL( vlambda ) ;
  XSH_ASSURE_NOT_NULL( xpos ) ;
  XSH_ASSURE_NOT_NULL( ypos ) ;
  XSH_ASSURE_NOT_NULL( orders ) ;
  XSH_ASSURE_NOT_NULL( wmap ) ;
  XSH_ASSURE_NOT_NULL( dispsol_param ) ;
  XSH_ASSURE_NOT_ILLEGAL( nitems > 0 ) ;
  XSH_ASSURE_NOT_ILLEGAL( wmap->size ) ;

  xsh_msg( "   X degree = %d, Y degree = %d", dispsol_param->deg_x,
	   dispsol_param->deg_y) ;

  /*
    split data by order (according to orders array)
    then fit a tchebitchev polynomial Lambda = T(X,Y) per order 
  */
  ordersize = 0;
  nborder = 0;

  XSH_REGDEBUG( "nitems %d ", nitems);

  for( i = 1 ; i<=nitems ; i++ ) {
     
    if ( i < nitems && orders[i-1] == orders[i] ) {
      ordersize++;
    }
    else {
      ordersize++;
      XSH_MALLOC( vx, double, ordersize);
      memcpy( vx, &(xpos[i-ordersize]), ordersize*sizeof(double));

      XSH_MALLOC( vy, double, ordersize);
      memcpy( vy, &(ypos[i-ordersize]), ordersize*sizeof(double));

      XSH_MALLOC( vl, double, ordersize);
      memcpy( vl, &(vlambda[i-ordersize]), ordersize*sizeof(double) ) ;

      wmap->list[nborder].sky_size = ordersize;
      /* Now fit lambda */
      wmap->list[nborder].order = orders[i-1] ;
      xsh_msg_dbg_high( "Order: %d, Size: %d", wmap->list[nborder].order,
			ordersize );

      check( lambda_fit( vl, vx, vy, ordersize, wmap, dispsol_param, nborder ) ) ;
      XSH_FREE( vx ) ;
      XSH_FREE( vy ) ;
      XSH_FREE( vl ) ;
      nborder++ ;
      ordersize = 0 ;
    }
  }
  
 cleanup:
    XSH_FREE( vx ) ;
    XSH_FREE( vy ) ;
    XSH_FREE( vl ) ;
    return ;
}

/*---------------------------------------------------------------------------*/
/**
  @brief
    eval the polynomial wave solution in x,y,ord
  @param wmap polynomial wave solution
  @param x position x
  @param y position y
  @param ord order value
 * @return return the solution in lambda at x, y
 */
/*---------------------------------------------------------------------------*/

static double 
xsh_wavemap_list_eval_lambda( xsh_wavemap_list * wmap,
			      double x, double y, int ord )
{
  double tcheb_x = 0.0, tcheb_y = 0.0;
  double lambda = -1.;
  double res=0.0;
  int i, j ;
  cpl_size pows[2] ;
  int degx, degy ;
  cpl_vector *tcheb_coef_x = NULL;
  cpl_vector *tcheb_coef_y = NULL;

  XSH_ASSURE_NOT_NULL( wmap);
  degx = wmap->degx;
  degy = wmap->degy;
  XSH_ASSURE_NOT_ILLEGAL( degx >0 && degy > 0);
 
  xsh_msg_dbg_high( "    eval_lambda: degx: %d, min: %lf, max: %lf",
		    degx, wmap->list[ord].xmin,
		    wmap->list[ord].xmax ) ;
  // Bug fixing
  if ( wmap->list[ord].xmin >= wmap->list[ord].xmax ||
       wmap->list[ord].lambda_min >= wmap->list[ord].lambda_max ||
       wmap->list[ord].ymin >= wmap->list[ord].ymax ) {
    return 0. ;
  }

  check( tcheb_x = xsh_tools_tchebitchev_transform( x, wmap->list[ord].xmin,
    wmap->list[ord].xmax));
  check( tcheb_y = xsh_tools_tchebitchev_transform( y,
    wmap->list[ord].ymin, wmap->list[ord].ymax));
  check( tcheb_coef_x = xsh_tools_tchebitchev_poly_eval(
    degx, tcheb_x));
  check( tcheb_coef_y = xsh_tools_tchebitchev_poly_eval(
    degy, tcheb_y));

  for ( i=0; i < degx+1; i++) {
    for( j=0; j < degy+1; j++) {
      double vx, vy;
      double coef;

      check( vx = cpl_vector_get( tcheb_coef_x, i));
      check( vy = cpl_vector_get( tcheb_coef_y, j));
      pows[0] = j;
      pows[1] = i;
      check( coef = cpl_polynomial_get_coeff( wmap->list[ord].tcheb_pol_lambda,
        pows));
      res += coef*vx*vy ;
    }
  }
  check( lambda = xsh_tools_tchebitchev_reverse_transform( res,
    wmap->list[ord].lambda_min, wmap->list[ord].lambda_max));

  cleanup:
    xsh_free_vector( &tcheb_coef_x);
    xsh_free_vector( &tcheb_coef_y);
    return lambda ;
}

/** 
 * Create a WaveMap Image and save it as a frame.
 * The wave map image is  "raw" image (not a PRE !).
 * 
 * @param wmap Wavemap list
 * @param order_frame Order table frame
 * @param pre pre frame (to get frame size and header)
 * @param instr instrument arm setting
 * @param prefix prefix used to define Final frame file name
 * 
 * @return The created frame
 */
cpl_frame * 
xsh_wavemap_list_save( xsh_wavemap_list * wmap,
		       cpl_frame * order_frame,
		       xsh_pre * pre,
		       xsh_instrument * instr, 
                       const char * prefix)
{

  cpl_frame * result = NULL ;
  int order, x, y ;
  int start_y, end_y ;
  int xmin, xmax ;
  int nx, ny ;
  double lambda ;
  cpl_image * wimg = NULL ;
  double * dimg = NULL ;
  cpl_propertylist * wheader = NULL ;
  char * final_name = NULL;
  xsh_order_list * order_list = NULL ;

  XSH_ASSURE_NOT_NULL( wmap ) ;
  XSH_ASSURE_NOT_NULL( order_frame ) ;
  XSH_ASSURE_NOT_NULL( pre ) ;
  XSH_ASSURE_NOT_NULL( prefix ) ;
  XSH_ASSURE_NOT_NULL( instr ) ;

  final_name = xsh_stringcat_any( prefix, ".fits", (void*)NULL ) ;
  xsh_msg( "Entering xsh_wavemap_save, file \"%s\"", final_name ) ;
  /*
    create the wave map image filled with zeroes.
    Fill the orders with lambda calculated from wave map polynomial
  */
  /*
    Load the order table into the order_list
  */
  check(order_list=xsh_order_list_load(order_frame, instr )) ;
  /*
    Create a new image according to parameters included in instrument
    structure
  */

  nx = pre->nx ;
  ny = pre->ny ;

  xsh_msg/*_dbg_medium*/( "Image size:%d,%d", nx, ny ) ;

  check( wimg = cpl_image_new( nx, ny, CPL_TYPE_DOUBLE ) ) ;
  check( dimg = cpl_image_get_data_double( wimg ) ) ;

  for( order = 0 ; order < wmap->size ; order++ ) {
    int count = 0 ;

    // Fix 
    if ( wmap->list[order].tcheb_pol_lambda == NULL ) {
      xsh_msg( "Order %d: NULL Polynome", order ) ;
      continue ;
    }
    if ( wmap->list[order].sky_size <= 0 ) {
      xsh_msg( "NOT ENOUGH DATA FOR ORDER %d",
	       order_list->list[order].absorder ) ;
      continue ;
    }
    /* get start_y, end_y from order list */
    start_y = xsh_order_list_get_starty( order_list, order ) ;
    end_y = xsh_order_list_get_endy( order_list, order ) ;
    xsh_msg_dbg_low( "    Order %d, Ymin: %d, Ymax: %d",
		     order, start_y, end_y ) ;

    for ( y = start_y ; y < end_y ; y++ ) {
      double dxmin, dxmax ;

      /*    get xmin, xmax */
      check( dxmin = cpl_polynomial_eval_1d(order_list->list[order].edglopoly,
					    (double)y, NULL ) ) ;
      check( dxmax = cpl_polynomial_eval_1d(order_list->list[order].edguppoly,
					    (double)y, NULL ) ) ;
      xmin = floor(dxmin) ;
      xmax = ceil(dxmax) ;
      for (x = xmin ; x<xmax ; x++ ) {
	count++ ;
	/*calculate lambda(x,y)*/
	lambda = xsh_wavemap_list_eval_lambda( wmap, (double)x, 
          (double)y, order);
        if ( cpl_error_get_code() != CPL_ERROR_NONE){
          lambda =0.0;
          xsh_error_reset();
        }
	/*add lambda to image */
	*(dimg + x + (y*nx)) = (float)lambda ;
      }
    }
    xsh_msg_dbg_high( "          %d points to calculate", count ) ;
  }

  /* Now save image */
  check( wheader = cpl_propertylist_duplicate( pre->data_header ) ) ;
  check( cpl_image_save( wimg, final_name, CPL_BPP_IEEE_FLOAT,
			 wheader, CPL_IO_DEFAULT ) ) ;
  check(result=xsh_frame_product(final_name,
				 "TAG",
				 CPL_FRAME_TYPE_IMAGE,
				 CPL_FRAME_GROUP_PRODUCT,
				 CPL_FRAME_LEVEL_TEMPORARY));

  cleanup:
    xsh_order_list_free( &order_list);
    XSH_FREE( final_name ) ;
    xsh_free_image ( &wimg ) ;
    xsh_free_propertylist( &wheader);

  return result ;
}



cpl_error_code
xsh_wavemap_list_sky_image_save( xsh_wavemap_list * smap,
                             xsh_instrument * instr,
                             const int abs_ord, const int sid,const int iter)
{

    xsh_pre* sky_model = NULL ;
    xsh_pre* fit_model = NULL ;
    cpl_image* sky_wmap = NULL;
    cpl_image* sky_smap = NULL;

    float* pdata_sky =NULL;
    float* perrs_sky =NULL;
    int* pqual_sky =NULL;
    float* pdata_fit =NULL;
    float* perrs_fit =NULL;
    int* pqual_fit =NULL;

    float* psky_wmap =NULL;
    float* psky_smap =NULL;

    int order;
    int sx, sy ;
    //char fname[80];
    char * final_name = NULL;

    XSH_ASSURE_NOT_NULL( smap ) ;
    XSH_ASSURE_NOT_NULL( instr ) ;

    sx = instr->config->nx /instr->binx;
    sy = instr->config->ny /instr->biny;
    xsh_msg( "Image size:%d,%d", sx, sy ) ;

    /* create a model of the object and the sky as a PRE format frame */
    sky_model = xsh_pre_new(sx,sy);
    fit_model = xsh_pre_new(sx,sy);

    pdata_sky = cpl_image_get_data_float(xsh_pre_get_data(sky_model));
    perrs_sky = cpl_image_get_data_float(xsh_pre_get_errs(sky_model));
    pqual_sky = cpl_image_get_data_int(xsh_pre_get_qual(sky_model));

    pdata_fit = cpl_image_get_data_float(xsh_pre_get_data(fit_model));
    perrs_fit = cpl_image_get_data_float(xsh_pre_get_errs(fit_model));
    pqual_fit = cpl_image_get_data_int(xsh_pre_get_qual(fit_model));


    /* associate to object and sky the corresponding wave and slit maps */
    sky_wmap  = cpl_image_new(sx,sy,XSH_PRE_DATA_TYPE);
    sky_smap  = cpl_image_new(sx,sy,XSH_PRE_DATA_TYPE);

    psky_wmap = cpl_image_get_data_float(sky_wmap);
    psky_smap = cpl_image_get_data_float(sky_smap);

    int ix, iy,pix;
    for( order = 0 ; order < smap->size ; order++ ) {

        wavemap_item * psky = smap->list[order].sky;
        int sky_size = smap->list[order].sky_size;

        for (int k = 0; k < sky_size; k++) {
            ix = psky->ix;
            iy = psky->iy;
            pix = iy*sx+ix;
            pdata_sky[pix]=psky->flux;
            perrs_sky[pix]=psky->sigma;
            pqual_sky[pix]=psky->qual;
            pdata_fit[pix]=psky->fitted;
            perrs_fit[pix]=psky->fit_err;
            pqual_fit[pix]=psky->qual;

            psky_wmap[pix]=psky->lambda;
            psky_smap[pix]=psky->slit;
            psky++;
        }

    }

#if REGDEBUG_BSPLINE
    cpl_frame* frm = NULL;
    sprintf(fname,"sky_model_ord_%2.2d_slice_%2.2d_iter_%2.2d.fits",abs_ord,sid,iter);
    frm = xsh_pre_save(sky_model,fname, "SKY_MODEL",0);
    xsh_free_frame(&frm);
    sprintf(fname,"sky_fit_ord_%2.2d_slice_%2.2d_iter_%2.2d.fits",abs_ord,sid,iter);
    frm = xsh_pre_save(fit_model,fname, "SKY_FIT",0);
    xsh_free_frame(&frm);
    sprintf(fname,"sky_wmap_%2.2d.fits",iter);
    cpl_image_save(sky_wmap,"sky_wmap.fits", XSH_PRE_DATA_BPP, NULL, CPL_IO_DEFAULT);
    sprintf(fname,"sky_smap_%2.2d.fits",iter);
    cpl_image_save(sky_smap,"sky_smap.fits", XSH_PRE_DATA_BPP, NULL, CPL_IO_DEFAULT);
#endif

    cleanup:
    cpl_free(final_name);
    xsh_pre_free(&fit_model);
    xsh_pre_free(&sky_model);
    xsh_free_image(&sky_wmap);
    xsh_free_image(&sky_smap);

    return cpl_error_get_code() ;
}
/**
 * Create a WaveMap Image and save it as an multi extension image
 *
 * @param wmap Wavemap list
 * @param instr instrument arm setting
 * @param iter id
 *
 * @return The created frame
 */
cpl_error_code
xsh_wavemap_list_object_image_save( xsh_wavemap_list * omap,
                             xsh_instrument * instr,
                             const int iter)
{

    xsh_pre* obj_model = NULL ;
    cpl_image* obj_wmap = NULL;
    cpl_image* obj_smap = NULL;

    float* pdata_obj =NULL;
    float* perrs_obj =NULL;
    int* pqual_obj =NULL;

    float* pobj_wmap =NULL;
    float* pobj_smap =NULL;

    int order;
    int sx, sy ;
    //char fname[80];

    XSH_ASSURE_NOT_NULL( omap ) ;
    XSH_ASSURE_NOT_NULL( instr ) ;

    sx = instr->config->nx / instr->binx;
    sy = instr->config->ny / instr->biny;

    /* create a model of the object and the sky as a PRE format frame */
    obj_model = xsh_pre_new(sx,sy);

    pdata_obj = cpl_image_get_data_float(xsh_pre_get_data(obj_model));
    perrs_obj = cpl_image_get_data_float(xsh_pre_get_errs(obj_model));
    pqual_obj = cpl_image_get_data_int(xsh_pre_get_qual(obj_model));

    /* associate to object and sky the corresponding wave and slit maps */
    obj_wmap  = cpl_image_new(sx,sy,XSH_PRE_DATA_TYPE);
    obj_smap  = cpl_image_new(sx,sy,XSH_PRE_DATA_TYPE);

    pobj_wmap = cpl_image_get_data_float(obj_wmap);
    pobj_smap = cpl_image_get_data_float(obj_smap);

    int ix, iy,pix;
    for( order = 0 ; order < omap->size ; order++ ) {

        wavemap_item * pobj = omap->list[order].object;
        int obj_size = omap->list[order].object_size;

        for (int k = 0; k < obj_size; k++) {
            ix = pobj->ix;
            iy = pobj->iy;
            pix = iy*sx+ix;

            pdata_obj[pix]=pobj->flux;
            perrs_obj[pix]=pobj->sigma;
            pqual_obj[pix]=pobj->qual;
            pobj_wmap[pix]=pobj->lambda;
            pobj_smap[pix]=pobj->slit;

            pobj++;
        }


    }
#if REGDEBUG_BSPLINE
    cpl_frame* frm = NULL;
    sprintf(fname,"object_model_%2.2d.fits",iter);
    frm = xsh_pre_save(obj_model,fname, "OBJECT_MODEL",0);
    xsh_free_frame(&frm);

    sprintf(fname,fname,iter);
    cpl_image_save(obj_wmap,"object_wmap.fits", XSH_PRE_DATA_BPP, NULL, CPL_IO_DEFAULT);
    sprintf(fname,"object_smap_%2.2d.fits",iter);
    cpl_image_save(obj_smap,fname, XSH_PRE_DATA_BPP, NULL, CPL_IO_DEFAULT);
#endif

    cleanup:
    xsh_pre_free(&obj_model);
    xsh_free_image(&obj_wmap);
    xsh_free_image(&obj_smap);

    return cpl_error_get_code() ;
}


/**
 * Create a WaveMap Image and save it as an multi extension image
 *
 * @param wmap Wavemap list
 * @param instr instrument arm setting
 * @param prefix prefix to define Final frame file name
 *
 * @return The created frame
 */
cpl_error_code
xsh_wavemap_list_save4debug( xsh_wavemap_list * wmap,
                             xsh_instrument * instr,
                             const char * prefix)
{

    xsh_pre* obj_model = NULL ;
    xsh_pre* sky_model = NULL ;
    cpl_image* obj_wmap = NULL;
    cpl_image* obj_smap = NULL;
    cpl_image* sky_wmap = NULL;
    cpl_image* sky_smap = NULL;

    float* pdata_obj =NULL;
    float* perrs_obj =NULL;
    int* pqual_obj =NULL;

    float* pdata_sky =NULL;
    float* perrs_sky =NULL;
    int* pqual_sky =NULL;

    float* pobj_wmap =NULL;
    float* pobj_smap =NULL;

    float* psky_wmap =NULL;
    float* psky_smap =NULL;

    int order;
    int sx, sy ;
    char * final_name = NULL;

    XSH_ASSURE_NOT_NULL( wmap ) ;
    XSH_ASSURE_NOT_NULL( prefix ) ;
    XSH_ASSURE_NOT_NULL( instr ) ;

    final_name = xsh_stringcat_any( prefix, ".fits", (void*)NULL ) ;
    xsh_msg( "Entering xsh_wavemap_save, file \"%s\"", final_name ) ;

    sx = instr->config->nx /instr->binx;
    sy = instr->config->ny /instr->biny;
    xsh_msg( "Image size:%d,%d", sx, sy ) ;

    /* create a model of the object and the sky as a PRE format frame */
    obj_model = xsh_pre_new(sx,sy);
    sky_model = xsh_pre_new(sx,sy);

    pdata_obj = cpl_image_get_data_float(xsh_pre_get_data(obj_model));
    perrs_obj = cpl_image_get_data_float(xsh_pre_get_errs(obj_model));
    pqual_obj = cpl_image_get_data_int(xsh_pre_get_qual(obj_model));

    pdata_sky = cpl_image_get_data_float(xsh_pre_get_data(sky_model));
    perrs_sky = cpl_image_get_data_float(xsh_pre_get_errs(sky_model));
    pqual_sky = cpl_image_get_data_int(xsh_pre_get_qual(sky_model));

    /* associate to object and sky the corresponding wave and slit maps */
    obj_wmap  = cpl_image_new(sx,sy,XSH_PRE_DATA_TYPE);
    obj_smap  = cpl_image_new(sx,sy,XSH_PRE_DATA_TYPE);
    sky_wmap  = cpl_image_new(sx,sy,XSH_PRE_DATA_TYPE);
    sky_smap  = cpl_image_new(sx,sy,XSH_PRE_DATA_TYPE);

    pobj_wmap = cpl_image_get_data_float(obj_wmap);
    pobj_smap = cpl_image_get_data_float(obj_smap);

    psky_wmap = cpl_image_get_data_float(sky_wmap);
    psky_smap = cpl_image_get_data_float(sky_smap);

    int ix, iy,pix;
    for( order = 0 ; order < wmap->size ; order++ ) {

        wavemap_item * psky = wmap->list[order].sky;
        wavemap_item * pobj = wmap->list[order].object;
        int sky_size = wmap->list[order].sky_size;
        int obj_size = wmap->list[order].object_size;

        for (int k = 0; k < obj_size; k++) {
            ix = pobj->ix;
            iy = pobj->iy;
            pix = iy*sx+ix;
            pdata_obj[pix]=pobj->flux;
            perrs_obj[pix]=pobj->sigma;
            pqual_obj[pix]=pobj->qual;
            pobj_wmap[pix]=pobj->lambda;
            pobj_smap[pix]=pobj->slit;
            pobj++;
        }

        for (int k = 0; k < sky_size; k++) {
            ix = psky->ix;
            iy = psky->iy;
            pix = iy*sx+ix;
            pdata_sky[pix]=psky->flux;
            perrs_sky[pix]=psky->sigma;
            pqual_sky[pix]=psky->qual;
            psky_wmap[pix]=psky->lambda;
            psky_smap[pix]=psky->slit;
            psky++;
        }

    }
#if REGDEBUG_BSPLINE
    cpl_frame* frm = NULL;
    frm = xsh_pre_save(obj_model,"object_model.fits", "OBJECT_MODEL",0);
    xsh_free_frame(&frm);
    frm = xsh_pre_save(sky_model,"sky_model.fits", "SKY_MODEL",0);
    xsh_free_frame(&frm);

    cpl_image_save(obj_wmap,"object_wmap.fits", XSH_PRE_DATA_BPP, NULL, CPL_IO_DEFAULT);
    cpl_image_save(obj_smap,"object_smap.fits", XSH_PRE_DATA_BPP, NULL, CPL_IO_DEFAULT);
    cpl_image_save(sky_wmap,"sky_wmap.fits", XSH_PRE_DATA_BPP, NULL, CPL_IO_DEFAULT);
    cpl_image_save(sky_smap,"sky_smap.fits", XSH_PRE_DATA_BPP, NULL, CPL_IO_DEFAULT);
#endif

    cleanup:
    xsh_pre_free(&obj_model);
    xsh_pre_free(&sky_model);
    xsh_free_image(&obj_wmap);
    xsh_free_image(&obj_smap);
    xsh_free_image(&sky_wmap);
    xsh_free_image(&sky_smap);

    return cpl_error_get_code() ;
}

/** 
 * Create a WaveMap Image and save it as a frame.
 * The wave map image is  "raw" image (not a PRE !).
 * 
 * @param wmap Wavemap list
 * @param order_list Order table list
 * @param pre pre frame (to get frame size and header)
 * @param instr instrument arm setting
 * @param prefix prefix to define Final frame file name
 * 
 * @return The created frame
 */
cpl_frame * 
xsh_wavemap_list_save2( xsh_wavemap_list * wmap,
			xsh_order_list * order_list,
			xsh_pre * pre,
			xsh_instrument * instr, 
                        const char * prefix)
{
  cpl_frame * result = NULL ;
  int order, x, y ;
  int start_y, end_y ;
  int xmin, xmax ;
  int nx, ny ;
  double lambda ;
  cpl_image * wimg = NULL ;
  double * dimg = NULL ;
  cpl_propertylist * wheader = NULL ;
  char * final_name = NULL;

  XSH_ASSURE_NOT_NULL( wmap ) ;
  XSH_ASSURE_NOT_NULL( order_list ) ;
  XSH_ASSURE_NOT_NULL( pre ) ;
  XSH_ASSURE_NOT_NULL( prefix ) ;
  XSH_ASSURE_NOT_NULL( instr ) ;

  final_name = xsh_stringcat_any( prefix, ".fits", (void*)NULL ) ;
  xsh_msg( "Entering xsh_wavemap_save, file \"%s\"", final_name ) ;

  /*
    create the wave map image filled with zeroes.
    Fill the orders with lambda calculated from wave map polynomial
  */
  /*
    Load the order table into the order_list
  */

  /*
    Create a new image according to parameters included in instrument
    structure
  */

  nx = pre->nx ;
  ny = pre->ny ;
  xsh_msg/*_dbg_medium*/( "Image size:%d,%d", nx, ny ) ;

  check( wimg = cpl_image_new( nx, ny, CPL_TYPE_DOUBLE ) ) ;
  check( dimg = cpl_image_get_data_double( wimg ) ) ;

  for( order = 0 ; order < wmap->size ; order++ ) {
    int count = 0 ;

    /*
    // Fix 
    if ( wmap->list[order].tcheb_pol_lambda == NULL ) {
      xsh_msg( "Order %d: NULL Polynome", order ) ;
      continue ;
    }
    if ( wmap->list[order].sky_size <= 0 ) {
      xsh_msg( "NOT ENOUGH DATA FOR ORDER %d",
	       order_list->list[order].absorder ) ;
      continue ;
    }
    */
    /* get start_y, end_y from order list */
    start_y = xsh_order_list_get_starty( order_list, order ) ;
    end_y = xsh_order_list_get_endy( order_list, order ) ;
    xsh_msg_dbg_low( "    Order %d, Ymin: %d, Ymax: %d",
		     order, start_y, end_y ) ;
    xsh_msg( "    Order %d, Ymin: %d, Ymax: %d",
                 order, start_y, end_y ) ;
    for ( y = start_y ; y < end_y ; y++ ) {
      double dxmin, dxmax ;

      /*    get xmin, xmax */
      check( dxmin = cpl_polynomial_eval_1d(order_list->list[order].edglopoly,
					    (double)y, NULL ) ) ;
      check( dxmax = cpl_polynomial_eval_1d(order_list->list[order].edguppoly,
					    (double)y, NULL ) ) ;
      xmin = floor(dxmin) ;
      xmax = ceil(dxmax) ;
      xsh_msg( "    Order %d, Xmin: %d, Xmax: %d",
                       order, xmin, xmax ) ;
      for (x = xmin ; x<xmax ; x++ ) {
	count++ ;
	/*calculate lambda(x,y)*/
	lambda = xsh_wavemap_list_eval_lambda( wmap, (double)x, 
          (double)y, order);
        if ( cpl_error_get_code() != CPL_ERROR_NONE){
          lambda =0.0;
          xsh_msg("degx=%d degy=%d", wmap->degx, wmap->degy);
          xsh_print_rec_status(0);
          //xsh_error_reset();
        }
	/*add lambda to image */
	*(dimg + x + (y*nx)) = (float)lambda ;
      }
    }
    xsh_msg_dbg_high( "          %d points to calculate", count ) ;
  }

  /* Now save image */
  check( wheader = cpl_propertylist_duplicate( pre->data_header ) ) ;

  check( cpl_image_save( wimg, final_name, CPL_BPP_IEEE_FLOAT,
			 wheader, CPL_IO_DEFAULT ) ) ;
  check(result=xsh_frame_product(final_name,
				 "TAG",
				 CPL_FRAME_TYPE_IMAGE,
				 CPL_FRAME_GROUP_PRODUCT,
				 CPL_FRAME_LEVEL_TEMPORARY));

  cleanup:
    xsh_order_list_free( &order_list);
    XSH_FREE( final_name ) ;
    xsh_free_image ( &wimg ) ;
    xsh_free_propertylist( &wheader);

  return result ;
}


/*****************************************************************************/
/**
  @brief 
    Save the wave_map slit_map and disp_tab
 * @param wmap Wavemap list
 * @param order_frame orderpos frame
 * @param pre  multi-pinhole frame in PRE format
 * @param instr instrument setting structure
 * @param prefix product prefix
 * @param dispersol_frame dispersion solution frame
 * @param slitmap_frame  slit map solution frame
   @return wavelength map frame, dispersion and slitmap solution frames

*/
/*****************************************************************************/
cpl_frame* 
xsh_wavemap_list_save_poly( xsh_wavemap_list* wmap,
                            cpl_frame *order_frame, 
                            xsh_pre *pre, 
                            xsh_instrument *instr, 
                            const char *prefix, 
                            cpl_frame **dispersol_frame, 
                            cpl_frame **slitmap_frame)
{
  cpl_frame *result = NULL;
  const char *slit_tag=NULL;
  int order;
  xsh_dispersol_list *dispersol_list = NULL;
  const char* disp_tag=NULL;

  XSH_ASSURE_NOT_NULL( wmap);
  XSH_ASSURE_NOT_NULL( order_frame);
  XSH_ASSURE_NOT_NULL( prefix);
  XSH_ASSURE_NOT_NULL( dispersol_frame);
  XSH_ASSURE_NOT_NULL( instr);


  check( dispersol_list = xsh_dispersol_list_new( wmap->size,
    wmap->degx, wmap->degy, instr));

  for( order = 0 ; order < wmap->size ; order++ ) {
    /* get start_y, end_y from order list */
    check( xsh_dispersol_list_add( dispersol_list, order,
       wmap->list[order].order,  wmap->list[order].pol_lambda,
       wmap->list[order].pol_slit));
    wmap->list[order].pol_lambda = NULL;
    wmap->list[order].pol_slit = NULL;
  }

  if (pre != NULL){

    check( result = xsh_dispersol_list_to_wavemap( dispersol_list,
						   order_frame, pre, 
						   instr,prefix));
 
    slit_tag = XSH_GET_TAG_FROM_ARM( XSH_SLIT_MAP_POLY, instr);


    check (*slitmap_frame = xsh_dispersol_list_to_slitmap( dispersol_list,
							   order_frame, pre, 
							   instr, slit_tag));
  }

  if(strstr(cpl_frame_get_tag(order_frame),"AFC") != NULL) {
    disp_tag = XSH_GET_TAG_FROM_ARM( XSH_DISP_TAB_AFC,instr);
  } else {
    disp_tag = XSH_GET_TAG_FROM_ARM( XSH_DISP_TAB,instr);
  }
  check(*dispersol_frame = xsh_dispersol_list_save( dispersol_list,disp_tag));

  cleanup:
    if (cpl_error_get_code() != CPL_ERROR_NONE){
      xsh_free_frame( &result);
    }
    xsh_dispersol_list_free( &dispersol_list);


    return result ;
}
/*****************************************************************************/
/**@}*/
