/*                                                                              *
 *   This file is part of the ESO UVES Pipeline                                 *
 *   Copyright (C) 2004,2005 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-01-12 16:44:43 $
 * $Revision: 1.68 $
 * $Name: not supported by cvs2svn $
 * $Log: not supported by cvs2svn $
 * Revision 1.67  2011/12/08 14:03:32  amodigli
 * Fix warnings with CPL6
 *
 * Revision 1.66  2010/09/24 09:32:08  amodigli
 * put back QFITS dependency to fix problem spot by NRI on FIBER mode (with MIDAS calibs) data
 *
 * Revision 1.64  2007/09/11 17:08:49  amodigli
 * mooved uves_polynomial_convert_from_plist_midas to uves_dfs
 *
 * Revision 1.63  2007/08/21 13:08:26  jmlarsen
 * Removed irplib_access module, largely deprecated by CPL-4
 *
 * Revision 1.62  2007/06/20 15:34:50  jmlarsen
 * Changed indentation
 *
 * Revision 1.61  2007/06/20 08:30:00  amodigli
 * added index parameter to support FIBER mode lintab in uves_polynomial_convert_from_plist_midas
 *
 * Revision 1.60  2007/06/06 08:17:33  amodigli
 * replace tab with 4 spaces
 *
 * Revision 1.59  2007/05/03 15:23:08  jmlarsen
 * Removed dead code
 *
 * Revision 1.58  2007/05/03 15:18:29  jmlarsen
 * Added function to add polynomials
 *
 * Revision 1.57  2007/04/27 07:21:51  jmlarsen
 * Polyfit: Changed error code from ILLEGAL_INPUT to SINGULAR_MATRIX
 *
 * Revision 1.56  2007/04/24 12:50:29  jmlarsen
 * Replaced cpl_propertylist -> uves_propertylist which is much faster
 *
 * Revision 1.55  2007/03/23 08:01:55  jmlarsen
 * Fixed mixed code and declarations
 *
 * Revision 1.54  2007/03/19 15:10:03  jmlarsen
 * Optimization in 2d fitting: do not call pow too often
 *
 * Revision 1.53  2007/03/13 15:35:11  jmlarsen
 * Made a few time optimizations
 *
 * Revision 1.52  2007/03/05 10:20:49  jmlarsen
 * Added uves_polynomial_delete_const()
 *
 * Revision 1.51  2007/01/15 08:48:51  jmlarsen
 * Shortened lines
 *
 * Revision 1.50  2006/11/24 09:36:49  jmlarsen
 * Workaround for slow uves_propertylist_get_size
 *
 * Revision 1.49  2006/11/15 15:02:15  jmlarsen
 * Implemented const safe workarounds for CPL functions
 *
 * Revision 1.47  2006/11/15 14:04:08  jmlarsen
 * Removed non-const version of parameterlist_get_first/last/next which is
 * already in CPL, added const-safe wrapper, unwrapper and deallocator functions
 *
 * Revision 1.46  2006/11/13 14:23:55  jmlarsen
 * Removed workarounds for CPL const bugs
 *
 * Revision 1.45  2006/11/06 15:19:42  jmlarsen
 * Removed unused include directives
 *
 * Revision 1.44  2006/09/08 14:06:29  jmlarsen
 * Removed profiling code
 *
 * Revision 1.43  2006/09/06 14:46:21  jmlarsen
 * Added missing newline in uves_polynomial_dump()
 *
 * Revision 1.42  2006/08/17 14:11:25  jmlarsen
 * Use assure_mem macro to check for memory allocation failure
 *
 * Revision 1.41  2006/08/17 13:56:53  jmlarsen
 * Reduced max line length
 *
 * Revision 1.40  2006/07/03 13:27:52  jmlarsen
 * Moved failing assertion to where it should be
 *
 * Revision 1.39  2006/06/01 14:43:17  jmlarsen
 * Added missing documentation
 *
 * Revision 1.38  2006/05/05 13:59:03  jmlarsen
 * Support fitting zero-degree polynomial
 *
 * Revision 1.37  2006/04/24 09:28:29  jmlarsen
 * Added function to create zero-polynomial
 *
 * Revision 1.36  2006/03/27 09:41:37  jmlarsen
 * Added timing markers
 *
 * Revision 1.35  2006/03/09 10:52:25  jmlarsen
 * Renamed pow->power
 *
 * Revision 1.34  2006/03/03 13:54:11  jmlarsen
 * Changed syntax of check macro
 *
 * Revision 1.33  2005/12/19 16:17:56  jmlarsen
 * Replaced bool -> int
 *
 */

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

/*----------------------------------------------------------------------------*/
/**
   @defgroup uves_utils_polynomial  Polynomials

   This module provides N dimensional polynomials.

   This class is a wrapper of CPL's polynomial class, but it improves the accuracy of
   the fitting routine (related to DFS ticket: DFS02237), and it allows fitting
   with a 2d polynomial with different degree of the independent variables (which CPL does not
   support), and also allows propagation of the uncertainty of the fit.

   Also, the module adds simple functionalities like shifting a 2d polynomial,
   collapsing a 2d polynomial to a 1d polynomial, and conversion of a
   polynomial to/from a CPL table (which can be used for I/O).

   The functionality in this module has been implemented only as needed. Therefore,
   1) some functionality which "should" to be there (like collapsing a polynomial
   of any dimension) is missing, but 2) all the
   functionality present has been tested.
 */
/*----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------
                                Defines
 -----------------------------------------------------------------------------*/

/*
 *  When storing a 2d polynomial in a table,
 *  these column names are used
 */
#define COLUMN_ORDER1 "Order1"
#define COLUMN_ORDER2 "Order2"
#define COLUMN_COEFF  "Coeff"

/**@{*/

/*-----------------------------------------------------------------------------
                                Includes
 -----------------------------------------------------------------------------*/
#include <uves_utils_polynomial.h>

#include <uves_utils.h>
#include <uves_utils_wrappers.h>
#include <uves_dump.h>
#include <uves_msg.h>
#include <uves_error.h>

#include <cpl.h>

/*-----------------------------------------------------------------------------
                            Typedefs
 -----------------------------------------------------------------------------*/
/** The value of a _polynomial(x)  is
    cpl_polynomial((x - shift_x)/scale_x) * scale_y + shift_y  */
struct _polynomial 
{
    /** CPL polynomial */
    cpl_polynomial *pol; 

    /** Used internally, for efficiency */
    cpl_vector *vec;
    double *vec_data;

    int dimension;  /* for efficiency */

    /** shift[0] = shift of p(x)  ;   shift[i>0] = shift of x_i  */
    double *shift;

    /** scale[0] = scale of p(x)  ;   scale[i>0] = scale of x_i  */
    double *scale;
};

