/*                                                                           *
 *   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: 2011-12-09 10:10:12 $
 * $Revision: 1.59 $
 * $Name: not supported by cvs2svn $
 */

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

/*---------------------------------------------------------------------------*/
/**
 * @defgroup xsh_data_wavesol Wave Solution
 * @ingroup data_handling
 */
/*---------------------------------------------------------------------------*/

/**@{*/


/*-----------------------------------------------------------------------------
                                 Includes
 ----------------------------------------------------------------------------*/
#include <xsh_data_wavesol.h>
#include <xsh_data_spectralformat.h>
#include <xsh_utils.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>
#include <xsh_utils_table.h>
/*----------------------------------------------------------------------------
                                 Function implementation
 ----------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*/
/**
  @brief
    Create a new wavelength solution structure
  @param[in] spectral_format_frame
    The spectral format frame
  @param[in] params 
    Pointer to the parameters structure
  @param[in] instrument
    The instrument structure
  @return
    The NEW ALLOCATED wavelength solution
*/
/*---------------------------------------------------------------------------*/
xsh_wavesol* xsh_wavesol_create( cpl_frame* spectral_format_frame,
  xsh_detect_arclines_param* params, xsh_instrument *instrument) 
{
  xsh_wavesol* res = NULL;
  xsh_spectralformat_list *spec_list = NULL;
  int i;
  float sol_lambda_min=0, sol_lambda_max=0;
  int sol_absorder_min=0, sol_absorder_max=0;

  /* Check parameters */
  XSH_ASSURE_NOT_NULL( params);
  XSH_ASSURE_NOT_NULL( spectral_format_frame);
  XSH_ASSURE_NOT_NULL( instrument ) ;

  /* Allocate memory */  
  XSH_MALLOC(res, xsh_wavesol, 1);
  res->deg_slit = params->wavesol_deg_slit;
  res->deg_order = params->wavesol_deg_order;
  res->deg_lambda = params->wavesol_deg_lambda;
  res->nbcoefs = (res->deg_order+1)*(res->deg_slit+1)*(res->deg_lambda+1); 
  xsh_msg_dbg_high("nbcoef %d deg_lambda %d deg_n %d deg_s %d",res->nbcoefs,
    res->deg_lambda, res->deg_order, res->deg_slit);
  res->polx = cpl_polynomial_new(3); 
  res->poly = cpl_polynomial_new(3);
  res->dim = cpl_vector_new(3);
  res->header = cpl_propertylist_new();

  /* Add binx,y */
  res->bin_x = xsh_instrument_get_binx( instrument ) ;
  res->bin_y = xsh_instrument_get_biny( instrument ) ;

  /* compute range of lambda and orders */
  check( spec_list = xsh_spectralformat_list_load( spectral_format_frame,
    instrument));
  sol_lambda_min = spec_list->list[0].lambda_min_full;
  sol_lambda_max = spec_list->list[0].lambda_max_full;
  sol_absorder_min = spec_list->list[0].absorder;
  sol_absorder_max = spec_list->list[0].absorder;

  for( i=1; i< spec_list->size; i++){
    int absorder;
    float lambda_min, lambda_max;
    absorder = spec_list->list[i].absorder;
    lambda_min = spec_list->list[i].lambda_min_full;
    lambda_max = spec_list->list[i].lambda_max_full;
    if ( absorder > sol_absorder_max){
      sol_absorder_max = absorder;
    }
    if ( absorder < sol_absorder_min){
      sol_absorder_min = absorder;
    }
    if ( lambda_min < sol_lambda_min){
      sol_lambda_min = lambda_min;
    }
    if ( lambda_max > sol_lambda_max){
      sol_lambda_max = lambda_max;
    }
  }
  xsh_msg_dbg_high("Order range %d-%d",sol_absorder_min, sol_absorder_max);
  xsh_msg_dbg_high("Lambda range %f-%f", sol_lambda_min, sol_lambda_max);
  res->min_lambda = sol_lambda_min;
  res->max_lambda = sol_lambda_max;
  res->min_order = sol_absorder_min;
  res->max_order = sol_absorder_max;
 
  cleanup:
    xsh_spectralformat_list_free( &spec_list); 
    return res;
}

/**
 * @brief duplicate a wavelength solution structure
 *
 * @param org Original structure
 * @return the NEW ALLOCATED wavelength solution
 */
/*---------------------------------------------------------------------------*/
xsh_wavesol * xsh_wavesol_duplicate( xsh_wavesol * org )
{
  xsh_wavesol * res = NULL ;

  XSH_MALLOC( res, xsh_wavesol, 1 ) ;
  res->bin_x = org->bin_x ;
  res->bin_y = org->bin_y ;
  res->deg_slit = org->deg_slit ;
  res->deg_order = org->deg_order ;
  res->deg_lambda = org->deg_lambda ;
  res->min_lambda = org->min_lambda ;
  res->max_lambda = org->max_lambda ;
  res->min_order = org->min_order ;
  res->max_order = org->max_order ;
  res->min_slit = org->min_slit ;
  res->max_slit = org->max_slit ;
  res->min_x = org->min_x ;
  res->max_x = org->max_x ;
  res->min_y = org->min_y ;
  res->max_y = org->max_y ;
  res->nbcoefs = org->nbcoefs ;
  res->polx = cpl_polynomial_duplicate( org->polx ) ;
  res->poly = cpl_polynomial_duplicate( org->poly ) ;
  res->dim = cpl_vector_duplicate( org->dim ) ;
  res->header = cpl_propertylist_duplicate( org->header ) ;

 cleanup:
  return res ;
}

