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

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

#include <cpl.h>

#include "kmo_dfs.h"

static int kmos_test_create(cpl_plugin *);
static int kmos_test_exec(cpl_plugin *);
static int kmos_test_destroy(cpl_plugin *);
static int kmos_test(cpl_parameterlist *, cpl_frameset *);

static char kmos_test_description[] = "Testing\n" ;

int cpl_plugin_get_info(cpl_pluginlist *list)
{
    cpl_recipe *recipe = cpl_calloc(1, sizeof *recipe);
    cpl_plugin *plugin = &recipe->interface;

    cpl_plugin_init(plugin,
            CPL_PLUGIN_API,
            KMOS_BINARY_VERSION,
            CPL_PLUGIN_TYPE_RECIPE,
            "kmos_test",
            "Testing",
            kmos_test_description,
            "Yves Jung",
            "yjung@eso.org",
            kmos_get_license(),
            kmos_test_create,
            kmos_test_exec,
            kmos_test_destroy);

    cpl_pluginlist_append(list, plugin);

    return 0;
}

static int kmos_test_create(cpl_plugin *plugin)
{
    cpl_recipe *recipe;

    /* Check that the plugin is part of a valid recipe */
    if (cpl_plugin_get_type(plugin) == CPL_PLUGIN_TYPE_RECIPE) 
        recipe = (cpl_recipe *)plugin;
    else
        return -1;

    /* Create the parameters list in the cpl_recipe object */
    recipe->parameters = cpl_parameterlist_new();

    return 0 ;
}

static int kmos_test_exec(cpl_plugin *plugin)
{
    cpl_recipe  *recipe;

    /* Get the recipe out of the plugin */
    if (cpl_plugin_get_type(plugin) == CPL_PLUGIN_TYPE_RECIPE) 
        recipe = (cpl_recipe *)plugin;
    else return -1;

    return kmos_test(recipe->parameters, recipe->frames);
}

static int kmos_test_destroy(cpl_plugin *plugin)
{
    cpl_recipe *recipe;

    /* Get the recipe out of the plugin */
    if (cpl_plugin_get_type(plugin) == CPL_PLUGIN_TYPE_RECIPE) 
        recipe = (cpl_recipe *)plugin;
    else return -1 ;

    cpl_parameterlist_delete(recipe->parameters);
    return 0 ;
}

static int kmo_priv_lorentz1d_fnc(const double x[], const double a[], 
        double *result) 
{
    *result = a[0] + (a[1]*a[3]) / (2*CPL_MATH_PI*(pow((x[0]-a[2]), 2) + 
                pow((a[3]/2), 2))) +a[4]*x[0];
    return 0;
}
static int kmo_priv_lorentz1d_fncd(const double x[], const double a[], 
        double result[])
{
    if (a == NULL) result = NULL;
    double aa = pow((x[0]-a[2]), 2) + pow((a[3]/2),2);
    double pow2aa = pow(aa, 2);
    result[0] = 1;
    result[1] = a[3] / (2*aa*CPL_MATH_PI);
    result[2] = (a[1]*a[3]*(x[0]-a[2])) / (CPL_MATH_PI*pow2aa);
    result[3] = a[1]/(CPL_MATH_2PI*aa) - (a[1]*pow(a[3], 2)) / 
    (4*CPL_MATH_PI*pow2aa);
    result[4] = x[0];
    return 0;
}

