/* $Id: fors_polynomial.c,v 1.9 2013-02-15 10:55:19 cgarcia Exp $
 *
 * This file is part of the FORS Library
 * Copyright (C) 2002-2010 European Southern Observatory
 *
 * This program 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, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
 */

/*
 * $Author: cgarcia $
 * $Date: 2013-02-15 10:55:19 $
 * $Revision: 1.9 $
 * $Name: not supported by cvs2svn $
 */

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

#include "fors_polynomial.h"

#include "fors_utils.h"

#include <cpl.h>
#include <math.h>
#include <string.h>

/*----------------------------------------------------------------------------*/
/**
 * @defgroup fors_polynomial    Tools for Polynomial Coefficient Handling
 */
/*----------------------------------------------------------------------------*/

/**@{*/
/*-----------------------------------------------------------------------------
    Prototypes
 -----------------------------------------------------------------------------*/

static bool
fors_polynomial_is_coeff_set(               const cpl_polynomial    *p,
                                            const cpl_size          *powers);

static bool
fors_polynomial_powers_next(                const cpl_polynomial    *p,
                                            cpl_size                *powers);

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

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup
/**
 * @brief   Implementation of the check whether a coefficient exists.
 * @param   p       Polynomial
 * @param   powers  Integer array (of the same dimension) containing the powers
 * @return  1 if it exists, 0 otherwise or in the case of error
 */
/*----------------------------------------------------------------------------*/
static bool
fors_polynomial_is_coeff_set(               const cpl_polynomial    *p,
                                            const cpl_size          *powers)
{
    cassure_automsg(                        p != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return false);
    cassure_automsg(                        powers != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return false);
    
    return (fabs(cpl_polynomial_get_coeff(p, powers)) > DBL_EPSILON);
}

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup
/**
 * @brief   Step the powers of a polynomials coefficient to the next.
 * @param   p       Polynomial
 * @param   powers  Integer array of the same dimensionality as @a p
 * @return  "true" if overflow or error, otherwise "false" if success
 */
/*----------------------------------------------------------------------------*/
static bool
fors_polynomial_powers_next(const cpl_polynomial    *p,
                            cpl_size                *powers)
{
    int             dim = 0,
                    ndims,
                    ndegs;
    bool            overflow = false;
    cpl_errorstate  errstat = cpl_errorstate_get();
    
    cassure_automsg(                        p != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return true);
    cassure_automsg(                        powers != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return true);
    
    ndims = cpl_polynomial_get_dimension(p);
    ndegs = cpl_polynomial_get_degree(p);
    passure(cpl_errorstate_is_equal(errstat), return true);
    
    powers[dim]++;
    while(dim < ndims && powers[dim] > ndegs) /* carry over */
    {
        powers[dim] = 0;
        dim++;
        overflow = (!(dim < ndims));
        if (!overflow)
            powers[dim]++;
    }
    
    return overflow;
}

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

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup \
do { \
    if (pows != NULL) \
        cpl_free(pows); \
    pows = NULL; \
} while (0)
/**
 * @brief   Count the total number of non-zero coefficients.
 * @param   p   Polynomial
 * @return  Number
 */
/*----------------------------------------------------------------------------*/
int
fors_polynomial_count_coeff(                const cpl_polynomial    *p)
{
    int             ndims,
                    Ncoeff = 0;
    cpl_size       *pows = NULL;
    bool            overflow = false;
    cpl_errorstate  errstat = cpl_errorstate_get();
    
    cassure_automsg(                        p != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return 0);
    
    ndims = cpl_polynomial_get_dimension(p);
    passure(cpl_errorstate_is_equal(errstat), return 0);
    
    pows = cpl_calloc(ndims, sizeof(*pows));
    
    while (!overflow) 
    {
        if (fors_polynomial_is_coeff_set(p, pows))
            Ncoeff++;
        
        overflow = fors_polynomial_powers_next(p, pows);
    }
    
    passure(cpl_errorstate_is_equal(errstat), return 0);
    cleanup;
    
    return Ncoeff;
}

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup
/**
 * @brief   Find the first non-zero coefficient.
 * @param   p       Polynomial
 * @param   powers  Integer array (of the same dimension) containing the powers
 * @return  "true" if overflow or error, otherwise "false"
 */