/** 
 * Calculates the sum of the coefficients of poly polynomial.
 * 
 * @param to Result wavesol
 * @param from Original wavesol
 */
void xsh_wavesol_add_poly( xsh_wavesol * to, xsh_wavesol * from )
{
  int i, j, k ;
  cpl_size pows[3];

  pows[0] = 0;
  i = 0 ;

  for(j = 0;  j<= from->deg_order; j++)
    for(k = 0; k <= from->deg_lambda; k++) {
      double vto = 0.0, vfrom = 0.0;

      pows[0] = i ;
      pows[1] = j;
      pows[2] = k;
      xsh_msg_dbg_high( "Add_poly: %d %d %d", i, j, k ) ;
      check(vfrom = cpl_polynomial_get_coeff(from->poly, pows));
      check(vto = cpl_polynomial_get_coeff(to->poly, pows));
      vto += vfrom;
      check( cpl_polynomial_set_coeff( to->poly, pows, vto));
    }

 cleanup:
  return ;
}

/*---------------------------------------------------------------------------*/
/**
 * @brief free wavelength solution structure
 *
 * @param w adress of pointer on wavelength solution to DEALLOCATED
 */
/*---------------------------------------------------------------------------*/
void xsh_wavesol_free(xsh_wavesol** w)
{
  if (w && *w) {
    xsh_free_polynomial(&(*w)->polx);
    xsh_free_polynomial(&(*w)->poly);
    xsh_free_vector(&(*w)->dim);
    xsh_free_propertylist(&(*w)->header);
    cpl_free(*w);
    *w = NULL;
  }
}


/*---------------------------------------------------------------------------*/
/**
 * @brief get the solution in Y
 *
 * @param sol the wavelength solution
 * @return return the solution in Y
 */
/*---------------------------------------------------------------------------*/
cpl_polynomial* xsh_wavesol_get_poly(xsh_wavesol* sol)
{
  cpl_polynomial* res = NULL;

  XSH_ASSURE_NOT_NULL(sol);
  res = sol->poly;

  cleanup:
    return res;
}


/*---------------------------------------------------------------------------*/
/**
 *  @brief set the type of the wave table
 *   
 *  @param wsol the wavelength solution
 *  @param type the type GUESS or 2D
 */
/*---------------------------------------------------------------------------*/

void xsh_wavesol_set_type(xsh_wavesol * wsol, enum wavesol_type type)
{
  XSH_ASSURE_NOT_NULL( wsol);
  wsol->type = type;

  cleanup:
    return; 
}



/*---------------------------------------------------------------------------*/
/**
 *  @brief get the type of the wave table
 *   
 *  @param wsol the wavelength solution
 *  @return the type GUESS or 2D
 */
enum wavesol_type xsh_wavesol_get_type(xsh_wavesol *wsol)
{
  enum wavesol_type type = XSH_WAVESOL_GUESS;
  XSH_ASSURE_NOT_NULL( wsol);
  type = wsol->type;

  cleanup:
    return type; 
}

/*---------------------------------------------------------------------------*/
/**
 * @brief get the solution in X
 *
 * @param sol the wavelength solution
 * @return return the solution in X
 */
/*---------------------------------------------------------------------------*/
cpl_polynomial* xsh_wavesol_get_polx(xsh_wavesol* sol)
{
  cpl_polynomial* res = NULL;

  XSH_ASSURE_NOT_NULL(sol);
  res = sol->polx;

  cleanup:
    return res;
}

/*---------------------------------------------------------------------------*/
/** 
  @brief get header of the table
  @param sol the wavelength solution
  @return the header associated to the table
 */
/*---------------------------------------------------------------------------*/
cpl_propertylist* xsh_wavesol_get_header(xsh_wavesol* sol)
{
  cpl_propertylist * res = NULL;

  XSH_ASSURE_NOT_NULL(sol);
  res = sol->header;
  cleanup:
    return res;
}

/*---------------------------------------------------------------------------*/
/**
 * @brief eval the polynomial solution in X
 *
 * @param sol the wavelength solution
 * @param lambda lambda value
 * @param order order value
 * @param slit slit value
 * @return return the solution in X at lambda, order, slit
 */