/*-----------------------------------------------------------------------------
                            Implementation
 -----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
/**
   @brief    Create a polynomial
   @param    pol             The CPL polynomial to wrap
   @return   A new polynomial, which must be deallocated with 
             @c uves_polynomial_delete(), or NULL on error.

   @note The provided CPL polynomial is duplicated and must still
   be deallocated independently from the polynomial returned from 
   this function.
*/
/*----------------------------------------------------------------------------*/
polynomial *
uves_polynomial_new(const cpl_polynomial *pol)
{
    polynomial *p = NULL;
    int i;
    
    /* Test input */
    assure(pol != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");

    /* Allocate and initialize struct */
    p = cpl_calloc(1, sizeof(polynomial)) ;
    assure_mem( p );

    check( p->dimension = cpl_polynomial_get_dimension(pol), "Error reading dimension");

    /* Allocate vector */
    p->vec = cpl_vector_new(p->dimension);
    assure_mem( p->vec );
    p->vec_data = cpl_vector_get_data(p->vec);

    /* Shifts are initialized to zero, scales to 1 */
    p->shift = cpl_calloc(p->dimension + 1, sizeof(double));
    assure_mem( p->shift );

    p->scale = cpl_malloc((p->dimension + 1) * sizeof(double));
    assure_mem( p->scale );
    for (i = 0; i <= p->dimension; i++)
    p->scale[i] = 1.0;

    check( p->pol = cpl_polynomial_duplicate(pol), "Error copying polynomial");
    
  cleanup:
    if (cpl_error_get_code() != CPL_ERROR_NONE)
    uves_polynomial_delete(&p);
    
    return p;
}

/*----------------------------------------------------------------------------*/
/**
   @brief    Create a zero polynomial
   @param    dim             Dimension of polynomial
   @return   A new polynomial, which must be deallocated with 
             @c uves_polynomial_delete(), or NULL on error.

*/
/*----------------------------------------------------------------------------*/
polynomial *
uves_polynomial_new_zero(int dim)
{
    polynomial *result = NULL;
    cpl_polynomial *p = NULL;

    assure( dim >= 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dim);

    p = cpl_polynomial_new(dim);
    assure_mem( p );

    result = uves_polynomial_new(p);
    assure_mem( result );

  cleanup:
    uves_free_polynomial(&p);

    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Delete a polynomial 
  @param    p    polynomial to delete

  @em p is deleted and set to NULL.
 */
/*----------------------------------------------------------------------------*/
void 
uves_polynomial_delete(polynomial **p)
{
    uves_polynomial_delete_const((const polynomial **)p);
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Delete a const polynomial 
  @param    p    polynomial to delete

  @em p is deleted and set to NULL.
 */
/*----------------------------------------------------------------------------*/
void 
uves_polynomial_delete_const(const polynomial **p)
{
    if (*p == NULL) return;
    cpl_polynomial_delete((*p)->pol);
    cpl_vector_delete((*p)->vec);
    cpl_free((*p)->shift);
    cpl_free((*p)->scale);
    uves_free(*p);
    *p = NULL;
    return;
}
/*----------------------------------------------------------------------------*/
/**
  @brief    Get degree
  @param    p    polynomial
  @return degree
 */
/*----------------------------------------------------------------------------*/
int
uves_polynomial_get_degree(const polynomial *p)
{
    int result = -1;
    assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    
    result = cpl_polynomial_get_degree(p->pol);

  cleanup:
    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Copy a polynomial 
  @param    p    polynomial to copy
  @return   A clone of the input polynomial or NULL on error.
 */
/*----------------------------------------------------------------------------*/
polynomial *
uves_polynomial_duplicate(const polynomial *p)
{
    polynomial *result = NULL;
    int dimension;
    int i;

    assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    dimension = uves_polynomial_get_dimension(p);

    check( result = uves_polynomial_new(p->pol),
       "Error allocating polynomial");
    
    for (i = 0; i <= dimension; i++)
    {
        result->shift[i] = p->shift[i];
        result->scale[i] = p->scale[i];
    }

  cleanup:
    if (cpl_error_get_code() != CPL_ERROR_NONE)
    {
        uves_polynomial_delete(&result);
        return NULL;
    }
    
    return result;
}


/*----------------------------------------------------------------------------*/
/**
  @brief    Convert a polynomial to a table
  @param    p    polynomial to convert
  @return   A table representation of the polynomial, or NULL on error.

  Currently, only 2d polynomials are supported. The polynomial is written
  to the table in an internal format ; Therefore the table should not be read or
  edited manually, but only read using the function
  @c uves_polynomial_convert_from_table() .
 */
/*----------------------------------------------------------------------------*/
cpl_table *
uves_polynomial_convert_to_table(const polynomial *p)
{
    cpl_table *t = NULL; /* Result */
    int degree;
    int i, j, row;

    /* Check input */
    assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure( uves_polynomial_get_dimension(p) == 2, 
        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2D");
    
    degree = cpl_polynomial_get_degree(p->pol);

    /* Allocate space for 3 shifts, 3 scale factors and all
       coefficients */
    t = cpl_table_new(3 + 3 + (degree + 1)*(degree + 2)/2);
    cpl_table_new_column(t, COLUMN_ORDER1, CPL_TYPE_INT);
    cpl_table_new_column(t, COLUMN_ORDER2, CPL_TYPE_INT);
    cpl_table_new_column(t, COLUMN_COEFF , CPL_TYPE_DOUBLE);


    row = 0;

    /* First write the shifts, write non-garbage to coeff columns (which are not used) */
    cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
    cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
    cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[0]); row++;

    cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
    cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
    cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[1]); row++;

    cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
    cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
    cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[2]); row++;

    /* Then the scale factors */
    cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
    cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
    cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[0]); row++;

    cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
    cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
    cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[1]); row++;

    cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
    cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
    cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[2]); row++;

    /* And then write the coefficients */
    for (i = 0; i <= degree; i++){
    for (j = 0; j+i <= degree; j++){
        double coeff;
        cpl_size power[2];
        power[0] = i;
        power[1] = j;
        
        coeff = cpl_polynomial_get_coeff(p->pol, power);
        cpl_table_set_int   (t, COLUMN_ORDER1, row, power[0]);
        cpl_table_set_int   (t, COLUMN_ORDER2, row, power[1]);
        cpl_table_set_double(t, COLUMN_COEFF , row, coeff);
        
        row++;
    }
    }
    cpl_table_set_column_unit(t, COLUMN_ORDER1 , " ");
    cpl_table_set_column_unit(t, COLUMN_ORDER2 , " ");
    cpl_table_set_column_unit(t, COLUMN_COEFF , " ");

  cleanup:
    return t;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Convert a table to a polynomial
  @param    t    Table to convert
  @return   The polynomial stored in the table, which must be deallocated
            with @c uves_polynomial_delete(), or NULL on error.

  Currently, only 2d polynomials are supported. See also @c uves_polynomial_convert_to_table() .
 */
