xsh_utils_polynomial.c

Go to the documentation of this file.

/*                                                                              *
 *   This file is part of the ESO X-Shooter 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: 2013-03-15 11:50:04 $
 * $Revision: 1.8 $
 * $Name: not supported by cvs2svn $
 *
 */
 
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
/*-----------------------------------------------------------------------------
                                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 <xsh_utils_polynomial.h>
 
#include <xsh_utils.h>
//#include <xsh_utils_wrappers.h>
#include <xsh_dump.h>
#include <xsh_msg.h>
#include <xsh_error.h>
 
#include <cpl.h>
 
/*-----------------------------------------------------------------------------
                            Typedefs
 -----------------------------------------------------------------------------*/
struct _polynomial 
{
    cpl_polynomial *pol; 
 
    cpl_vector *vec;
    double *vec_data;
 
    int dimension;  /* for efficiency */
 
    double *shift;
 
    double *scale;
};
 
 
 
/* COPY FROM CPL6.2 WITH sum changed from double to long double: DFS12436 */
cpl_matrix * xsh_matrix_product_normal_create(const cpl_matrix * self)
{
 
    long double         sum; 
    cpl_matrix   * product;
    const double * ai = cpl_matrix_get_data_const(self);
    const double * aj;
    double       * bwrite;
    const size_t      m = cpl_matrix_get_nrow(self);
    const size_t      n = cpl_matrix_get_ncol(self);
    size_t       i, j, k;
 
 
    cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);
 
#if 0
    /* Initialize all values to zero.
       This is done to avoid access of uninitilized memory,  in case
       someone passes the matrix to for example cpl_matrix_dump(). */
    product = cpl_matrix_new(m, m);
    bwrite = cpl_matrix_get_data(product);
#else
    bwrite = (double *) cpl_malloc((size_t)m * (size_t)m * sizeof(double));
    product = cpl_matrix_wrap(m, m, bwrite);
#endif
 
    /* The result at (i,j) is the dot-product of i'th and j'th row */
    for (i = 0; i < m; i++, bwrite += m, ai += n) { 
        aj = ai; /* aj points to first entry in j'th row */
        for (j = i; j < m; j++, aj += n) { 
            sum = 0.0; 
            for (k = 0; k < n; k++) {
                sum += (long double)ai[k] * (long double)aj[k];
            }    
            bwrite[j] = sum; 
        }    
    }    
 
    //cpl_tools_add_flops((cpl_flops)(n * m * (m + 1)));
 
    return product;
 
}
 
/* COPY FROM CPL6.2 calling xsh_matrix_product_normal_create instead of cpl_matrix_product_normal_create
 * and private symbols replaced with public ones */
cpl_matrix *xsh_matrix_solve_normal(const cpl_matrix *coeff,
                                    const cpl_matrix *rhs)
{
 
    cpl_matrix * solution;
    cpl_matrix * At;
    cpl_matrix * AtA;
 
    cpl_ensure(coeff != NULL, CPL_ERROR_NULL_INPUT, NULL);
    cpl_ensure(rhs   != NULL, CPL_ERROR_NULL_INPUT, NULL);
    //cpl_ensure(rhs->nr == coeff->nr, CPL_ERROR_INCOMPATIBLE_INPUT, NULL);
 
    At  = cpl_matrix_transpose_create(coeff);
    solution = cpl_matrix_product_create(At, rhs);
 
    AtA = xsh_matrix_product_normal_create(At);
 
    cpl_matrix_delete(At);
 
 
    /* not a public symbol, copy the two lines it does
    if (cpl_matrix_solve_spd(AtA, solution)) {
    */
    if (cpl_matrix_decomp_chol(AtA) != CPL_ERROR_NONE ||
        cpl_matrix_solve_chol(AtA, solution) != CPL_ERROR_NONE) {
        cpl_matrix_delete(solution);
        solution = NULL;
        //(void)cpl_error_set_where_();
        (void)cpl_error_set_where(cpl_func);
    }
 
    cpl_matrix_delete(AtA);
 
    return solution;
 
}
 
 
 