/*---------------------------------------------------------------------------*/
double xsh_wavesol_eval_polx(xsh_wavesol* sol, double lambda, double order,
  double slit)
{
  double tcheb_slit = 0.0, tcheb_order, tcheb_lambda;
  double pos = 0.0, res = 0.0;
  int i, j, k;
  cpl_size pows[3];
  cpl_vector *tcheb_coef_lambda = NULL;
  cpl_vector *tcheb_coef_order = NULL;
  cpl_vector *tcheb_coef_slit = NULL;

  /* Check input parameters */
  XSH_ASSURE_NOT_NULL( sol);

  if (sol->deg_slit > 0){
  check(tcheb_slit = xsh_tools_tchebitchev_transform(slit, sol->min_slit,
    sol->max_slit));
  }
  check(tcheb_order = xsh_tools_tchebitchev_transform(order, sol->min_order,
    sol->max_order));
  check(tcheb_lambda = xsh_tools_tchebitchev_transform(lambda,
    sol->min_lambda, sol->max_lambda));
  if ( tcheb_lambda < -1. ) tcheb_lambda = -1. ;
  if ( tcheb_lambda > 1. ) tcheb_lambda = 1. ;

  check( tcheb_coef_lambda = xsh_tools_tchebitchev_poly_eval( 
    sol->deg_lambda, tcheb_lambda));
  check( tcheb_coef_order = xsh_tools_tchebitchev_poly_eval( 
    sol->deg_order, tcheb_order));
  check( tcheb_coef_slit = xsh_tools_tchebitchev_poly_eval( 
    sol->deg_slit, tcheb_slit));

  for(i = 0; i < sol->deg_lambda +1 ; i++) {
    for(j = 0; j < sol->deg_order +1 ; j++){
      for(k = 0; k < sol->deg_slit +1 ; k++){
        double coef;
        double vlambda ;
        double vslit ;
        double vorder ;

        check( vlambda = cpl_vector_get( tcheb_coef_lambda, i));
        check( vorder = cpl_vector_get( tcheb_coef_order, j));
        check( vslit = cpl_vector_get( tcheb_coef_slit, k));
        pows[0] = k;
        pows[1] = j;
        pows[2] = i;
        check( coef = cpl_polynomial_get_coeff(sol->polx, pows));
        res += coef*vslit*vorder*vlambda;
      }
    }
  }
  check(pos = xsh_tools_tchebitchev_reverse_transform(res, sol->min_x,
    sol->max_x));

  /* Convert with binning */
  pos = convert_data_to_bin( pos, sol->bin_x ) ;

  cleanup:
    xsh_free_vector( &tcheb_coef_lambda);
    xsh_free_vector( &tcheb_coef_order);
    xsh_free_vector( &tcheb_coef_slit);
    return pos;
}


/*---------------------------------------------------------------------------*/
/**
 * @brief eval the polynomial solution in Y
 *
 * @param sol the wavelength solution
 * @param lambda lambda value
 * @param order order value
 * @param slit slit value
 * @return return the solution in Y at lambda, order, slit
 */
/*---------------------------------------------------------------------------*/
double xsh_wavesol_eval_poly(xsh_wavesol* sol, double lambda, double order,
  double slit)
{
  double tcheb_slit = 0.0, tcheb_order, tcheb_lambda;
  double pos = 0.0, res = 0.0;
  int i, j, k;
  cpl_size pows[3];
  cpl_vector *tcheb_coef_lambda = NULL;
  cpl_vector *tcheb_coef_order = NULL;
  cpl_vector *tcheb_coef_slit = NULL;


  XSH_ASSURE_NOT_NULL(sol);

  if (sol->deg_slit > 0){
    check(tcheb_slit = xsh_tools_tchebitchev_transform(slit, sol->min_slit,
						       sol->max_slit));
  }
  check(tcheb_order = xsh_tools_tchebitchev_transform(order, sol->min_order,
    sol->max_order));
  check(tcheb_lambda = xsh_tools_tchebitchev_transform(lambda,
						       sol->min_lambda,
						       sol->max_lambda));
  if ( tcheb_lambda < -1. ) tcheb_lambda = -1. ;
  if ( tcheb_lambda > 1. ) tcheb_lambda = 1. ;

  check( tcheb_coef_lambda = xsh_tools_tchebitchev_poly_eval(
    sol->deg_lambda, tcheb_lambda));
  check( tcheb_coef_order = xsh_tools_tchebitchev_poly_eval(
    sol->deg_order, tcheb_order));
  check( tcheb_coef_slit = xsh_tools_tchebitchev_poly_eval(
    sol->deg_slit, tcheb_slit));

  for(i = 0; i < sol->deg_lambda +1 ; i++) {
    for(j = 0; j < sol->deg_order +1 ; j++){
      for(k = 0; k < sol->deg_slit +1 ; k++){
        double coef;
        double vlambda ;
        double vslit ;
        double vorder ;

        check( vlambda = cpl_vector_get( tcheb_coef_lambda, i));
        check( vorder = cpl_vector_get( tcheb_coef_order, j));
        check( vslit = cpl_vector_get( tcheb_coef_slit, k));
        pows[0] = k;
        pows[1] = j;
        pows[2] = i;
        check( coef = cpl_polynomial_get_coeff(sol->poly, pows));
        res += coef*vslit*vorder*vlambda;
      }
    }
  }
  check(pos = xsh_tools_tchebitchev_reverse_transform(res, sol->min_y,
    sol->max_y));

  /* Convert with binning */
  pos = convert_data_to_bin( pos, sol->bin_y ) ;

  cleanup:
    xsh_free_vector( &tcheb_coef_lambda);
    xsh_free_vector( &tcheb_coef_order);
    xsh_free_vector( &tcheb_coef_slit);
    return pos;
}