static int kmos_test(cpl_parameterlist * parlist, cpl_frameset * frameset)
{
    cpl_frame   *   frame_x ;
    cpl_frame   *   frame_y ;
    cpl_vector  *   vec_x ;
    cpl_vector  *   vec_y ;
    cpl_vector  *   sigma_y ;
    cpl_vector  *   vec_x_extract ;
    cpl_vector  *   vec_y_extract ;
	cpl_vector	*	fit_par ;
	cpl_vector	*	guess_vec ;
    double      *   pguess_vec ;
	cpl_vector	*	fit ;
    double      *   pfit ;
    cpl_matrix  *   x_mat ;
    double          line_center, line_width ;
    int             lowIndex, highIndex, i ;	
    double          red_chisq ;
    cpl_matrix  *   covariance ;
    double          x_array[1] ;
    double          y_array[1] ;
    
    /* Check entries */
    if (parlist == NULL || frameset == NULL) {
        cpl_msg_error(__func__, "Null Inputs") ;
        cpl_error_set(__func__, CPL_ERROR_NULL_INPUT) ;
        return -1 ;
    }

    /* Initialise */
    line_center = 0.88635 ;
    line_width = 0.013 ;
    red_chisq = 0.0 ;
    covariance = NULL ;

    /* Identify the RAW and CALIB frames in the input frameset */
    if (kmo_dfs_set_groups(frameset) != 1) {
        cpl_msg_error(__func__, "Cannot identify RAW and CALIB frames") ;
        cpl_error_set(__func__, CPL_ERROR_ILLEGAL_INPUT) ;
        return -1 ;
    }

    /* Load */
    frame_x = cpl_frameset_get_position(frameset, 0);
    frame_y = cpl_frameset_get_position(frameset, 1);
    vec_x = cpl_vector_load(cpl_frame_get_filename(frame_x), 0) ;
    vec_y = cpl_vector_load(cpl_frame_get_filename(frame_y), 0) ;

    /* Extract the part to fit */
    lowIndex = cpl_vector_find(vec_x, line_center - line_width/2);
    highIndex = cpl_vector_find(vec_x, line_center + line_width/2);
	vec_x_extract = cpl_vector_extract(vec_x, lowIndex, highIndex, 1);
    vec_y_extract = cpl_vector_extract(vec_y, lowIndex, highIndex, 1);

    cpl_plot_vector("set grid;", "t 'line to fit' w lines", "", vec_y_extract);

    /* Fitting parameters */
	fit_par = cpl_vector_new(5);

    /* Initial estimates */
    cpl_vector_set(fit_par, 0, 0.0);
    cpl_vector_set(fit_par, 0, 17000.0);
    cpl_vector_set(fit_par, 1, cpl_vector_get_min(vec_y_extract));
    cpl_vector_set(fit_par, 1, 17310.7);
    cpl_vector_set(fit_par, 1, 6000);
    cpl_vector_set(fit_par, 2, line_center);
    cpl_vector_set(fit_par, 2, 0.88635);
    cpl_vector_set(fit_par, 3, 0.000151367);
    cpl_vector_set(fit_par, 4, 0.0);
    cpl_vector_dump(fit_par, stdout);

    
    /* cpl_vector_set(fit_par, 0, 23000.0); */
    /* cpl_vector_set(fit_par, 1, 0.0); */
    /* cpl_vector_set(fit_par, 2, 0.0); */
    /* cpl_vector_set(fit_par, 3, 0.0); */
    /* cpl_vector_set(fit_par, 4, 0.0); */
 
    /* Compute the guess vector for plotting*/
    guess_vec = cpl_vector_new(cpl_vector_get_size(vec_y_extract));
    pguess_vec = cpl_vector_get_data(guess_vec);
	for (i = 0; i < cpl_vector_get_size(guess_vec); i++) {
		x_array[0] = cpl_vector_get(vec_x_extract, i);
		kmo_priv_lorentz1d_fnc(x_array, cpl_vector_get_data(fit_par),
				y_array);
		pguess_vec[i] = y_array[0];
	}
	cpl_plot_vector("set grid;", "t 'Guess' w lines", "", guess_vec);
    cpl_vector_delete(guess_vec) ;


    /* Prepare data for fitting */
    x_mat = cpl_matrix_wrap(cpl_vector_get_size(vec_x_extract), 1,
            cpl_vector_get_data(vec_x_extract));

    sigma_y = cpl_vector_new(cpl_vector_get_size(vec_x_extract));
    cpl_vector_fill(sigma_y, 1.0);

    /* Run fitting */
    if (cpl_fit_lvmq(x_mat, NULL, vec_y_extract, sigma_y, fit_par, 
            NULL, &kmo_priv_lorentz1d_fnc, &kmo_priv_lorentz1d_fncd,
            CPL_FIT_LVMQ_TOLERANCE,  /* 0.01 */
            CPL_FIT_LVMQ_COUNT,      /* 5 */
            CPL_FIT_LVMQ_MAXITER,    /* 1000 */
            NULL, &red_chisq, &covariance) != CPL_ERROR_NONE) {
        return -1 ;
    }
    cpl_vector_dump(fit_par, stdout);

    /* Compute the fitted spectrum */
    fit = cpl_vector_new(cpl_vector_get_size(vec_y_extract));
    pfit = cpl_vector_get_data(fit);
	for (i = 0; i < cpl_vector_get_size(fit); i++) {
		x_array[0] = cpl_vector_get(vec_x_extract, i);
		kmo_priv_lorentz1d_fnc(x_array, cpl_vector_get_data(fit_par),
				y_array);
		pfit[i] = y_array[0];
	}
	cpl_plot_vector("set grid;", "t 'Fitted' w lines", "", fit);

    cpl_vector_delete(vec_x) ;
    cpl_vector_delete(vec_x_extract) ;
    cpl_vector_delete(vec_y) ;
    cpl_vector_delete(vec_y_extract) ;

    return CPL_ERROR_NONE;
}