/*----------------------------------------------------------------------------*/
int
fors_polynomial_powers_find_first_coeff(const cpl_polynomial    *p,
                                        cpl_size                *powers)
{
    int             ndims,
                    dim;
    bool            overflow;
    cpl_errorstate  errstat = cpl_errorstate_get();
    
    cassure_automsg(                        p != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return true);
    cassure_automsg(                        powers != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return true);
    
    ndims = cpl_polynomial_get_dimension(p);
    for (dim = 0; dim < ndims; dim++)
        powers[dim] = 0;
    
    if (fors_polynomial_is_coeff_set(p, powers))
        return false;
    
    overflow = fors_polynomial_powers_find_next_coeff(p, powers);
    passure(cpl_errorstate_is_equal(errstat), return true);
    cleanup;
    
    return overflow;
}

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup
/**
 * @brief   Find the next non-zero coefficient.
 * @param   p       Polynomial
 * @param   powers  Integer array (of the same dimension) containing the powers
 * @return  "true" if overflow or error, otherwise "false"
 */
/*----------------------------------------------------------------------------*/
int
fors_polynomial_powers_find_next_coeff(const cpl_polynomial    *p,
                                       cpl_size                *powers)
{
    bool            overflow;
    cpl_errorstate  errstat = cpl_errorstate_get();
    
    cassure_automsg(                        p != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return true);
    cassure_automsg(                        powers != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return true);
    
    overflow = fors_polynomial_powers_next(p, powers);
    while (!overflow)
    {
        if (fors_polynomial_is_coeff_set(p, powers))
            break;
        
        overflow = fors_polynomial_powers_next(p, powers);
    }
    
    passure(cpl_errorstate_is_equal(errstat), return true);
    cleanup;
    
    return overflow;
}

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup \
do { \
    if (pows != NULL) \
        cpl_free(pows); \
    pows = NULL; \
} while (0)
/**
 * @brief   Set the already existing coefficients in a polynomial to values taken from an array.
 * @param   p           Polynomial
 * @param   coeffs      Array containing coefficient values
 * @param   n_coeffs    Size of @a coeffs array
 * @return  Number
 */
/*----------------------------------------------------------------------------*/
cpl_error_code
fors_polynomial_set_existing_coeff(         cpl_polynomial  *p,
                                            const double    *coeffs,
                                            int             n_coeffs)
{
    int             n,
                    ndims;
    cpl_size       *pows = NULL;
    bool            overflow = false;
    cpl_errorstate  errstat = cpl_errorstate_get();
    
    if (p == NULL)
        return 0;
    
    /* check input */
    cassure_automsg(                        p != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return cpl_error_get_code());
    cassure_automsg(                        coeffs != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return cpl_error_get_code());
    cassure_automsg(                        n_coeffs > 0,
                                            CPL_ERROR_ILLEGAL_INPUT,
                                            return cpl_error_get_code());
    
    /* start */
    ndims = cpl_polynomial_get_dimension(p);
    pows = cpl_calloc(ndims, sizeof(*pows));
    
    overflow = fors_polynomial_powers_find_first_coeff(p, pows);
    n = 0;
    while (!overflow)
    {
        cassure(                            n < n_coeffs,
                                            CPL_ERROR_ACCESS_OUT_OF_RANGE,
                                            return cpl_error_get_code(),
                                            "p contains more coefficients "
                                            "than coeffs");
        cpl_polynomial_set_coeff(p, pows, coeffs[n]);
        n++;
        
        overflow = fors_polynomial_powers_find_next_coeff(p, pows);
    }
    
    cleanup;
    
    return  cpl_errorstate_is_equal(errstat) ?
                CPL_ERROR_NONE :
                cpl_error_get_code();
}

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup \
do { \
    if (powersA != NULL) { cpl_free(powersA); powersA = NULL;} \
    if (powersB != NULL) { cpl_free(powersB); powersB = NULL;} \
    if (powersE != NULL) { cpl_free(powersE); powersE = NULL;} \
    cpl_polynomial_delete(ep); ep = NULL; \
} while (0)
/**
 * @brief   Create a polynomial modelling the squared influence of the error of the coefficients of another polynomial.
 * @param   p_def       Polynomial definition (please read below)
 * @param   cov_coeff   Covariance matrix of the coefficients
 * @return  The error polynomial, NULL in the case of error
 * 
 * @par Input Parameter Details:
 * - @a p_def is a polynomial which is used as a definition of which
 *   coefficients are set, therefore:
 * - the coefficients in @a p_def must be either zero (treated as non-existing)
 *   or significantly different from zero (currently checked against
 *   DBL_EPSILON),
 * - @a cov_coeff must be square, and have as many columns as @a p_def has
 *   coefficients,
 * - the entries of @a cov_coeff must refer to the coefficients of @a p_dev
 *   in the order of their occurrence, with the first dimension of @a p_dev
 *   as the fastest index.
 */