/*---------------------------------------------------------------------------*/
/**
 @brief compute a wavelength solution

 @param sol The Wavesolution to compute
 @param size Number of entries in the data vectors
 @param pos Pointer to position arrray
 @param[out] posmin Pointer to minimum of the position
 @param[out] posmax Pointer to maximum of the position
 @param[in] lambda Array of wavelength
 @param[in] order Array of orders
 @param[in] slit Array of slits
 @param result Calculated polynomial
 */
/*---------------------------------------------------------------------------*/

void xsh_wavesol_compute(xsh_wavesol* sol, int size, 
  double* pos,double *posmin, double *posmax, double* lambda, 
  double* order, double* slit, cpl_polynomial* result)
{
  int i, j, k, l;
  double chisq;
  gsl_matrix *X = NULL, *cov = NULL;
  gsl_vector *y = NULL, *c = NULL;
  gsl_multifit_linear_workspace *work = NULL;
  cpl_size pows[3];
  double *tcheb_pos = NULL, *tcheb_lambda = NULL;
  double *tcheb_order = NULL, *tcheb_slit = NULL;


  XSH_ASSURE_NOT_NULL(sol);
  XSH_ASSURE_NOT_ILLEGAL( sol->nbcoefs < size);

  /* search min and max */
  check(xsh_tools_min_max(size, slit, &(sol->min_slit), &(sol->max_slit)));

  /* extrapolate limit in slit */
  sol->min_slit *=  XSH_SLIT_RANGE;
  sol->max_slit *= XSH_SLIT_RANGE;

  check(xsh_tools_min_max(size, pos, posmin, posmax));

  /* transform in tchebitchev plan */
  XSH_CALLOC(tcheb_lambda, double, size);
  XSH_CALLOC(tcheb_order, double, size);
  XSH_CALLOC(tcheb_slit, double, size);
  XSH_CALLOC(tcheb_pos, double, size);

  check(xsh_tools_tchebitchev_transform_tab( size, lambda, sol->min_lambda, 
    sol->max_lambda, tcheb_lambda));
  check(xsh_tools_tchebitchev_transform_tab( size, order, sol->min_order, 
    sol->max_order, tcheb_order));
  if (sol->deg_slit > 0){
    check( xsh_tools_tchebitchev_transform_tab( size, slit, sol->min_slit, 
      sol->max_slit, tcheb_slit));
  }
  check(xsh_tools_tchebitchev_transform_tab( size, pos, *posmin, *posmax, 
    tcheb_pos));

  X = gsl_matrix_alloc (size, sol->nbcoefs);
  y = gsl_vector_alloc (size);
  c = gsl_vector_alloc (sol->nbcoefs);
  cov = gsl_matrix_alloc (sol->nbcoefs, sol->nbcoefs);

  for (i = 0; i < size; i++) {
    for(j = 0; j < sol->deg_lambda +1 ; j++) {
      for(k = 0; k < sol->deg_order +1 ; k++){
        for(l = 0; l < sol->deg_slit +1 ; l++){
          double vslit = cos(l*acos(tcheb_slit[i]));
          double vorder =  cos(k*acos(tcheb_order[i]));
          double vlambda = cos(j*acos(tcheb_lambda[i]));

          gsl_matrix_set (X, i, l+k*(sol->deg_slit+1)+j*(sol->deg_order+1)*
            (sol->deg_slit+1), vslit*vorder*vlambda);
        }
      }
    }
    gsl_vector_set (y, i, tcheb_pos[i]);
  }

  work = gsl_multifit_linear_alloc(size,sol->nbcoefs);
  gsl_multifit_linear(X, y, c, cov, &chisq, work);

  for(j=0; j< sol->deg_lambda+1; j++){
    for(k = 0; k < sol->deg_order +1 ; k++){
      for(l = 0; l < sol->deg_slit +1 ; l++){
        pows[0] = l;
        pows[1] = k;
        pows[2] = j;
        cpl_polynomial_set_coeff(result, pows, gsl_vector_get(c,
          l+k*(sol->deg_slit+1)+j*(sol->deg_order+1)*(sol->deg_slit+1)));
      }
    }
  }
  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_pos);
    XSH_FREE(tcheb_lambda);
    XSH_FREE(tcheb_order);
    XSH_FREE(tcheb_slit);
}

