#include "smooth.h"
#include "vicinity.h"

#include <iostream>
#include <algorithm>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_statistics.h>

void smooth( const std::vector<double> &x, const std::vector<double> &y, std::vector<double> &smoothed_y, int param ) throw ( TError_Generic_error ) {
    if( x.size() < size_t( param*2 ) )
        throw TError_Generic_error( "Not enough elements in vecotr x" );
    if( x.size() != y.size() )
        throw TError_Generic_error( "x.size() != y.size() in smooth(...)" );

    using std::max;
    using std::min;

    const size_t chunk_size = 100,
                 chunk_overlap = 20;

    smoothed_y.resize( y.size() );

    for( size_t k=0; k<x.size(); k+=chunk_size )
    {
        // this part will be written
        size_t chunk_start = k;
        size_t chunk_end   = min( k+chunk_size, x.size() );
        // this part is used in calculation to ensure continuity
        // chunk_overlap_end - chunk_overlap_start is guaranteed to be at least chunk_overlap long provided the x.size()>=chunk_overlap
        size_t chunk_overlap_start = max( chunk_overlap, chunk_start )-chunk_overlap;
        size_t chunk_overlap_end   = min( chunk_end+chunk_overlap, x.size() );

        size_t n = chunk_overlap_end-chunk_overlap_start;
        gsl_vector_const_view gV_x  = gsl_vector_const_view_array( &x[chunk_overlap_start], n );
        gsl_vector_const_view gV_y  = gsl_vector_const_view_array( &y[chunk_overlap_start], n );
        gsl_vector_view       gV_sy = gsl_vector_view_array      ( &smoothed_y[chunk_overlap_start], n );


        size_t ncoeffs = n/param;
        size_t nbreak = ncoeffs - 2;
        gsl_bspline_workspace *bw;
        gsl_vector *B;
        gsl_vector *c;
        const gsl_vector *gx, *gy;
        gsl_matrix *X, *cov;
        gsl_multifit_linear_workspace *mw;
        double chisq;//, Rsq, dof, tss;

        /* allocate a cubic bspline workspace (k = 4) */
        bw = gsl_bspline_alloc(4, nbreak);
        B = gsl_vector_alloc(ncoeffs);

        gx = &gV_x.vector;
        gy = &gV_y.vector;

        X = gsl_matrix_alloc(n, ncoeffs);
        c = gsl_vector_alloc(ncoeffs);
        cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
        mw = gsl_multifit_linear_alloc(n, ncoeffs);

        /* use uniform breakpoints */
        gsl_bspline_knots_uniform(x[chunk_overlap_start], x[chunk_overlap_end-1], bw);

        /* construct the fit matrix X */
        for( size_t i = 0; i < n; ++i ) {
            double xi = gsl_vector_get(gx, i);

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

            /* fill in row i of X */
            for( size_t j=0; j < ncoeffs; ++j ) {
                double Bj = gsl_vector_get(B, j);
                gsl_matrix_set(X, i, j, Bj);
            }
        }

        /* do the fit */
        //size_t rank;
        gsl_multifit_linear(X, gy, c, cov, &chisq, mw);

        /*        dof = n - ncoeffs;
                tss = gsl_stats_wtss(w->data, 1, gy->data, 1, gy->size);
                Rsq = 1.0 - chisq / tss;

                fprintf(stderr, "chisq/dof = %e, Rsq = %f\n",
                        chisq / dof, Rsq);
        */
        /* output the smoothed curve */
        {
            double yi, yerr;

            //printf("#m=1,S=0\n");
            for( size_t i=chunk_start; i<chunk_end; i++ ) {
                gsl_bspline_eval(x[i], B, bw);
                gsl_multifit_linear_est(B, c, cov, &yi, &yerr);
                smoothed_y[i] = yi;
            }
        }

        gsl_bspline_free(bw);
        gsl_vector_free(B);
        gsl_matrix_free(X);
        gsl_vector_free(c);
        gsl_matrix_free(cov);
        gsl_multifit_linear_free(mw);

    }
}