/*----------------------------------------------------------------------------*/
cpl_polynomial*
fors_polynomial_create_variance_polynomial( const cpl_polynomial    *p_def,
                                            const cpl_matrix        *cov_coeff)
{
    int             n_dims,
                    n_coeffs,
                    n_col,
                    ia = 0;
    cpl_size        *powersA = NULL,
                    *powersB = NULL,
                    *powersE = NULL;
    cpl_polynomial  *ep = NULL;
    bool            overflowa = false;
    cpl_errorstate  errstat = cpl_errorstate_get();
    
    /* check input */
    cassure_automsg(                        p_def != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return ep);
    cassure_automsg(                        cov_coeff != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return ep);
    
    n_coeffs = fors_polynomial_count_coeff(p_def);
    n_dims = cpl_polynomial_get_dimension(p_def);
    n_col = cpl_matrix_get_ncol(cov_coeff);
    passure(cpl_errorstate_is_equal(errstat), return ep);
    
    cassure(                                n_coeffs == n_col,
                                            CPL_ERROR_INCOMPATIBLE_INPUT,
                                            return ep,
                                            "number of p_def coefficients "
                                            "!= nr of columns");
    cassure(                                n_col == cpl_matrix_get_nrow(
                                                        cov_coeff),
                                            CPL_ERROR_ILLEGAL_INPUT,
                                            return ep,
                                            "cov_coeff is not square");
    
    ep = cpl_polynomial_new(n_dims);
    powersA = cpl_calloc(n_dims, sizeof(*powersA));
    powersB = cpl_calloc(n_dims, sizeof(*powersB));
    powersE = cpl_calloc(n_dims, sizeof(*powersE));
    passure(cpl_errorstate_is_equal(errstat), return ep);
    
    overflowa = fors_polynomial_powers_find_first_coeff(p_def, powersA);
    while (!overflowa)
    {
        bool    overflowb = false;
        int     ib = 0;
        
        overflowb = fors_polynomial_powers_find_first_coeff(p_def, powersB);
        while (!overflowb)
        {
            double  coeff;
            int     d;
            passure(cpl_errorstate_is_equal(errstat), return ep);
            /* 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]) 
             */
            for (d = 0; d < n_dims; d++)
                powersE[d] = powersA[d] + powersB[d];
            
            coeff = cpl_polynomial_get_coeff(ep, powersE);
            cpl_polynomial_set_coeff(ep, powersE,
                                     coeff
                                     + cpl_matrix_get(
                                        cov_coeff, ia, ib));
            
            ib++;
            overflowb = fors_polynomial_powers_find_next_coeff(p_def, powersB);
        }
        ia++;
        overflowa = fors_polynomial_powers_find_next_coeff(p_def, powersA);
    }
    passure(cpl_errorstate_is_equal(errstat), return ep);
    
    cpl_polynomial  *ret_ep = ep; ep = NULL;
    cleanup;
    
    return ret_ep;
}

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup \
do { \
    if (pows != NULL) \
        cpl_free(pows); \
    pows = NULL; \
    if (ndxstr != NULL) \
        cpl_free(ndxstr); \
    ndxstr = NULL; \
} while (0)
/**
 * @brief   Count the total number of non-zero coefficients.
 * @param   p       Polynomial
 * @param   name    (Optional) name, can be NULL
 * @param   level   Message level
 * @param   p_def   (Optional) polynomial definition, used to determine
 *                  non-zero coefficients
 * @return  CPL error code
 */