/** 
 * Calculate the Tchebitchev polynomial coefficients for the residuals
 * between new and old positions. The coefficients of the result should be
 * added to the original polynomial.
 * 
 * @param sol Final wavesolution
 * @param adj Original wavesol
 * @param size Nb of elements
 * @param new_pos Pointer to array of fitted positions
 * @param lambda Pointer to wavelength values
 * @param order Pointer to order values
 * @param slit Pointer to Slit values
 * @param result Final Polynomial
 * @param axis Which axis ('y' or 'x')
 */
void xsh_wavesol_residual(xsh_wavesol* sol, xsh_wavesol * adj, int size, 
			  double* new_pos, double* lambda, 
			  double* order, double* slit,
			  cpl_polynomial* result, char axis)
{
  int i, j, k, l;
  double chisq;
  gsl_matrix *X = NULL, *cov = NULL;
  gsl_vector *y = NULL, *c = NULL;
  gsl_multifit_linear_workspace *work = NULL;
  cpl_size pows[3];
  double *tcheb_pos = NULL, *tcheb_lambda = NULL;
  double *tcheb_order = NULL, *tcheb_slit = NULL;
  double * tcheb_new = NULL ;
  double posmin, posmax ;

  XSH_ASSURE_NOT_NULL(sol);
  XSH_ASSURE_NOT_NULL(adj);

  /* set min and max */
  sol->min_lambda = adj->min_lambda ;
  sol->max_lambda = adj->max_lambda ;
  sol->min_order = adj->min_order ;
  sol->max_order = adj->max_order ;
  sol->min_slit = adj->min_slit ;
  sol->max_slit = adj->max_slit ;
  sol->min_y = adj->min_y ;
  sol->max_y = adj->max_y ;
  sol->min_x = adj->min_x ;
  sol->max_x = adj->max_x ;

  if ( axis == 'y' ){
      posmin = adj->min_y ;
      posmax = adj->max_y ;
  }
  else{
    posmin = adj->min_x ;
    posmax = adj->max_x ;
  }

  /* transform in tchebitchev space */
  XSH_CALLOC(tcheb_lambda, double, size);
  XSH_CALLOC(tcheb_order, double, size);
  XSH_CALLOC(tcheb_slit, double, size);
  XSH_CALLOC(tcheb_pos, double, size);
  XSH_CALLOC(tcheb_new, double, size);

  check(xsh_tools_tchebitchev_transform_tab(size, lambda, sol->min_lambda, 
					    sol->max_lambda, tcheb_lambda));
  check(xsh_tools_tchebitchev_transform_tab(size, order, sol->min_order, 
					    sol->max_order,tcheb_order));
  if (sol->deg_slit > 0){
    check(xsh_tools_tchebitchev_transform_tab(size, slit, sol->min_slit,
					      sol->max_slit, 
					      tcheb_slit));
  }
  check(xsh_tools_tchebitchev_transform_tab(size, new_pos, posmin, posmax, 
    tcheb_new));

  /* Calculate residuals in Tcheb Space
     tcheb_new = fitted (new) positions in tcheb space
     tcheb_pos = residual in tcheb space
  */
  {
    int indx ;
    double temp0, temp1, a, b ;

    a = 2/(posmax-posmin);
    b = 1-2*posmax/(posmax-posmin);

    for( indx = 0 ; indx < size ; indx++ ) {
      if ( axis == 'y' ) {
	temp0 = xsh_wavesol_eval_poly( adj, lambda[indx], order[indx],
				       slit[indx] ) ;
      }
      else
	temp0 = xsh_wavesol_eval_polx( adj, lambda[indx], order[indx],
				       slit[indx] ) ;

      temp1 = (temp0*a) + b ;
      tcheb_pos[indx] = tcheb_new[indx] - temp1 ;
    }
  }

  X = gsl_matrix_alloc (size, sol->nbcoefs);
  y = gsl_vector_alloc (size);
  c = gsl_vector_alloc (sol->nbcoefs);
  cov = gsl_matrix_alloc (sol->nbcoefs, sol->nbcoefs);

  for (i = 0; i < size; i++) {
    for(j = 0; j < sol->deg_lambda +1 ; j++) {
      for(k = 0; k < sol->deg_order +1 ; k++) {
        for(l = 0; l < sol->deg_slit +1 ; l++) {
          double vslit = cos(l*acos(tcheb_slit[i]));
          double vorder =  cos(k*acos(tcheb_order[i]));
          double vlambda = cos(j*acos(tcheb_lambda[i]));

          gsl_matrix_set (X, i, l+k*(sol->deg_slit+1)+j*(sol->deg_order+1)*
            (sol->deg_slit+1), vslit*vorder*vlambda);
        }
      }
    }
    gsl_vector_set (y, i, tcheb_pos[i]);
  }

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

  for(j=0; j< sol->deg_lambda+1; j++){
    for(k = 0; k < sol->deg_order +1 ; k++){
      for(l = 0; l < sol->deg_slit +1 ; l++){
        pows[0] = l;
        pows[1] = k;
        pows[2] = j;
        cpl_polynomial_set_coeff(result, pows, gsl_vector_get(c,
          l+k*(sol->deg_slit+1)+j*(sol->deg_order+1)*(sol->deg_slit+1)));
      }
    }
  }
  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_pos);
    XSH_FREE(tcheb_lambda);
    XSH_FREE(tcheb_order);
    XSH_FREE(tcheb_slit);
    XSH_FREE(tcheb_new);
}