/*----------------------------------------------------------------------------*/
polynomial *
uves_polynomial_convert_from_table(cpl_table *t)
{
    polynomial *p = NULL;  /* Result */
    cpl_polynomial *pol = NULL;
    cpl_type type;
    int i;
    
    /* Only 2d supported */
    check( pol = cpl_polynomial_new(2), "Error initializing polynomial");

    /* Check table format */
    assure(t != NULL, CPL_ERROR_NULL_INPUT, "Null table");
    assure(cpl_table_has_column(t, COLUMN_ORDER1), CPL_ERROR_ILLEGAL_INPUT, 
       "No '%s' column found in table", COLUMN_ORDER1);
    assure(cpl_table_has_column(t, COLUMN_ORDER2), CPL_ERROR_ILLEGAL_INPUT,
       "No '%s' column found in table", COLUMN_ORDER2);
    assure(cpl_table_has_column(t, COLUMN_COEFF ), CPL_ERROR_ILLEGAL_INPUT,
       "No '%s' column found in table", COLUMN_COEFF );
    
    type = cpl_table_get_column_type(t, COLUMN_ORDER1);
    assure(type == CPL_TYPE_INT   , CPL_ERROR_INVALID_TYPE,
       "Column '%s' has type %s. Integer expected", COLUMN_ORDER1,
       uves_tostring_cpl_type(type));
    
    type = cpl_table_get_column_type(t, COLUMN_ORDER2);
    assure(type == CPL_TYPE_INT   , CPL_ERROR_INVALID_TYPE,
       "Column '%s' has type %s. Integer expected", COLUMN_ORDER2,
       uves_tostring_cpl_type(type));
    
    type = cpl_table_get_column_type(t, COLUMN_COEFF);
    assure(type == CPL_TYPE_DOUBLE, CPL_ERROR_INVALID_TYPE,
       "Column '%s' has type %s. Double expected", COLUMN_COEFF ,
       uves_tostring_cpl_type(type));

    assure(cpl_table_get_nrow(t) > 1 + 2 + 1 + 2, CPL_ERROR_ILLEGAL_INPUT,
       "Table must contain at least one coefficient");
    
    /* Read the coefficients */
    for(i = 3 + 3; i < cpl_table_get_nrow(t); i++) {
    double coeff;
    cpl_size power[2];
    
    check(( power[0] = cpl_table_get_int(t, COLUMN_ORDER1, i, NULL),
        power[1] = cpl_table_get_int(t, COLUMN_ORDER2, i, NULL),
        coeff  = cpl_table_get_double(t, COLUMN_COEFF , i, NULL)),
           "Error reading table row %d", i);
    
    uves_msg_debug("Pol.coeff.(%" CPL_SIZE_FORMAT ", %" CPL_SIZE_FORMAT ") = %e", power[0], power[1], coeff);

    check( cpl_polynomial_set_coeff(pol, power, coeff), "Error creating polynomial");
    }
    p = uves_polynomial_new(pol);

    /* Read shifts and rescaling */
    uves_polynomial_rescale(p, 0, cpl_table_get_double( t, COLUMN_COEFF, 3, NULL));
    uves_polynomial_rescale(p, 1, cpl_table_get_double( t, COLUMN_COEFF, 4, NULL));
    uves_polynomial_rescale(p, 2, cpl_table_get_double( t, COLUMN_COEFF, 5, NULL));
    uves_polynomial_shift  (p, 0, cpl_table_get_double( t, COLUMN_COEFF, 0, NULL));
    uves_polynomial_shift  (p, 1, cpl_table_get_double( t, COLUMN_COEFF, 1, NULL));
    uves_polynomial_shift  (p, 2, cpl_table_get_double( t, COLUMN_COEFF, 2, NULL));

  cleanup:
    uves_free_polynomial(&pol);
    if (cpl_error_get_code() != CPL_ERROR_NONE)
    uves_polynomial_delete(&p);

    return p;
}


