/* $Id: image_spline_fit.tcc,v 1.7 2013-09-09 13:14:32 cgarcia Exp $
 *
 * This file is part of the MOSCA library
 * Copyright (C) 2013 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 Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

/*
 * $Author: cgarcia $
 * $Date: 2013-09-09 13:14:32 $
 * $Revision: 1.7 $
 * $Name: not supported by cvs2svn $
 */


#ifndef IMAGE_SPLINE_FIT_TCC
#define IMAGE_SPLINE_FIT_TCC

#include <iostream>
#include <vector>
#include <algorithm>
#include <stdexcept>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_multifit.h>
#include "mosca_image.h"
#include "image_spline_fit.h"

cpl_vector * fit_cubic_bspline(cpl_vector * values, int nknots, double threshold);


/**
 * @brief
 *   TODO 
 *
 * @param TODO
 * @return TODO
 *
 */
template<typename T>
void mosca::image_cubicspline_1d_fit(mosca::image& image, 
                                     int nknots, double flux_threshold,
                                     mosca::axis fitting_axis)
{
    cpl_size ny = image.size_y();
    cpl_size nx = image.size_x();
    cpl_size nlines;
    cpl_size nfit;
    cpl_size stride_line;
    cpl_size stride_pixel;
    mosca::axis image_axis;
    int collapse_dir;

    /* If the fitting direction is given in terms of dispersion or spatial
       axis, then decide whether it is X or Y. If not, it is already X or Y */
    image_axis = image.axis_to_image(fitting_axis);
    
    /* Lines are contiguous pixels along the fitting direction.
       i.e., if image_axis == mosca::X_AXIS the lines are rows and there are
       ny lines.
       Each line contains nfit pixels. For image_axis == mosca::X_AXIS this
       means nfit = nx. */
    if(image_axis == mosca::X_AXIS)
    {
        nlines = ny;
        nfit   = nx;
        stride_pixel = 1;
        stride_line = nx;
        collapse_dir = 0;
    }
    else
    {
        nlines = nx;
        nfit   = ny;
        stride_pixel = nx;
        stride_line = 1;
        collapse_dir = 1;
    }
        
    
    //We collapse in the oposite direction of the fitting
    cpl_image * collapse_median = 
          cpl_image_collapse_median_create(image.get_cpl_image(),
                                           collapse_dir, 0, 0);
    
    //We fit a spline to the line-image
    cpl_vector * collapse_line;
    if(image_axis == mosca::X_AXIS)
        collapse_line = cpl_vector_new_from_image_row(collapse_median, 1);
    else
        collapse_line = cpl_vector_new_from_image_column(collapse_median, 1);

    cpl_vector * spline_fit = fit_cubic_bspline(collapse_line, nknots, flux_threshold);

    //Place in each column or row the spline fit
    T *      p_image      = image.get_data<T>();
    double * p_spline_fit = cpl_vector_get_data(spline_fit);
    for (cpl_size j = 0; j< ny; ++j)
    {
        for (cpl_size i = 0; i< nx; ++i, ++p_image)
        {
            if(image_axis == mosca::X_AXIS)
                *p_image = p_spline_fit[i];
            else
                *p_image =  p_spline_fit[j];
        }
    }

    cpl_image_delete(collapse_median);    
    cpl_vector_delete(collapse_line);    
}
        
cpl_vector * fit_cubic_bspline(cpl_vector * values, int nknots, double threshold)
{
    //Allocate result
    cpl_vector * result;
    cpl_size     npix = cpl_vector_get_size(values);
    result = cpl_vector_new(npix);

    //This is valid for cubic splines, if not nbreak = ncoeffs + 2 - k, 
    //where k is the degree of the spline
    int ncoeffs = nknots + 2;
    
    /* Get the threshold in terms of the maximum */
    double max_value = cpl_vector_get_max(values);
    double thres_value = threshold * max_value;
    
    /* Create a "mask" of pixels not to use */
    cpl_array * values_mask = cpl_array_new(npix, CPL_TYPE_INT);
    int nfit = 0;
    for (cpl_size i = 0; i < npix; ++i)
        if(cpl_vector_get(values, i) < thres_value)
            cpl_array_set_int(values_mask, i, 0);
        else
        {
            cpl_array_set_int(values_mask, i, 1);
            nfit++;
        }

    /* allocate a cubic bspline workspace (k = 4) */
    gsl_bspline_workspace *bspl_wspc;
    gsl_vector *B;
    gsl_matrix * X;
    bspl_wspc = gsl_bspline_alloc(4, nknots);
    B = gsl_vector_alloc(ncoeffs);
    X = gsl_matrix_alloc(nfit, ncoeffs);
    
    /* allocate objects for the fitting */
    gsl_matrix *cov;
    gsl_vector * y_fit;
    gsl_vector * weigth;
    gsl_multifit_linear_workspace * mfit_wspc;
    gsl_vector *spl_coeffs;
    double chisq;
    y_fit = gsl_vector_alloc(nfit);
    weigth = gsl_vector_alloc(nfit);
    mfit_wspc = gsl_multifit_linear_alloc(nfit, ncoeffs);
    spl_coeffs = gsl_vector_alloc(ncoeffs);
    cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
    
    /* use uniform breakpoints on [0, npix], which is the range of fitting */
    gsl_bspline_knots_uniform(0.0, (double)npix, bspl_wspc);
    

    /* construct the fit matrix X */
    for (int i = 0, ifit = 0; i < npix; ++i)
    {
        int null;
        if(cpl_array_get(values_mask, (cpl_size)i, &null) == 1)
        {
            double xi = i;
            double yi = cpl_vector_get(values, i);

            /* Fill the vector to fit */
            gsl_vector_set(y_fit, ifit, yi);
            gsl_vector_set(weigth, ifit, 1.);

            /* compute B_j(xi) for all j */
            gsl_bspline_eval(xi, B, bspl_wspc);

            /* fill in row i of X */
            for (int j = 0; j < ncoeffs; ++j)
            {
                double Bj = gsl_vector_get(B, j);
                gsl_matrix_set(X, ifit, j, Bj);
            }
            ifit++;
        }
    } 
    
    /* do the fit */
    gsl_multifit_wlinear(X, weigth, y_fit, spl_coeffs, cov, &chisq, mfit_wspc);
    
    /* output the fit */

    for(int i = 0; i < npix; i ++)
    {
        double yi, yerr;
        gsl_bspline_eval((double)i, B, bspl_wspc);
        gsl_multifit_linear_est(B, spl_coeffs, cov, &yi, &yerr);
        cpl_vector_set(result, i, yi);
    }
    
    return result;
}

#endif