/*---------------------------------------------------------------------------*/
/**
 * @brief save a wavelength solution
 *
 * @param w Pointer to the Wave Solution structure
 * @param trace table with wave solution 
 * @param filename the name of the file
 * @param tag product category
 * @return the new frame containing the wavelength solution
 */
/*---------------------------------------------------------------------------*/

cpl_frame* 
xsh_wavesol_save(xsh_wavesol *w, cpl_table* trace, const char* filename,const char* tag)
{
  cpl_table* table = NULL;
  cpl_frame * result = NULL ;
  int i, j, k;
  char coefname[20];
  cpl_size pows[3];

  XSH_ASSURE_NOT_NULL(w);
  XSH_ASSURE_NOT_NULL(trace);
  XSH_ASSURE_NOT_NULL(filename);

  /* create a table */
  check(table = cpl_table_new(XSH_WAVESOL_TABLE_NB_COL+w->nbcoefs));
  check(cpl_table_set_size(table,XSH_WAVESOL_TABLE_NB_ROWS));
  /* create column names */
  check(
    cpl_table_new_column(table,XSH_WAVESOL_TABLE_COLNAME_AXIS,
      CPL_TYPE_STRING));
  check(
    cpl_table_new_column(table,XSH_WAVESOL_TABLE_COLNAME_DEGSLIT,
      CPL_TYPE_INT));
  check(
    cpl_table_new_column(table,XSH_WAVESOL_TABLE_COLNAME_DEGORDER,
      CPL_TYPE_INT));
  check(
    cpl_table_new_column(table,XSH_WAVESOL_TABLE_COLNAME_DEGLAMBDA,
      CPL_TYPE_INT));
  check(cpl_table_set_string(table,XSH_WAVESOL_TABLE_COLNAME_AXIS,
    0,"X"));
  check(cpl_table_set_string(table,XSH_WAVESOL_TABLE_COLNAME_AXIS,
    1,"Y"));
  check(cpl_table_set_int(table,XSH_WAVESOL_TABLE_COLNAME_DEGSLIT,
    0,w->deg_slit));
  check(cpl_table_set_int(table,XSH_WAVESOL_TABLE_COLNAME_DEGSLIT,
    1, w->deg_slit));
  check(cpl_table_set_int(table,XSH_WAVESOL_TABLE_COLNAME_DEGORDER,
    0,w->deg_order));
  check(cpl_table_set_int(table,XSH_WAVESOL_TABLE_COLNAME_DEGORDER,
    1, w->deg_order));
  check(cpl_table_set_int(table,XSH_WAVESOL_TABLE_COLNAME_DEGLAMBDA,
    0,w->deg_lambda));
  check(cpl_table_set_int(table,XSH_WAVESOL_TABLE_COLNAME_DEGLAMBDA,
    1, w->deg_lambda));

  for(i=0; i<= w->deg_slit; i++){
    for(j = 0;  j<= w->deg_order; j++){
      for(k = 0; k <= w->deg_lambda; k++){
        double vx = 0.0, vy = 0.0;
        pows[0] = i;
        pows[1] = j;
        pows[2] = k;
        sprintf(coefname,"C%d%d%d",i,j,k);
        check(cpl_table_new_column(table,coefname, CPL_TYPE_DOUBLE));
        check(vx = cpl_polynomial_get_coeff(w->polx, pows));
        check(vy = cpl_polynomial_get_coeff(w->poly, pows));
        check(cpl_table_set_double( table, coefname, 0, vx));
        check(cpl_table_set_double( table, coefname, 1, vy));
      }
    }
  }

  /* write min max in header */
  check(xsh_pfits_set_wavesol_lambda_min(w->header, w->min_lambda));
  check(xsh_pfits_set_wavesol_lambda_max(w->header, w->max_lambda));
  check(xsh_pfits_set_wavesol_order_min(w->header, w->min_order));
  check(xsh_pfits_set_wavesol_order_max(w->header, w->max_order));
  check(xsh_pfits_set_wavesol_slit_min(w->header, w->min_slit));
  check(xsh_pfits_set_wavesol_slit_max(w->header, w->max_slit));  
  check(xsh_pfits_set_wavesol_x_min(w->header, w->min_x));
  check(xsh_pfits_set_wavesol_x_max(w->header, w->max_x));
  check(xsh_pfits_set_wavesol_y_min(w->header, w->min_y));
  check(xsh_pfits_set_wavesol_y_max(w->header, w->max_y));
  check( xsh_pfits_set_pcatg( w->header, tag));
  check(cpl_table_save(table, w->header, NULL, filename, CPL_IO_DEFAULT));
  check(cpl_table_save(trace, NULL, NULL, filename, CPL_IO_EXTEND));

  /* Create the frame */
  check(result=xsh_frame_product(filename,
				 tag,
				 CPL_FRAME_TYPE_TABLE,
				 CPL_FRAME_GROUP_PRODUCT,
				 CPL_FRAME_LEVEL_TEMPORARY));

  cleanup:
    XSH_TABLE_FREE( table);
    return result;
}


