/* $Id: xsh_fit_body.h,v 1.3 2007-10-03 14:06:16 rhaigron Exp $
 *
 * This file is part of the ESO irplib package
 * Copyright (C) 2001-2007 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#define TYPE_ADD(a) CONCAT2X(a, CPL_TYPE)
#define TYPE_ADD_CONST(a) CONCAT2X(TYPE_ADD(a),const) 
#define IMAGE_GET_DATA TYPE_ADD(cpl_image_get_data)

/*----------------------------------------------------------------------------*/
/**
  @brief  Fit a polynomial to each pixel in a list of images
  @param  self     Preallocated imagelist to hold fitting result
  @param  mh       Upper triangular part of SPD Hankel matrix, H = V' * V
  @param  mv       The transpose of the Vandermonde matrix, V'
  @param  x_pos    The vector of positions to fit (used for fiterror only)
  @param  values   The list of images with values to fit
  @param  xpow     The mindeg powers of x_pos (or NULL, when mindeg is zero)
  @param  xnmean   Minus the mean value of the x_pos elements (or zero)
  @param  np       The number of sample points (length of values and x_pos)
  @param  nc       The number of polynomial coefficients to determine per pixel
  @param  fiterror      When non-NULL, the error of the fit
  @return void
  @see irplib_fit_imagelist_polynomial

 */
/*----------------------------------------------------------------------------*/
static void
TYPE_ADD(irplib_fit_imagelist_polynomial)(cpl_imagelist * self,
                                              const cpl_matrix * mh,
                                              const cpl_matrix * mv,
                                              const cpl_vector * x_pos,
                                              const cpl_imagelist * values,
                                              const cpl_vector * xpow,
                                              double xnmean, int np, int nc,
                                              cpl_image * fiterror)
{

    const cpl_image * value = cpl_imagelist_get_const(values, 0);
    const int         nx = cpl_image_get_size_x(value);
    const int         ny = cpl_image_get_size_y(value);

    const CPL_TYPE  * pi;
    double          * pdest;
    double          * px  = cpl_malloc(nx * nc * sizeof(double));
    double          * pbw = cpl_malloc(nx * np * sizeof(double));
    /* The polynomial coefficients to be solved for
       - transposed, so the nx polynomials are stored consequtively */
    cpl_matrix      * mx  = cpl_matrix_wrap(nx, nc, px);
    /* The transpose of one row of values to be fitted */
    cpl_matrix      * mb  = cpl_matrix_wrap(nx, np, pbw); 
    const cpl_image* image = NULL;
    int               i, j, k, jj;


    /* Process one image row at a time
       - Improves cache usage  */
    for (jj = 0; jj < ny; jj++) {
 
        /* Fill in matrices */
        for (j=0; j < np; j++) {
            image = cpl_imagelist_get_const( values, j); 
            pi = TYPE_ADD_CONST(cpl_image_get_data)
                (image);
            /* The fact the mb is transposed makes this operation more
               expensive - which is OK, since it has a lower complexity
               than the subsequent use of mb */
            for (i=0; i < nx; i++) {
                pbw[np * i + j] = (double)pi[nx * jj + i];
            }
        }

        /* Form the right hand side of the normal equations, X = V' * B */
        irplib_matrix_product_transpose(mx, mb, mv);

        /* In-place solution of the normal equations, V' * V * X = V' * B */
        irplib_matrix_solve_chol_transpose(mh, mx);

        if (xnmean != 0.0) {
            /* Shift polynomials back */
            for (i=0; i < nx; i++) {
                irplib_polynomial_shift_double(px + i * nc, nc, xnmean);
            }
        }

        /* Copy results to output image list */

        for (k=0; k < nc; k++) {
            pdest = cpl_image_get_data_double(cpl_imagelist_get(self, k));
            /* The fact the mx is transposed makes this operation more
               expensive - which is OK, since it has a lower complexity
               than the other use of mx */
            for (i=0; i < nx; i++) {
                pdest[nx * jj + i] = px[nc * i + k];
            }
        }

        if (fiterror != NULL) {
            switch (cpl_image_get_type(fiterror)) {
            case CPL_TYPE_DOUBLE:
                irplib_fit_imagelist_residual_double(fiterror, jj, x_pos,
                                                     xpow, mx, mb);
                break;
            case CPL_TYPE_FLOAT:
                irplib_fit_imagelist_residual_float(fiterror, jj, x_pos,
                                                    xpow, mx, mb);
                break;
            case CPL_TYPE_INT:
                irplib_fit_imagelist_residual_int(fiterror, jj, x_pos,
                                                  xpow, mx, mb);
                break;
            default:
                /* It is an error in CPL to reach this point */
                assert( 0 );
            }
        }
    }
    cpl_matrix_delete(mx);
    cpl_matrix_delete(mb);

}


/*----------------------------------------------------------------------------*/
/**
  @brief  Compute the residual of a polynomial fit to an imagelist
  @param  self     Preallocated image to hold residual
  @param  jj       The index of the image row to compute
  @param  x_pos    The vector of positions to fit (used for fiterror only)
  @param  xpow     The mindeg powers of x_pos (or NULL, when mindeg is zero)
  @param  mx       The transpose of the computed fitting coefficients
  @param  mb       The transpose of the values to be fitted
  @return void
  @see irplib_fit_imagelist_polynomial()

  The call requires nx * (np * (2 * nc + 1) + 1) FLOPs.

 */
/*----------------------------------------------------------------------------*/
static void TYPE_ADD(irplib_fit_imagelist_residual)(cpl_image * self,
                                                        int jj,
                                                        const cpl_vector * x_pos,
                                                        const cpl_vector * xpow,
                                                        const cpl_matrix * mx,
                                                        const cpl_matrix * mb)
{
    double         err, sq_err;

    int      nx = cpl_matrix_get_nrow(mx);
    int      nc = cpl_matrix_get_ncol(mx);
    int      np = cpl_vector_get_size(x_pos);

    /* pself points to 1st element in jj'th row */
    CPL_TYPE     * pself = nx * jj + TYPE_ADD(cpl_image_get_data)(self);

    const double * pp = cpl_vector_get_data_const(x_pos);

    /* If mindeg == 0 xpow is NULL (since no multiplication is needed) */
    const double * pm = xpow != NULL ? cpl_vector_get_data_const(xpow) : NULL;
    const double * px = cpl_matrix_get_data_const(mx);
    const double * pb = cpl_matrix_get_data_const(mb);

    int i, j, k;


    for (i=0; i < nx; i++, pb += np, px += nc) {
        sq_err = 0.0;
        for (j=0; j < np; j++) {

            /* Evaluate the polynomial using Horners scheme, see
               cpl_polynomial_eval_1d() */
            k = nc;
            err = px[--k];

            while (k) err = pp[j] * err + px[--k];

            /* Multiply by x^mindeg - wheen needed */
            /* Substract expected value to compute the residual */
            if (pm != NULL) {
                err = err * pm[j] - pb[j];
            } else {
                err -= pb[j];
            }

            sq_err += err * err;
        }
        pself[i] = (CPL_TYPE)(sq_err/(double)np);
    }

}

#undef TYPE_ADD
#undef IMAGE_GET_DATA