/*----------------------------------------------------------------------------*/
/**
  @brief    Get the dimension of a polynomial
  @param    p       The input polynomial
  @return   The dimension of @em p, undefined in case of error.
*/
/*----------------------------------------------------------------------------*/
int
uves_polynomial_get_dimension(const polynomial *p)
{
    int dim = -1;
    assure(p != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");

/* slow     check( dim = cpl_polynomial_get_dimension(p->pol), "Error reading dimension"); */
    dim = p->dimension;
    
  cleanup:
    return dim;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Print a polynomial
  @param    p       The polynomial to print
  @param    stream  Where to dump the polynomial (e.g. "stdout")

  This function does not use CPL's messaging system and should be used only for debugging.
*/
/*----------------------------------------------------------------------------*/
void uves_polynomial_dump(const polynomial *p, FILE *stream)
{
    if (p == NULL)
    fprintf(stream, "Null polynomial\n");
    else {
    int i;
    cpl_polynomial_dump(p->pol, stream);
    fprintf(stream, "shift_y \t= %f  \tscale_y \t= %f\n", p->shift[0], p->scale[0]);
    for (i = 1; i <= uves_polynomial_get_dimension(p); i++)
        {
        fprintf(stream, "shift_x%d \t= %f  \tscale_x%d \t= %f\n", 
            i, p->shift[i], i, p->scale[i]);
        }
    }
    return;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Rescale a polynomial
  @param    p       The polynomial to rescale
  @param    varno   Rescale with respect to this variable (number)
  @param    scale   The rescaling factor
  @return  CPL_ERROR_NONE iff OK

  The variable specified by @em varno is rescaled:
  @em p (x_1, ..., x_varno, ..., x_n) :=@em p (x_1, ..., x_varno / @em scale, ..., x_n).

  If @em varno is zero, a the polynomial itself is rescaled: @em p(x) := p(x) * @em scale .
  Negative values of @em varno are illegal.
*/
/*----------------------------------------------------------------------------*/
cpl_error_code
uves_polynomial_rescale(polynomial *p, int varno, double scale)
{
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(0 <= varno && varno <= uves_polynomial_get_dimension(p), 
       CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);

    /*  Rescaling an x variable by the factor S corresponds to:  
     *    p'(x) := p(x/S)  =
     *  cpl( (x/S -  shiftx ) /    scalex  ) * scaley + shifty  = 
     *  cpl( (x - (S*shiftx)) / (S*scalex) ) * scaley + shifty      */

    /*  Rescaling the y variable by the factor S corresponds to:  
     *    p'(x) := S*p(x)  =
     *  S * ( cpl((x - shiftx)/scalex) * scaley     + shifty )  = 
     *        cpl((x - shiftx)/scalex) * (S*scaley) + (S*shifty) 
     *
     *  therefore the implementation is the same in the two cases. */
     
    p->shift[varno] *= scale;
    p->scale[varno] *= scale;

  cleanup:
    return cpl_error_get_code();
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Shift a polynomial
  @param    p       The polynomial to shift
  @param    varno   Shift with respect to this variable (number)
  @param    shift   The amount to shift
  @return  CPL_ERROR_NONE iff OK

  The polynomial is shifted: 
  @em p (x_1, ..., x_varno, ..., x_n) :=@em p (x_1, ..., x_varno - @em shift, ..., x_n).

  If @em varno is zero, a constant is added to the polynomial: @em p(x) := p(x) + @em shift .
  Negative values of @em varno are illegal.
*/
/*----------------------------------------------------------------------------*/
cpl_error_code
uves_polynomial_shift(polynomial *p, int varno, double shift)
{
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(0 <= varno && varno <= uves_polynomial_get_dimension(p), 
       CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);

    /* The implementation is similar for x and y variables because
     *  p(x-S)  =
     *  cpl( (x-S - shiftx)   / scalex ) * scaley + shifty  = 
     *  cpl( (x - (shiftx+S)) / scalex ) * scaley + shifty
     * and
     *  p(x) + S  =
     *  cpl( (x - shiftx)/scalex ) * scaley + shifty + S  = 
     *  cpl( (x - shiftx)/scalex ) * scaley + (shifty+S)      */

    p->shift[varno] += shift;

  cleanup:
    return cpl_error_get_code();
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Evaluate a 1d polynomial
  @param    p       The polynomial to evaluate
  @param    x       Where to evaluate the polynomial
  @return   @em p ( @em x ), or undefined on error.

  The polynomial must be 1d. See also @c uves_polynomial_evaluate_2d() .
*/
/*----------------------------------------------------------------------------*/
double
uves_polynomial_evaluate_1d(const polynomial *p, double x)
{
    double result = 0;
    
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(uves_polynomial_get_dimension(p) == 1, 
       CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
    
    check( result = 
       cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], NULL)
       * p->scale[0] + p->shift[0],
       "Could not evaluate polynomial");
    
  cleanup:
    return result;
}


/*----------------------------------------------------------------------------*/
/**
  @brief    Evaluate a 2d polynomial
  @param    p       The polynomial to evaluate
  @param    x1      Where to evaluate the polynomial
  @param    x2      Where to evaluate the polynomial
  @return   @em p ( @em x1 ,@em x2 ), or undefined on error.

  The polynomial must be 2d. See also @c uves_polynomial_evaluate_1d() .
*/
/*----------------------------------------------------------------------------*/

double
uves_polynomial_evaluate_2d(const polynomial *p, double x1, double x2)
{
    double result = 0;

    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(p->dimension == 2, CPL_ERROR_ILLEGAL_INPUT,
       "Polynomial must be 2d. It's %dd", p->dimension);
    {
        double scale = p->scale[0];
        double shift = p->shift[0];

        //    cpl_vector_set(p->vec, 0, (x1 - p->shift[1]) / p->scale[1]);
        //    cpl_vector_set(p->vec, 1, (x2 - p->shift[2]) / p->scale[2]);
        p->vec_data[0] = (x1 - p->shift[1]) / p->scale[1];
        p->vec_data[1] = (x2 - p->shift[2]) / p->scale[2];
        
        result = cpl_polynomial_eval(p->pol, p->vec) * scale + shift;
    }

  cleanup:
    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Solve p(x) = value
  @param    p       The input polynomial
  @param    value   The requested value of the polynomial
  @param    guess   A guess solution
  @param    multiplicity The multiplycity of the root (or 1 if unknown)
  @return   x satisfying the equation @em p (x) = @em value, or undefined on error.

  This function uses @c cpl_polynomial_solve_1d() to solve the equation 
    @em p (x) = @em value .
  See @c cpl_polynomial_solve_1d() for a description of the algorithm.
*/
/*----------------------------------------------------------------------------*/
double
uves_polynomial_solve_1d(const polynomial *p, double value, double guess, int multiplicity)
{
    double result = 0;
    cpl_size power[1];
    double coeff0;

    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(uves_polynomial_get_dimension(p) == 1, CPL_ERROR_ILLEGAL_INPUT, 
       "Polynomial must be 1d");
    
    /* Solving p(x) = value corresponds to solving
       <=>    cpl_p( (x-xshift)/xscale )*yscale + yshift = value
       <=>    cpl_p( (x-xshift)/xscale ) + (yshift - value)/yscale = 0 

       So   1) find zero point for the polynomial   cpl() + (yshift-value)/yscale
       Then 2) shift and rescale the result
    */

    power[0] = 0;
    check(( coeff0 = cpl_polynomial_get_coeff(p->pol, power),
        cpl_polynomial_set_coeff(p->pol, power, coeff0 + (p->shift[0] - value)/p->scale[0])),
      "Error setting coefficient");

    check( cpl_polynomial_solve_1d(p->pol, (guess - p->shift[1]) / p->scale[1],
                   &result, multiplicity), "Could not find root");
    /* Restore polynomial */
    cpl_polynomial_set_coeff(p->pol, power, coeff0);
    
    /* Shift solution */
    result = result * p->scale[1] + p->shift[1];

  cleanup:
    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Solve p(x1, x2) = value
  @param    p            The input polynomial
  @param    value        The requested value of the polynomial
  @param    guess        A guess solution
  @param    multiplicity The multiplycity of the root (or 1 if unknown)
  @param    varno        The variable number to fix (1 or 2)
  @param    x_value      Variable number @em varno is fixed to this value
  @return   The solution of the equation, or undefined on error.

  This function solves the equation @em p (x1, x2) = @em value, where either x1 or x2
  is already fixed to @em x_value. 

  For example, to solve the equation @em p (37, x) = 500 for x, 
  call @c uves_polynomial_solve_2d(p, 500, x_guess, 1, 1, 37) .
*/
/*----------------------------------------------------------------------------*/
double
uves_polynomial_solve_2d(const polynomial *p, double value, double guess,
             int multiplicity, int varno, double x_value)
{
    double result = 0;
    polynomial *pol_1d = NULL;

    assure( 1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
        "Illegal variable number: %d", varno);

    check( pol_1d = uves_polynomial_collapse(p, varno, x_value),
       "Could not collapse polynomial");

    check( result = uves_polynomial_solve_1d(pol_1d, value, guess, multiplicity),
       "Could not find root");

  cleanup:
    uves_polynomial_delete(&pol_1d);
    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Evaluate the partial derivative of a 2d polynomial
  @param    p            The input polynomial
  @param    x1           Where to evaluate the derivative
  @param    x2           Where to evaluate the derivative
  @param    varno        Evaluate partial derivative with respect to this variable (1 or 2)
  @return   dp/dx_varno evaluated at (x1, x2), or undefined on error.
*/
/*----------------------------------------------------------------------------*/
double
uves_polynomial_derivative_2d(const polynomial *p, double x1, double x2, int varno)
{
    double result = 0;
    cpl_size power[2];

    assure (1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
        "Illegal variable number (%d)", varno);

    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(uves_polynomial_get_dimension(p) == 2, CPL_ERROR_ILLEGAL_INPUT,
       "Polynomial must be 2d. It's %dd", uves_polynomial_get_dimension(p));

    /*  d/dx_i [ p(x) ] =
     *  d/dx_i [ cpl( (x - shiftx) / scalex ) * scaley + shifty ] = 
     *  [ d(cpl)/dx_i ( (x - shiftx) / scalex ) * scaley ]
     */

    /* Shift, scale  (x1, x2) */
    x1 = (x1 - p->shift[1])/p->scale[1];
    x2 = (x2 - p->shift[2])/p->scale[2];
 
    /* Get derivative of cpl polynomial.
     * 
     */
    {
    int degree = cpl_polynomial_get_degree(p->pol);
    double yj = 1;  /* y^j */
    int i, j;
    
    result = 0;
    for (j = 0, yj = 1;
         j <= degree; j++,
         yj *= (varno == 1) ? x2 : x1)
        {
        /*  Proof by example (degree = 3): For each j account for these terms
         *  using Horner's rule:
         *
         * d/dx     y^j * [  c_3j x^3 +  c_2j x^2 +  c_1j x^1 + c_0j ]   =
         *
         *          y^j * [ 3c_3j x^2 + 2c_2j x^1 + 1c_1j ]     =
         *
         *          y^j * [ ((3c_3j) x + 2c_2j) x + 1c_1j ]
         */

        double sum = 0;
        for (i = degree; i >= 1; i--)
            {
            double c_ij;

            power[0] = (varno == 1) ? i : j;
            power[1] = (varno == 1) ? j : i;
            
            c_ij = cpl_polynomial_get_coeff(p->pol, power);
            
            sum += (i * c_ij);
            if (i >= 2) sum *= (varno == 1) ? x1 : x2;
            }

        /* Collect terms */
        result += yj * sum;
        }
    }

    result *= p->scale[0];


/* Old code: This method (valid for varno = 2)
   of getting the derivative of
   the CPL polynomial is slow because of the call to 
   uves_polynomial_collapse()

   check( pol_1d = uves_polynomial_collapse(p, 1, x1);
   dummy = cpl_polynomial_eval_1d(pol_1d->pol, (x2 - pol_1d->shift[1])/pol_1d->scale[1], &result),
   "Error evaluating derivative");
*/
    
  cleanup:
    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Evaluate the derivative of a 1d polynomial
  @param    p            The input polynomial
  @param    x           Where to evaluate the derivative
  @return   dp/dx evaluated at x, or undefined on error.
*/
/*----------------------------------------------------------------------------*/
double
uves_polynomial_derivative_1d(const polynomial *p, double x)
{
    double result = 0;
    double dummy;

    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(uves_polynomial_get_dimension(p) == 1, 
       CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
    
    check( dummy = cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], &result),
       "Error evaluating derivative");
    
  cleanup:
    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Add two polynomials
  @param    p1          left
  @param    p2          right
  @return   p1 + p2
*/
/*----------------------------------------------------------------------------*/
polynomial *
uves_polynomial_add_2d(const polynomial *p1, const polynomial *p2)
{
    polynomial *result = NULL;
    cpl_polynomial *pol = NULL;

    assure(p1 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(p2 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(uves_polynomial_get_dimension(p1) == 2, 
       CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
    assure(uves_polynomial_get_dimension(p2) == 2, 
       CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");

    /* cpl_polynomial1((x - shift_x1)/scale_x1) * scale_y1 + shift_y1
       +
       cpl_polynomial2((x - shift_x2)/scale_x2) * scale_y2 + shift_y2
       = ???
       Not easy.

       Use brute force:
    */
    
    {
        int degree, i, j;

        degree = uves_max_int(uves_polynomial_get_degree(p1),
                              uves_polynomial_get_degree(p2));
        
        pol = cpl_polynomial_new(2);
        for (i = 0; i <= degree; i++)
            for (j = 0; j <= degree; j++) {
                double coeff1, coeff2;
                cpl_size power[2];

                /* Simple: add coefficients of the same power */
                coeff1 = uves_polynomial_get_coeff_2d(p1, i, j);
                coeff2 = uves_polynomial_get_coeff_2d(p2, i, j);
                
                power[0] = i;
                power[1] = j;
                cpl_polynomial_set_coeff(pol, power, coeff1 + coeff2);
            }
    }

    result = uves_polynomial_new(pol);
   
  cleanup:
    uves_free_polynomial(&pol);
    return result;
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Calculate the partial derivative of a CPL-polynomial
  @param    p           The input polynomial
  @param    varno       Differentiate with respect to this variable number
                        (counting from 1 to dimension)
  @return   CPL_ERROR_NONE iff okay.

  The polynomial is transformed from @em p to @em dp/dx_varno.

  1D and 2D polynomials are supported.

*/
/*----------------------------------------------------------------------------*/
static cpl_error_code
derivative_cpl_polynomial(cpl_polynomial *p, int varno)
{
    int dimension, degree;
    int i, j;
    cpl_size power[2];
    
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    dimension = cpl_polynomial_get_dimension(p);
    degree = cpl_polynomial_get_degree(p);
    assure( 1 <= dimension && dimension <= 2, CPL_ERROR_ILLEGAL_INPUT, 
        "Illegal dimension: %d", dimension);
    assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
        "Illegal variable number: %d", varno);
    
    if (dimension == 1)
    {
        /*  a_i := (i+1) * a_(i+1) */
        for(i = 0; i <= degree; i++)
        {
            double coeff;
            power[0] = i+1;
            /* power[1] is ignored */
            
            coeff = cpl_polynomial_get_coeff(p, power);
                
            power[0] = i;            
            cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
        }
    }
    
    if (dimension == 2)
    {
        /*  a_ij := (i+1) * a_{(i+1),j} */
        for(i = 0; i <= degree; i++)
        {
            for(j = 0; i + j <= degree; j++)
            {
                double coeff;
                power[varno - 1] = i+1;    /* varno == 1:    0,1  */ 
                power[2 - varno] = j;      /* varno == 2:    1,0  */
                
                coeff = cpl_polynomial_get_coeff(p, power);
                
                power[varno - 1] = i;
                
                cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
            }
        }
    }

  cleanup:
    return cpl_error_get_code();
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Calculate the partial derivative of a polynomial
  @param    p           The input polynomial
  @param    varno       Differentiate with respect to this variable number
                        (counting from 1 to dimension)
  @return   CPL_ERROR_NONE iff okay.

  The polynomial is transformed from @em p to @em dp/dx_varno.
*/
/*----------------------------------------------------------------------------*/
cpl_error_code
uves_polynomial_derivative(polynomial *p, int varno)
{
    int dimension;
    
    assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
    assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT, 
        "Illegal variable number: %d", varno);


    /*   d/dx_i [ cpl( (x - shiftx) / scalex ) * scaley + shifty ] = 
     *     sum_j d(cpl)/dx_j ( (x - shiftx) / scalex ) * scaley * dx_j/dx_i / scalex_j =
     *     d(cpl)/dx_i ( (x - shiftx) / scalex ) * scaley/scalex_i,
     * 
     * so transform :      shifty -> 0
     *                     shiftx -> shiftx
     *                     scaley -> scaley/scalex_i
     *                     scalex -> scalex
     *                       cpl  -> d(cpl)/dx_i
     */

    p->shift[0] = 0;
    p->scale[0] = p->scale[0] / p->scale[varno];

    check( derivative_cpl_polynomial(p->pol, varno),
       "Error calculating derivative of CPL-polynomial");
    
  cleanup:
    return cpl_error_get_code();
}


/*----------------------------------------------------------------------------*/
/**
  @brief    Get a coefficient of a 2D polynomial
  @param    p             The input polynomial
  @param    degree1       The coefficient degree
  @param    degree2       The coefficient degree
  @return   The coefficient of the term (degree1, degree2), or undefined on error.

*/
/*----------------------------------------------------------------------------*/
double
uves_polynomial_get_coeff_2d(const polynomial *p, int degree1, int degree2)
{
    polynomial *pp = NULL;
    int dimension;
    double result = 0;
    double factorial;
    
    assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
    assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
    assure( 0 <= degree1, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree1);
    assure( 0 <= degree2, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree2);

    /* Calculate the coefficient as
     * d^N p / (dx1^degree1 dx2^degree2)  /  (degree1! * degree2!)
     * evaluated in (0,0)
    */

    pp = uves_polynomial_duplicate(p);

    factorial = 1;
    while(degree1 > 0)
    {
        check( uves_polynomial_derivative(pp, 1), "Error calculating derivative");

        factorial *= degree1;
        degree1 -= 1;
    }

    while(degree2 > 0)
    {
        check( uves_polynomial_derivative(pp, 2), "Error calculating derivative");

        factorial *= degree2;
        degree2 -= 1;
    }
    
    check( result = uves_polynomial_evaluate_2d(pp, 0, 0) / factorial,
       "Error evaluating polynomial");
    
  cleanup:
    uves_polynomial_delete(&pp);
    return result;
}
/*----------------------------------------------------------------------------*/
/**
  @brief    Get a coefficient of a 1D polynomial
  @param    p             The input polynomial
  @param    degree        Coefficient degree
  @return   The coefficient of the degree'th term, or undefined on error.

  If the required degree is greater than the polynomial's degree, the function
  does not fail but returns 0 as it should.
*/
/*----------------------------------------------------------------------------*/
double
uves_polynomial_get_coeff_1d(const polynomial *p, int degree)
{
    polynomial *pp = NULL;
    int dimension;
    double result = 0;
    double factorial;
    
    assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
    assure(dimension == 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
    assure( 0 <= degree, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree);
    
    /* Calculate the coefficient as
     *  d^degree p/dx^degree  /  (degree1! * degree2!)
     * evaluated in 0.
     */
    
    pp = uves_polynomial_duplicate(p);
    
    factorial = 1;
    while(degree > 0)
    {
        check( uves_polynomial_derivative(pp, 1), "Error calculating derivative");
        
        factorial *= degree;
        degree -= 1;
    }
    
    check( result = uves_polynomial_evaluate_1d(pp, 0) / factorial,
       "Error evaluating polynomial");
    
  cleanup:
    uves_polynomial_delete(&pp);
    return result;
}


/*----------------------------------------------------------------------------*/
/**
  @brief    Collapse a polynomial by fixing one variable to a constant
  @param    p           The polynomial to collapse
  @param    varno       Variable number to fix
  @param    value       Fix variable number @em varno to this value
  @return   A newly allocated, collapsed polynomial which must be deallocated 
            with @c uves_polynomial_delete(), or NULL on error.

  This function fixes one variable of a polynomial to a constant value, 
  thereby producing a polynomial,
  p(x1, ..., x_varno = value, ..., xn), with dimension n - 1.

  Currently, only n=2 is supported.
  
*/
/*----------------------------------------------------------------------------*/
polynomial *
uves_polynomial_collapse(const polynomial *p, int varno, double value)
{
    polynomial     *result  = NULL;
    cpl_polynomial *pol     = NULL;
    cpl_size            *power     = NULL;

    int i, j;
    int degree, dimension;
    
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    dimension = uves_polynomial_get_dimension(p);
    assure(dimension  > 0, CPL_ERROR_ILLEGAL_INPUT,
       "Polynomial has non-positive dimension: %d", dimension);
    assure(dimension != 1, CPL_ERROR_ILLEGAL_OUTPUT,
       "Don't collapse a 1d polynomial. Evaluate it!");

    /* To generalize this function to work with dimensions higher than 2,
       also changes needs to be made below (use varno properly). For now,
       support only 2d. */
    assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
    
    assure(1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT, 
       "Wrong variable number");
    value = (value - p->shift[varno]) / p->scale[varno];

    /* Compute new coefficients */
    degree = cpl_polynomial_get_degree(p->pol);
    pol    = cpl_polynomial_new(dimension - 1);
    power = cpl_malloc(sizeof(cpl_size) * dimension);
    assure_mem( power );
    for (i = 0; i <= degree; i++) 
    {
        double coeff;
        
        power[2-varno] = i;   /* map 2->0  and 1->1 */
        
        /* Collect all terms with x^i  (using Horner's rule) */
        coeff = 0;
        for (j = degree - i; j >= 0; j--) 
        {
            power[varno-1] = j;  /* map 2->1 and 1->0 */
            coeff += cpl_polynomial_get_coeff(p->pol, power);
            if (j > 0) coeff *= value;
        }
        /* Write coefficient in 1d polynomial */
        power[0] = i;
        cpl_polynomial_set_coeff(pol, power, coeff);
    }
    
    /* Wrap the polynomial */
    result = uves_polynomial_new(pol);

    /* Copy the shifts and scales, skip variable number varno */
    j = 0;
    for(i = 0; i <= dimension - 1; i++) 
    {
        if (i == varno) 
        {
            /* Don't copy */
            j += 2;
            /* For the remainder of this for loop, j = i+1 */
        }
        else 
        {
            result->shift[i] = p->shift[j];
            result->scale[i] = p->scale[j];
            j += 1;
        }
    }
    
    assure(cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(), 
       "Error collapsing polynomial");
    
  cleanup:
    cpl_free(power); power = NULL;
    uves_free_polynomial(&pol);
    if (cpl_error_get_code() != CPL_ERROR_NONE)
    {
        uves_polynomial_delete(&result);
    }
    return result;
}



/*----------------------------------------------------------------------------*/
/**
  @brief    Fit a 1d function with a polynomial.
  @param    x_pos       List of positions of the signal to fit.
  @param    values      List of values of the signal to fit.
  @param    sigmas      List of uncertainties of the surface points.
                        If NULL, constant uncertainties are used.
  @param    poly_deg    Polynomial degree.
  @param    mse         Output mean squared error.
  @return   The fitted polynomial or NULL in error case
 
  This function is a straightforward adaption of CPL's @c cpl_polynomial_fit_1d_create() .
  But before performing the fit, all values are shifted, so that they are
  centered around zero. This improves the accuracy of the fit.

  Also, there's support for taking into account the uncertainties of the
  dependent variable.

  See also @c cpl_polynomial_fit_1d_create() and @c uves_polynomial_regression_1d() .
*/
/*----------------------------------------------------------------------------*/
polynomial * uves_polynomial_fit_1d(
    const cpl_vector    *   x_pos,
    const cpl_vector    *   values,
    const cpl_vector    *   sigmas,
    int                     poly_deg,
    double              *   mse)
{
    int                 nc ;
    int                 np ;
    cpl_matrix      *   ma = NULL;
    cpl_matrix      *   mb = NULL;
    cpl_matrix      *   mx = NULL;
    const double    *   x_pos_data ;
    const double    *   values_data ;
    const double    *   sigmas_data = NULL;
    double              mean_x, mean_z;
    polynomial      *   result = NULL;
    cpl_polynomial  *   out ;
    cpl_vector      *   x_val = NULL;
    int                 i, j ;
    
    /* Check entries */
    assure_nomsg( x_pos != NULL && values != NULL, CPL_ERROR_NULL_INPUT);
    assure( poly_deg >= 0, CPL_ERROR_ILLEGAL_INPUT, 
        "Polynomial degree is %d. Must be non-negative", poly_deg);
    np = cpl_vector_get_size(x_pos) ;
    
    nc = 1 + poly_deg ;
    assure( np >= nc, CPL_ERROR_ILLEGAL_INPUT,
        "Not enough points (%d) to fit %d-order polynomial. %d point(s) needed",
        np, poly_deg, nc);

    /* Fill up look-up table for coefficients to compute */
    /* Initialize matrices */
    /* ma contains the polynomial terms for each input point. */
    /* mb contains the values */
    ma = cpl_matrix_new(np, nc) ;
    mb = cpl_matrix_new(np, 1) ;

    /* Get mean values */
    mean_x = cpl_vector_get_mean(x_pos);
    mean_z = cpl_vector_get_mean(values);

    /* Fill up matrices, shift */
    x_pos_data = cpl_vector_get_data_const(x_pos) ;
    values_data = cpl_vector_get_data_const(values) ;
    if (sigmas != NULL)
    {
        sigmas_data = cpl_vector_get_data_const(sigmas) ;
    }

    if (sigmas != NULL)
    {
        for (i=0 ; i<np ; i++) 
        {
            /* Catch division by zero */
            if (sigmas_data[i] == 0)
            {
                uves_free_matrix(&ma) ;
                uves_free_matrix(&mb) ;
                assure(false, CPL_ERROR_DIVISION_BY_ZERO,
                   "Sigmas must be non-zero");
            }
            for (j=0 ; j<nc ; j++) 
            {
                cpl_matrix_set(ma, i, j,  
                       uves_pow_int(x_pos_data[i] - mean_x, j) /
                       sigmas_data[i]) ;
            }
            /* mb contains surface values (z-axis) */
            cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / sigmas_data[i]);
        }
    }
    else  /* Use sigma = 1 */
    {
        for (i=0 ; i<np ; i++) 
        {
            for (j=0 ; j<nc ; j++) 
            {
                cpl_matrix_set(ma, i, j,  
                       uves_pow_int(x_pos_data[i] - mean_x, j) / 1);
            }
            /* mb contains surface values (z-values) */
            cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
        }
    }
    
    /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
    check( mx = cpl_matrix_solve_normal(ma, mb),
       "Could not invert matrix");
    uves_free_matrix(&ma);
    uves_free_matrix(&mb);

    /* Store coefficients for output */
    out = cpl_polynomial_new(1) ;
    cpl_size deg=0;
    for (deg=0 ; deg<nc ; deg++) {
        cpl_polynomial_set_coeff(out, &deg, cpl_matrix_get(mx, deg, 0)) ;
    }
    uves_free_matrix(&mx);

    /* If requested, compute mean squared error */
    if (mse != NULL) {
        *mse = 0.00 ;
        x_val = cpl_vector_new(1) ;
        for (i=0 ; i<np ; i++)
        {
        double residual;
        cpl_vector_set(x_val, 0, x_pos_data[i] - mean_x) ;
        /* Subtract from the true value, square, accumulate */
        residual = (values_data[i] - mean_z) - cpl_polynomial_eval(out, x_val);
        *mse += residual*residual;
        }
        uves_free_vector(&x_val) ;
        /* Average the error term */
        *mse /= (double)np ;
    }

    /* Create and shift result */
    result = uves_polynomial_new(out);
    uves_free_polynomial(&out);

    uves_polynomial_shift(result, 0, mean_z);
    uves_polynomial_shift(result, 1, mean_x);

  cleanup:
    uves_free_vector(&x_val);
    uves_free_matrix(&ma);
    uves_free_matrix(&mb);
    uves_free_matrix(&mx);
    return result;
}


/*----------------------------------------------------------------------------*/
/**
  @brief    Fit a 2d surface with a polynomial in x and y.
  @param    xy_pos      List of positions of the surface to fit.
  @param    values      List of values of the surface points.
  @param    sigmas      List of uncertainties of the surface points.
  @param    poly_deg1   Polynomial degree of 1st variable (x)
  @param    poly_deg2   Polynomial degree of 2nd variable (y)
  @param    mse         Output mean squared error
  @param    red_chisq   Output reduced chi square
  @param    variance    Variance polynomial (see below)
  @return   The fitted polynomial or NULL in error case.
 
  This function fits a 2d polynomial to a surface. The input grid is
  given in xy_pos and values. xy_pos and values of course must contain
  the same number of points. If @em sigmas is NULL, constant sigma (equal to
  1) is used.
  
  This function is an adaption of CPL's @c cpl_polynomial_fit_2d_create() .
  But the fit is made with a general rectangular coefficient matrix (the
  size of which is indicated by the polynomial degrees, @em poly_deg1 and
  @em poly_deg2) instead of the upper-left triangular matrix used by 
  @c cpl_polynomial_fit_2d_create().

  And before performing the fit, all values are shifted, so that they are
  centered around zero, which improves the accuracy of the fit. Rescaling
  with stdev makes the fit worse (empirically) so this is not done.

  If @em mse is non-NULL, the mean squared error of the fit is returned through
  this variable. If @em red_chisq is non-NULL, the reduced chi square of the
  fit is returned through this variable.

  If @em variance is non-NULL the variance polynomial defined as (using the 
  error propagation formula for correlated coefficients {coeff_i})
  variance(x,y) = sum_{ij}  d(p_fit)/d(coeff_i) * cov_{ij} * d(p_fit)/d(coeff_j)
  = sum_{ij} x^degx[i]*y^degy[i] * cov_{ij} * x^degx[j]*y^degy[j]
  = sum_{ij} cov_{ij} * x^(degx[i]+degx[j]) * y^(degy[i]+degy[j])
  will be returned through this variable (i.e. the parameter must be the address of a
  @em (polynomial*) variable. The variance polynomial gives the associated uncertainty 
  when evaluating the fitted polynomial, i.e. the variance of
  p_fit(x, y) = sum_{ij} (a_{ij} * x^i * y^j)

  See also @c cpl_polynomial_fit_2d_create() and @c uves_polynomial_regression_2d() .
*/
/*----------------------------------------------------------------------------*/
polynomial *
uves_polynomial_fit_2d(
    const cpl_bivector     *  xy_pos,
    const cpl_vector       *  values,
    const cpl_vector       *  sigmas,
    int                       poly_deg1,
    int                       poly_deg2,
    double                 *  mse,
    double                 *  red_chisq,
    polynomial             ** variance)
{
    int                 nc ;
    int                 degx, degy ;
    int             *   degx_tab ;
    int             *   degy_tab ;
    int                 np ;
    cpl_matrix      *   ma ;
    cpl_matrix      *   mb ;
    cpl_matrix      *   mx ;
    cpl_matrix      *   mat;
    cpl_matrix      *   mat_ma;
    cpl_matrix      *   cov = NULL;
    const double    *   xy_pos_data_x ;
    const double    *   xy_pos_data_y ;
    const double    *   values_data ;
    const double    *   sigmas_data = NULL;
    const cpl_vector*   xy_pos_x;
    const cpl_vector*   xy_pos_y;
    double              mean_x, mean_y, mean_z;
    cpl_polynomial  *   out ;
    cpl_polynomial  *   variance_cpl ;
    polynomial      *   result         = NULL;
    cpl_size             *   powers ;

    /* Check entries */
    assure(xy_pos && values, CPL_ERROR_NULL_INPUT, "Null input");
    assure(poly_deg1 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree1 is %d", poly_deg1);
    assure(poly_deg2 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree2 is %d", poly_deg2);
    np = cpl_bivector_get_size(xy_pos) ;

    /* Can't calculate variance and chi_sq without sigmas */
    assure( (variance == NULL && red_chisq == NULL) || sigmas != NULL, 
        CPL_ERROR_ILLEGAL_INPUT, 
        "Cannot calculate variance or chi_sq without knowing");

    /* Fill up look-up table for coefficients to compute */
    nc = (1 + poly_deg1)*(1 + poly_deg2) ;     /* rectangular matrix */
    
    assure(np >= nc, CPL_ERROR_SINGULAR_MATRIX, "%d coefficients. Only %d points", nc, np);
    /* The error code here is set to SINGULAR_MATRIX, in order to allow the caller
       to detect when too many coefficients are fitted to too few points */

    /* Need an extra point to calculate reduced chi^2 */
    assure(red_chisq == NULL || np > nc, CPL_ERROR_ILLEGAL_INPUT, 
       "%d coefficients. %d points. Cannot calculate chi square", nc, np);
    
    degx_tab = cpl_malloc(nc * sizeof(int)) ;
    assure_mem( degx_tab );

    degy_tab = cpl_malloc(nc * sizeof(int)) ;
    if (degy_tab == NULL) {
    cpl_free(degx_tab);
    assure_mem( false );
    }

    {
        int i=0 ;
        for (degy=0 ; degy<=poly_deg2 ; degy++) {     /* rectangular matrix */
            for (degx=0 ; degx<=poly_deg1 ; degx++) {
                degx_tab[i] = degx ;
                degy_tab[i] = degy ;
                i++ ;
            }
        }
    }
    
    /* Initialize matrices */
    /* ma contains the polynomial terms in the order described */
    /* above in each column, for each input point. */
    /* mb contains the values */
    ma = cpl_matrix_new(np, nc) ;
    mb = cpl_matrix_new(np, 1) ;

    /* Get the mean of each variable */
    xy_pos_x = cpl_bivector_get_x_const(xy_pos);
    xy_pos_y = cpl_bivector_get_y_const(xy_pos);

    mean_x = cpl_vector_get_mean(xy_pos_x);
    mean_y = cpl_vector_get_mean(xy_pos_y);
    mean_z = cpl_vector_get_mean(values);

    /* Fill up matrices. At the same time shift the data
       so that it is centered around zero */
    xy_pos_data_x = cpl_vector_get_data_const(xy_pos_x) ;
    xy_pos_data_y = cpl_vector_get_data_const(xy_pos_y) ;
    values_data   = cpl_vector_get_data_const(values) ;
    if (sigmas != NULL)
    {
        sigmas_data = cpl_vector_get_data_const(sigmas) ;
    }

    if (sigmas != NULL)
    {
            int i;
        for (i=0 ; i<np ; i++) {
                double *ma_data = cpl_matrix_get_data(ma);
                double *mb_data = cpl_matrix_get_data(mb);

                int j = 0;
                double valy = 1;

        /* Catch division by zero */
        if (sigmas_data[i] == 0)
            {
            uves_free_matrix(&ma) ;
            uves_free_matrix(&mb) ;
            cpl_free(degx_tab) ;
            cpl_free(degy_tab) ;
            assure(false, CPL_ERROR_DIVISION_BY_ZERO,
                               "Sigmas must be non-zero. sigma[%d] is %f", i, sigmas_data[i]);
            }

                for (degy=0 ; degy<=poly_deg2 ; degy++) {
                    double valx = 1; 
                    for (degx=0 ; degx<=poly_deg1 ; degx++) {
                        ma_data[j + i*nc] = valx * valy / sigmas_data[i];
                        valx *= (xy_pos_data_x[i] - mean_x);
                        j++;
                    }
                    valy *= (xy_pos_data_y[i] - mean_y);
                }

        /* mb contains surface values (z-axis) */

        mb_data[0 + i*1] = (values_data[i] - mean_z) / sigmas_data[i];
        }
    }
    else  /* Use sigma = 1 */
    {
            int i;
        for (i=0 ; i<np ; i++) {
                double *ma_data = cpl_matrix_get_data(ma);
                double *mb_data = cpl_matrix_get_data(mb);

                double valy = 1;
                int j = 0;
                for (degy=0 ; degy<=poly_deg2 ; degy++) {
                    double valx = 1; 
                    for (degx=0 ; degx<=poly_deg1 ; degx++) {
                        ma_data[j + i*nc] = valx * valy / 1;
                        valx *= (xy_pos_data_x[i] - mean_x);
                        j++;
                    }
                    valy *= (xy_pos_data_y[i] - mean_y);
                }

        /* mb contains surface values (z-axis) */
//        cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
        mb_data[0 + i*1] = (values_data[i] - mean_z) / 1;
        }
    }
    
    /* If variance polynomial is requested, 
       compute covariance matrix = (A^T * A)^-1 */
    if (variance != NULL)
    {
        mat    = cpl_matrix_transpose_create(ma);
        if (mat != NULL)
        {
            mat_ma = cpl_matrix_product_create(mat, ma);
            if (mat_ma != NULL)
            {
                cov          = cpl_matrix_invert_create(mat_ma);
                /* Here, one might do a (paranoia) check that the covariance
                   matrix is symmetrical and has positive eigenvalues (so that
                   the returned variance polynomial is guaranteed to be positive) */

                variance_cpl = cpl_polynomial_new(2);
            }
        }
        uves_free_matrix(&mat);
        uves_free_matrix(&mat_ma);
    }

    /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
    mx = cpl_matrix_solve_normal(ma, mb) ;

    uves_free_matrix(&ma) ;
    uves_free_matrix(&mb) ;
    if (mx == NULL) {
        cpl_free(degx_tab) ;
        cpl_free(degy_tab) ;
    uves_free_matrix(&cov) ;
        assure(false, CPL_ERROR_ILLEGAL_OUTPUT, "Matrix inversion failed") ;
    }

    /* Store coefficients for output */
    out = cpl_polynomial_new(2) ;
    powers = cpl_malloc(2 * sizeof(cpl_size)) ;
    if (powers == NULL) {
        cpl_free(degx_tab) ;
        cpl_free(degy_tab) ;
    uves_free_matrix(&mx) ;
    uves_free_matrix(&cov) ;
    uves_free_polynomial(&out) ;
    assure_mem( false );
    }

    {
        int i;
    for (i = 0 ; i < nc ; i++)
    {
        powers[0] = degx_tab[i] ;
        powers[1] = degy_tab[i] ;
        cpl_polynomial_set_coeff(out, powers, cpl_matrix_get(mx, i, 0)) ;
        
        /* Create variance polynomial (if requested) */
        if (variance != NULL &&                   /* Requested? */
        cov != NULL && variance_cpl != NULL   /* covariance computation succeeded? */
        )
        {
                    int j;
            for (j = 0; j < nc; j++)
            {
                double coeff;
                /* Add cov_ij to the proper coeff:
                   cov_ij * dp/d(ai) * dp/d(aj) =
                   cov_ij * (x^degx[i] * y^degy[i]) * (x^degx[i] * y^degy[i]) =
                   cov_ij * x^(degx[i]+degx[j]) * y^(degy[i] + degy[j]),
                   
                   i.e. add cov_ij to coeff (degx[i]+degx[j], degy[i]+degy[j]) */
                powers[0] = degx_tab[i] + degx_tab[j] ;
                powers[1] = degy_tab[i] + degy_tab[j] ;
                
                coeff = cpl_polynomial_get_coeff(variance_cpl, powers);
                cpl_polynomial_set_coeff(variance_cpl, powers, 
                             coeff + cpl_matrix_get(cov, i, j)) ;
            }
        }
    }
    }
    
    cpl_free(powers) ;
    cpl_free(degx_tab) ;
    cpl_free(degy_tab) ;
    uves_free_matrix(&cov) ;
    uves_free_matrix(&mx) ;
    
    /* Create and shift result */
    result = uves_polynomial_new(out);
    uves_free_polynomial(&out);
    uves_polynomial_shift(result, 0, mean_z);
    uves_polynomial_shift(result, 1, mean_x);
    uves_polynomial_shift(result, 2, mean_y);

    /* Wrap up variance polynomial */
    if (variance != NULL)
    {
        *variance = uves_polynomial_new(variance_cpl);
        uves_free_polynomial(&variance_cpl);
            /* The variance of the fit does not change
           when a constant is added to the a_00
           coefficient of the polynomial, so don't:
           uves_polynomial_shift(*variance, 0, mean_z); */
        uves_polynomial_shift(*variance, 1, mean_x);
        uves_polynomial_shift(*variance, 2, mean_y);

        /* Maybe here add a consistency check that the variance polynomial is 
           positive at all input points */
    }  

    /* If requested, compute mean squared error */
    if (mse != NULL || red_chisq != NULL) 
    {
            int i;

        if (mse       != NULL) *mse = 0.00 ;
        if (red_chisq != NULL) *red_chisq = 0.00 ;
        for (i = 0 ; i < np ; i++) 
        {
            double regress = uves_polynomial_evaluate_2d(result, 
                                 xy_pos_data_x[i],
                                 xy_pos_data_y[i]);
            /* Subtract from the true value, square, accumulate */
            if (mse != NULL)
            {
                double residual = values_data[i] - regress;
                *mse += residual*residual;
            }
            if (red_chisq != NULL)
            {
                *red_chisq += uves_pow_int((values_data[i] - regress) /
                               sigmas_data[i], 2);
            }
        }
        /* Get average */
        if (mse       != NULL)  *mse       /= (double) np ;
        
        if (red_chisq != NULL)
        {
            passure( np > nc, "%d %d", np, nc); /* Was already checked */
            *red_chisq /= (double) (np - nc) ;
        }
    }

  cleanup:
    return result ;
}


/**@}*/