/*---------------------------------------------------------------------------*/
/**
 * @brief load a wavelength solution
 *  
 * @param frame  input frame
 * @param instrument instrument arm setting
 * @return the wavelength solution
 */
/*---------------------------------------------------------------------------*/

xsh_wavesol * xsh_wavesol_load( cpl_frame * frame,
				xsh_instrument * instrument )
{
  cpl_table* table = NULL; 
  const char* tablename = NULL;
  xsh_wavesol * result = NULL ;
  int rows, cols ;

  XSH_ASSURE_NOT_NULL(frame);

  /* get table filename */
  check(tablename = cpl_frame_get_filename(frame));

  XSH_TABLE_LOAD( table, tablename);

  XSH_CALLOC( result, xsh_wavesol, 1 ) ;

  check(result->header = cpl_propertylist_load(tablename,0));

  check( rows = cpl_table_get_nrow( table ) ) ;
  check( cols = cpl_table_get_ncol( table ) ) ;
  xsh_msg_dbg_high( " === Wavesol Rows = %d, Columns = %d", rows, cols ) ;
 
  /* For now fix the binning to 1 */
  result->bin_x = 1;
  result->bin_y = 1;
 
  check( result->min_lambda = xsh_pfits_get_wavesol_lambda_min(
    result->header));
  check( result->max_lambda = xsh_pfits_get_wavesol_lambda_max(
    result->header));
  check( result->min_order = xsh_pfits_get_wavesol_order_min(
    result->header));
  check( result->max_order = xsh_pfits_get_wavesol_order_max(
    result->header));
  check( result->min_slit = xsh_pfits_get_wavesol_slit_min(
    result->header));
  check( result->max_slit = xsh_pfits_get_wavesol_slit_max(
    result->header));
  check( result->min_x = xsh_pfits_get_wavesol_x_min(
    result->header));
  check( result->max_x = xsh_pfits_get_wavesol_x_max(
    result->header));
  check( result->min_y = xsh_pfits_get_wavesol_y_min(
    result->header));
  check( result->max_y = xsh_pfits_get_wavesol_y_max(
    result->header));

  /* Now populate the structure from the table */
  {
    int i, j, k ;

    /* Get degrees */
    check( xsh_get_table_value( table, XSH_WAVESOL_TABLE_COLNAME_DEGSLIT,
				CPL_TYPE_INT, 0, &result->deg_slit ) ) ;
    check( xsh_get_table_value( table, XSH_WAVESOL_TABLE_COLNAME_DEGORDER,
				CPL_TYPE_INT, 0, &result->deg_order ) ) ;
    check( xsh_get_table_value( table, XSH_WAVESOL_TABLE_COLNAME_DEGLAMBDA,
				CPL_TYPE_INT, 0, &result->deg_lambda ) ) ;
    /* Create polynomials */
    result->nbcoefs = (result->deg_order+1)*(result->deg_slit+1)*
      (result->deg_lambda+1);
    xsh_msg_dbg_high( "nbcoef %d deg_lambda %d deg_n %d deg_s %d", result->nbcoefs,
	     result->deg_lambda, result->deg_order, result->deg_slit);
    result->polx = cpl_polynomial_new(3); 
    result->poly = cpl_polynomial_new(3);
    result->dim = cpl_vector_new(3);

    /* Get and set coeffs */
    for( i = 0 ; i<=result->deg_slit ; i++ ) {
      for( j = 0 ; j<=result->deg_order ; j++ ) {
	for( k = 0 ; k<=result->deg_lambda ; k++ ) {
	  char coefname[16] ;
	  double coef ;
	  cpl_size pows[3] ;

	  pows[0] = i ;
	  pows[1] = j ;
	  pows[2] = k ;
	  sprintf( coefname, "C%d%d%d", i, j, k ) ;
	  /* Get coeffs */
	  check( xsh_get_table_value( table, coefname, CPL_TYPE_DOUBLE,
				      0, &coef ) ) ;
	  /* Set at the right place */
	  check( cpl_polynomial_set_coeff( result->polx, pows, coef ) ) ;
	  check( xsh_get_table_value( table, coefname, CPL_TYPE_DOUBLE,
				      1, &coef ) ) ;
	  /* Set at the right place */
	  check( cpl_polynomial_set_coeff( result->poly, pows, coef ) ) ;
	}
      }
    }
  }

  /* Set binning */
  result->bin_x = xsh_instrument_get_binx( instrument ) ;
  result->bin_y = xsh_instrument_get_biny( instrument ) ;
  
 cleanup:
    if (cpl_error_get_code () != CPL_ERROR_NONE) {
      xsh_error_msg("can't load Wavesol frame %s", tablename );
      xsh_wavesol_free(&result);
    }
  XSH_TABLE_FREE( table) ;

  return result ;
}