/*-----------------------------------------------------------------------------
                            Implementation
 -----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
polynomial *
xsh_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_msg( 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_msg( p->pol = cpl_polynomial_duplicate(pol), "Error copying polynomial");
    
  cleanup:
    if (cpl_error_get_code() != CPL_ERROR_NONE)
    xsh_polynomial_delete(&p);
    
    return p;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
polynomial *
xsh_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 = xsh_polynomial_new(p);
    assure_mem( result );
 
  cleanup:
    xsh_free_polynomial(&p);
 
    return result;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
void 
xsh_polynomial_delete(polynomial **p)
{
    xsh_polynomial_delete_const((const polynomial **)p);
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
void 
xsh_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);
    xsh_free(*p);
    *p = NULL;
    return;
}
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
int
xsh_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;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
polynomial *
xsh_polynomial_duplicate(const polynomial *p)
{
    polynomial *result = NULL;
    int dimension;
    int i;
 
    assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    dimension = xsh_polynomial_get_dimension(p);
 
    check_msg( result = xsh_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)
    {
        xsh_polynomial_delete(&result);
        return NULL;
    }
    
    return result;
}
 
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
cpl_table *
xsh_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( xsh_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++;
    }
    }
 
  cleanup:
    return t;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
polynomial *
xsh_polynomial_convert_from_table(cpl_table *t)
{
    polynomial *p = NULL;  /* Result */
    cpl_polynomial *pol = NULL;
    cpl_type type;
    int i;
    
    /* Only 2d supported */
    check_msg( 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,
       xsh_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,
       xsh_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 ,
       xsh_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_msg(( 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);
    
    xsh_msg_debug("Pol.coeff.(%" CPL_SIZE_FORMAT ", %" CPL_SIZE_FORMAT ") = %e", power[0], power[1], coeff);
 
    check_msg( cpl_polynomial_set_coeff(pol, power, coeff), "Error creating polynomial");
    }
    p = xsh_polynomial_new(pol);
 
    /* Read shifts and rescaling */
    xsh_polynomial_rescale(p, 0, cpl_table_get_double( t, COLUMN_COEFF, 3, NULL));
    xsh_polynomial_rescale(p, 1, cpl_table_get_double( t, COLUMN_COEFF, 4, NULL));
    xsh_polynomial_rescale(p, 2, cpl_table_get_double( t, COLUMN_COEFF, 5, NULL));
    xsh_polynomial_shift  (p, 0, cpl_table_get_double( t, COLUMN_COEFF, 0, NULL));
    xsh_polynomial_shift  (p, 1, cpl_table_get_double( t, COLUMN_COEFF, 1, NULL));
    xsh_polynomial_shift  (p, 2, cpl_table_get_double( t, COLUMN_COEFF, 2, NULL));
 
  cleanup:
    xsh_free_polynomial(&pol);
    if (cpl_error_get_code() != CPL_ERROR_NONE)
    xsh_polynomial_delete(&p);
 
    return p;
}
 
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
int
xsh_polynomial_get_dimension(const polynomial *p)
{
    int dim = -1;
    assure(p != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
 
/* slow     check_msg( dim = cpl_polynomial_get_dimension(p->pol), "Error reading dimension"); */
    dim = p->dimension;
    
  cleanup:
    return dim;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
void xsh_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 <= xsh_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;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
cpl_error_code
xsh_polynomial_rescale(polynomial *p, int varno, double scale)
{
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(0 <= varno && varno <= xsh_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();
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
cpl_error_code
xsh_polynomial_shift(polynomial *p, int varno, double shift)
{
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(0 <= varno && varno <= xsh_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();
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
double
xsh_polynomial_evaluate_1d(const polynomial *p, double x)
{
    double result = 0;
    
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(xsh_polynomial_get_dimension(p) == 1, 
       CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
    
    check_msg( 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;
}
 
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
 
double
xsh_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;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
double
xsh_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(xsh_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_msg(( 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_msg( 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;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
double
xsh_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_msg( pol_1d = xsh_polynomial_collapse(p, varno, x_value),
       "Could not collapse polynomial");
 
    check_msg( result = xsh_polynomial_solve_1d(pol_1d, value, guess, multiplicity),
       "Could not find root");
 
  cleanup:
    xsh_polynomial_delete(&pol_1d);
    return result;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
double
xsh_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(xsh_polynomial_get_dimension(p) == 2, CPL_ERROR_ILLEGAL_INPUT,
       "Polynomial must be 2d. It's %dd", xsh_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 
   xsh_polynomial_collapse()
 
   check_msg( pol_1d = xsh_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;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
double
xsh_polynomial_derivative_1d(const polynomial *p, double x)
{
    double result = 0;
 
    assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    assure(xsh_polynomial_get_dimension(p) == 1, 
       CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
    
    check_msg( cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], &result),
       "Error evaluating derivative");
    
  cleanup:
    return result;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
polynomial *
xsh_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(xsh_polynomial_get_dimension(p1) == 2, 
       CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
    assure(xsh_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 = xsh_max_int(xsh_polynomial_get_degree(p1),
                              xsh_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 = xsh_polynomial_get_coeff_2d(p1, i, j);
                coeff2 = xsh_polynomial_get_coeff_2d(p2, i, j);
                
                power[0] = i;
                power[1] = j;
                cpl_polynomial_set_coeff(pol, power, coeff1 + coeff2);
            }
    }
 
    result = xsh_polynomial_new(pol);
   
  cleanup:
    xsh_free_polynomial(&pol);
    return result;
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
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();
}
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
cpl_error_code
xsh_polynomial_derivative(polynomial *p, int varno)
{
    int dimension;
    
    assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
    check_msg ( dimension = xsh_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_msg( derivative_cpl_polynomial(p->pol, varno),
       "Error calculating derivative of CPL-polynomial");
    
  cleanup:
    return cpl_error_get_code();
}
 
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
double
xsh_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_msg ( dimension = xsh_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 = xsh_polynomial_duplicate(p);
 
    factorial = 1;
    while(degree1 > 0)
    {
        check_msg( xsh_polynomial_derivative(pp, 1), "Error calculating derivative");
 
        factorial *= degree1;
        degree1 -= 1;
    }
 
    while(degree2 > 0)
    {
        check_msg( xsh_polynomial_derivative(pp, 2), "Error calculating derivative");
 
        factorial *= degree2;
        degree2 -= 1;
    }
    
    check_msg( result = xsh_polynomial_evaluate_2d(pp, 0, 0) / factorial,
       "Error evaluating polynomial");
    
  cleanup:
    xsh_polynomial_delete(&pp);
    return result;
}
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
double
xsh_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_msg ( dimension = xsh_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 = xsh_polynomial_duplicate(p);
    
    factorial = 1;
    while(degree > 0)
    {
        check_msg( xsh_polynomial_derivative(pp, 1), "Error calculating derivative");
        
        factorial *= degree;
        degree -= 1;
    }
    
    check_msg( result = xsh_polynomial_evaluate_1d(pp, 0) / factorial,
       "Error evaluating polynomial");
    
  cleanup:
    xsh_polynomial_delete(&pp);
    return result;
}
 
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
polynomial *
xsh_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 = xsh_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 gecpl_image_get_maxpos_windowneralize 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 = xsh_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;
    xsh_free_polynomial(&pol);
    if (cpl_error_get_code() != CPL_ERROR_NONE)
    {
        xsh_polynomial_delete(&result);
    }
    return result;
}
 
 
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
polynomial * xsh_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)
            {
                xsh_free_matrix(&ma) ;
                xsh_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,  
                       xsh_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,  
                       xsh_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_msg( mx = xsh_matrix_solve_normal(ma, mb),
       "Could not invert matrix");
    xsh_free_matrix(&ma);
    xsh_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)) ;
    }
    xsh_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;
        }
        xsh_free_vector(&x_val) ;
        /* Average the error term */
        *mse /= (double)np ;
    }
 
    /* Create and shift result */
    result = xsh_polynomial_new(out);
    xsh_free_polynomial(&out);
 
    xsh_polynomial_shift(result, 0, mean_z);
    xsh_polynomial_shift(result, 1, mean_x);
 
  cleanup:
    xsh_free_vector(&x_val);
    xsh_free_matrix(&ma);
    xsh_free_matrix(&mb);
    xsh_free_matrix(&mx);
    return result;
}
 
 
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/
polynomial *
xsh_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;
  int i_nc=0;
 
  /* 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) {
        xsh_free_matrix(&ma);
        xsh_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]);
      }
      i_nc=i*nc;
      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[i] = (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;
      i_nc=i*nc;
      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[i] = values_data[i] - mean_z;
    }
  }
 
  /* 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);
      }
    }
    xsh_free_matrix(&mat);
    xsh_free_matrix(&mat_ma);
  }
 
  /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
  mx = xsh_matrix_solve_normal(ma, mb);
 
  xsh_free_matrix(&ma);
  xsh_free_matrix(&mb);
  if (mx == NULL) {
    cpl_free(degx_tab);
    cpl_free(degy_tab);
    xsh_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);
    xsh_free_matrix(&mx);
    xsh_free_matrix(&cov);
    xsh_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);
  xsh_free_matrix(&cov);
  xsh_free_matrix(&mx);
 
  /* Create and shift result */
  result = xsh_polynomial_new(out);
  xsh_free_polynomial(&out);
  xsh_polynomial_shift(result, 0, mean_z);
  xsh_polynomial_shift(result, 1, mean_x);
  xsh_polynomial_shift(result, 2, mean_y);
 
  /* Wrap up variance polynomial */
  if (variance != NULL) {
    *variance = xsh_polynomial_new(variance_cpl);
    xsh_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:
     xsh_polynomial_shift(*variance, 0, mean_z); */
    xsh_polynomial_shift(*variance, 1, mean_x);
    xsh_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 = xsh_polynomial_evaluate_2d(result, xy_pos_data_x[i],
          xy_pos_data_y[i]);
      double residual = values_data[i] - regress;
      /* Subtract from the true value, square, accumulate */
      if (mse != NULL) {
        *mse += residual * residual;
      }
      if (red_chisq != NULL) {
        *red_chisq += (residual / sigmas_data[i]) * (residual / sigmas_data[i]) ;
      }
    }
    /* 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;
}