/*----------------------------------------------------------------------------*/
cpl_error_code
fors_polynomial_dump(                       const cpl_polynomial    *p,
                                            const char              *name,
                                            cpl_msg_severity        level,
                                            const cpl_polynomial    *p_def)
{
    int             ndims,
                    ndegs,
                    dim;
    cpl_size        *pows = NULL;
    char            *ndxstr = NULL;
    char            max_ndx_str[15];
    cpl_errorstate  errstat = cpl_errorstate_get();
    
    /* check input */
    cassure_automsg(                        p != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return cpl_error_get_code());
    
    if (p_def != NULL)
    {
        ndims = cpl_polynomial_get_dimension(p_def);
        cassure_automsg(                    ndims
                                            = cpl_polynomial_get_dimension(p),
                                            CPL_ERROR_INCOMPATIBLE_INPUT,
                                            return cpl_error_get_code());
    }
    else
    {
        ndims = cpl_polynomial_get_dimension(p);
        p_def = p;
    }
    
    ndegs = cpl_polynomial_get_degree(p_def);
    
    pows = cpl_calloc(ndims, sizeof(*pows));
    /* allocate a string that can contain "%d,%d,...,%d" */
    sprintf(max_ndx_str, "%d", ndegs);
    ndxstr = cpl_calloc(ndims*(strlen(max_ndx_str)+1), sizeof(*ndxstr));
    
    /* ATTENTION: below, the last dimension is the fastest index (for nicer
     * printing). This differs from the strategy in all other polynomial
     * handling functions above, so be careful when doing copy & paste!
     */
    while (pows[0] <= ndegs) 
    {
        if (fors_polynomial_is_coeff_set(p_def, pows))
        {
            double  coeff;
            coeff = cpl_polynomial_get_coeff(p, pows);
            /* create the index string */
            sprintf(ndxstr, "%"CPL_SIZE_FORMAT, pows[0]);
            for (dim = 1; dim < ndims; dim++)
                sprintf(ndxstr+strlen(ndxstr), ",%"CPL_SIZE_FORMAT, pows[dim]);
            fors_msg(                       level,
                                            "%s_%s = %e",
                                            (name != NULL) ? name : "p",
                                            ndxstr,
                                            coeff);
        }
        
        dim = ndims-1;
        pows[dim]++;
        while(dim > 0 && pows[dim] > ndegs) /* carry over */
        {
            pows[dim] = 0;
            dim--;
            pows[dim]++;
        }
    }
    
    cleanup;
    
    return  cpl_errorstate_is_equal(errstat) ?
                CPL_ERROR_NONE :
                cpl_error_get_code();
}

/*----------------------------------------------------------------------------*/
#undef cleanup
#define cleanup \
do { \
    if (ndxstr != NULL) \
        cpl_free(ndxstr); \
    ndxstr = NULL; \
} while (0)
/**
 * @brief   Print a coefficient's name into a newly allocated string.
 * @param   p       Polynomial
 * @param   powers  Integer array (of the same dimension) containing the powers
 * @param   name    (Optional) name prefix, can be NULL
 * @return  Allocated string, NULL in the case of error
 * 
 * A coefficient is printed into a newly allocated string like following:
 * - if @a prefix is NULL or empty, then in the format "%d,%d,%d,...,%d",
 * - otherwise in the format "%s_%d,%d,...,%d", prefix, etc.
 */
/*----------------------------------------------------------------------------*/
char*
fors_polynomial_sprint_coeff(               const cpl_polynomial    *p,
                                            cpl_size                *powers,
                                            const char              *prefix)
{
    int             ndims,
                    ndegs,
                    dim,
                    max_nr,
                    max_nr_strlen;
    char            *ndxstr = NULL;
    char            max_ndx_str[15];
    
    /* check input */
    cassure_automsg(                        p != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return ndxstr);
    cassure_automsg(                        powers != NULL,
                                            CPL_ERROR_NULL_INPUT,
                                            return ndxstr);
    
    ndims = cpl_polynomial_get_dimension(p);
    ndegs = cpl_polynomial_get_degree(p);
    
    /* be safe to cover maximum required str length */
    /* FIXME: catch negative numbers */
    max_nr = ndegs;
    for (dim = 0; dim < ndims; dim++)
        if (powers[dim] > max_nr)
            max_nr = powers[dim];
    
    sprintf(max_ndx_str, "%d", max_nr);
    max_nr_strlen = strlen(max_ndx_str);
    if (prefix != NULL && prefix[0] != '\0')
    {
        int len = strlen(prefix) + 1 + ndims*(max_nr_strlen+1);
        ndxstr = cpl_calloc(len, sizeof(*ndxstr));
        sprintf(ndxstr, "%s_", prefix);
    }
    else
    {
        ndxstr = cpl_calloc(ndims*(max_nr_strlen+1), sizeof(*ndxstr));
    }
    /* specify max length for snprintf including nul char */
    snprintf(ndxstr+strlen(ndxstr),         max_nr_strlen+1,
                                            (powers[0] >= 0) ? "%"CPL_SIZE_FORMAT : "?",
                                            powers[0]);
    for (dim = 1; dim < ndims; dim++)
        snprintf(ndxstr+strlen(ndxstr),     max_nr_strlen+2,
                                            (powers[dim] >= 0) ? ",%"CPL_SIZE_FORMAT : "?",
                                            powers[dim]);
    
    return ndxstr;
}

/**@}*/