void xsh_wavesol_dump( xsh_wavesol * wsol, const char * fname, int nb )
{
  FILE * fout = NULL ;
  int i, j, k, l ;
  cpl_size pows[3] ;

  if ( fname != NULL ) fout = fopen( fname, "w" ) ;
  l = 0 ;

  for( i = 0 ; i <= wsol->deg_slit ; i++ )
    for( j = 0 ; j <= wsol->deg_lambda ; j++ )
      for( k = 0 ; k <= wsol->deg_order ; k++ ) {
	double coeff ;
	pows[0] = i ;
	pows[1] = j;
	pows[2] = k;
	check(coeff = cpl_polynomial_get_coeff( wsol->poly, pows));
	if ( fout == NULL )
	  xsh_msg( "         %d%d%d; %lf", i, j, k, coeff ) ;
	else 
	  fprintf( fout, "%d%d%d: %lf\n", i, j, k, coeff ) ;
	l++ ;
	if ( nb != 0 && l >= nb ) goto cleanup ;
      }
 cleanup:
  if ( fout != NULL ) fclose( fout ) ;
}

cpl_table* 
xsh_wavesol_trace( xsh_wavesol * wsol, double* lambda, 
		   double* order, double* slit,int size)
{
  cpl_table* table = NULL ;
  int i;
  double* pw=NULL;
  double* po=NULL;
  double* px=NULL;
  double* py=NULL;
  double* ps=NULL;


  /* Check parameters */
  XSH_ASSURE_NOT_NULL( wsol);
  XSH_ASSURE_NOT_NULL( lambda);
  XSH_ASSURE_NOT_NULL( order);
  XSH_ASSURE_NOT_NULL( slit);


  table=cpl_table_new(size);
  cpl_table_new_column(table,"WAVELENGTH",CPL_TYPE_DOUBLE); 
  cpl_table_new_column(table,"ORDER",CPL_TYPE_DOUBLE); 
  cpl_table_new_column(table,"X",CPL_TYPE_DOUBLE); 
  cpl_table_new_column(table,"Y",CPL_TYPE_DOUBLE);
  cpl_table_new_column(table,"S",CPL_TYPE_DOUBLE); 

  cpl_table_fill_column_window(table,"WAVELENGTH",0,size,0);
  cpl_table_fill_column_window(table,"ORDER",0,size,0);
  cpl_table_fill_column_window(table,"X",0,size,0);
  cpl_table_fill_column_window(table,"Y",0,size,0);
  cpl_table_fill_column_window(table,"S",0,size,0);

  po=cpl_table_get_data_double(table,"ORDER");
  pw=cpl_table_get_data_double(table,"WAVELENGTH");
  px=cpl_table_get_data_double(table,"X");
  py=cpl_table_get_data_double(table,"Y");
  ps=cpl_table_get_data_double(table,"S");
 
  for(i = 0 ; i<size; i++) {
    pw[i] = lambda[i];
    po[i] = order[i];
    ps[i] = slit[i];
    check( px[i] = xsh_wavesol_eval_polx(wsol,pw[i],po[i],ps[i]));
    check( py[i] = xsh_wavesol_eval_poly(wsol,pw[i],po[i],ps[i]));
  }

  cleanup:
    return table;
}

/**
  @brief
    Set the bin of wave table in x
  @param[in] wsol
    The wavelength solution list to update
  @param[in] bin
    The binning value in x
*/
void xsh_wavesol_set_bin_x( xsh_wavesol * wsol, int bin )
{
  XSH_ASSURE_NOT_NULL( wsol);
  wsol->bin_x = bin;

  cleanup:
    return;
}

/**
  @brief
    Set the bin of wave table in y
  @param[in] wsol
    The wavelength solution list to update
  @param[in] bin
    The binning value in y
*/
void xsh_wavesol_set_bin_y( xsh_wavesol * wsol, int bin )
{
  XSH_ASSURE_NOT_NULL( wsol);
  wsol->bin_y = bin;

  cleanup:
    return;
}

/**
  @brief
    Apply a shift on X and Y to wave solution
  @param[in] wsol
    The wavesolution to shift
  @param[in] shift_x
    The shift in X
  @param[in] shift_y
    The shift in Y
*/
void xsh_wavesol_apply_shift( xsh_wavesol *wsol, float shift_x, float shift_y)
{

  XSH_ASSURE_NOT_NULL( wsol);

  wsol->min_x = wsol->min_x+shift_x;
  wsol->max_x = wsol->max_x+shift_x;
  wsol->min_y = wsol->min_y+shift_y;
  wsol->max_y = wsol->max_y+shift_y;
  
  cleanup:
    return;
}
/**@}*/
