#include "i3_model.h"
#include "i3_model_tools.h"
#include "i3_load_data.h"
#include "i3_data_set.h"
#include "i3_great.h"
#include "i3_mcmc.h"
#include "i3_math.h"
#include "i3_minimizer.h"
#include "i3_fisher.h"
#include "i3_psf.h"
#include "i3_image_fits.h"
#include "i3_model_tools.h"
#include "i3_utils.h"
#include "i3_options.h"
#include <time.h>

static bool MILESTONE_CENTRAL_PIXEL_TRICK = false;
void i3_milestone_image(i3_milestone_parameter_set * p, i3_data_set * dataset, i3_image * model_image, i3_flt A, int proc_id, int verb){

	char filename[128];
	//if(verb>10) printf("getting milestone image\n");

// get the image parameters

	int n_sub = dataset->upsampling;
	int n_pix = model_image->nx;
	int n_pad = dataset->padding;
	int n_all = n_pix+n_pad;

// Prepare the Sersic parameters for the bulge and the disc.

	i3_flt sersic_dvc = 4.0;
	i3_flt sersic_exp = 1.0;
	i3_flt ro_dvc = p->radius;
	i3_flt ro_exp = p->radius*2;
	i3_flt ab_dvc = (ro_dvc*n_sub)*(ro_dvc*n_sub);
	i3_flt ab_exp = (ro_exp*n_sub)*(ro_exp*n_sub);
	i3_flt truncation_factor = 3.0;
	i3_flt A_dvc = A/(1.-A);
	i3_flt A_exp = 1.;
	i3_flt x0 = p->x0*n_sub+n_sub/2+n_pad*n_sub/2;
	i3_flt y0 = p->y0*n_sub+n_sub/2+n_pad*n_sub/2;

	if(verb>0) printf("gal params: r_dvc=%2.4f r_exp=%2.4f %2.4f %2.4f %2.4f %2.4f\n",ro_dvc,ro_exp,A_dvc,A_exp,x0,y0);

// allocate memory

 	//if(verb>10) printf("allocating memory\n");


	i3_image * high_resolution_image_pad_dvc = i3_image_create(n_all*n_sub,n_all*n_sub);	i3_image_zero(high_resolution_image_pad_dvc);
	i3_image * high_resolution_image_pad_exp = i3_image_create(n_all*n_sub,n_all*n_sub);	i3_image_zero(high_resolution_image_pad_exp);

	i3_image * low_resolution_image_dvc = i3_image_create(n_pix,n_pix); 	i3_image_zero(low_resolution_image_dvc);
	i3_image * low_resolution_image_exp = i3_image_create(n_pix,n_pix); 	i3_image_zero(low_resolution_image_exp);

 	//if(verb>10) printf("allocated memory\n");

	int n_central_upsampling = 9;
	int n_central_pixels_to_upsample = 10;


// Make the de Vaucolouers bulge, truncated -- 20110809TK did not modify this section except variable rename


	i3_add_real_space_sersic_truncated_radius(high_resolution_image_pad_dvc, ab_dvc, p->e1, p->e2, A_dvc, x0, y0, sersic_dvc, truncation_factor);
	if(verb>10){ snprintf( filename, 128, "image_gal_%03d_nopsf_hires_nocpu_dvc.fits", proc_id );	i3_image_save_fits( high_resolution_image_pad_dvc, filename );}

	i3_image_zero(high_resolution_image_pad_dvc);
	i3_add_real_space_sersic_truncated_radius_upsample_central(high_resolution_image_pad_dvc, ab_dvc, p->e1, p->e2, A_dvc, x0, y0, sersic_dvc, truncation_factor, n_central_pixels_to_upsample, n_central_upsampling);
	if(verb>10){ snprintf( filename, 128, "image_gal_%03d_nopsf_hires_cpu_dvc.fits", proc_id );	i3_image_save_fits( high_resolution_image_pad_dvc, filename );}


	{
		i3_flt r0 = i3_sqrt(ab_dvc);
		i3_flt truncation = truncation_factor*r0;
		i3_flt analytic_flux = i3_sersic_flux_to_radius(sersic_dvc, A_dvc, ab_dvc, truncation);
		i3_flt current_flux = i3_image_sum(high_resolution_image_pad_dvc);
		int peak_row = (int) x0;
		int peak_col = (int) y0;

		if (MILESTONE_CENTRAL_PIXEL_TRICK) if ( peak_row>=0 && peak_row<high_resolution_image_pad_dvc->nx && peak_col>=0 && peak_col<high_resolution_image_pad_dvc->ny ) high_resolution_image_pad_dvc->row[peak_row][peak_col] += (analytic_flux-current_flux);
		if((analytic_flux-current_flux) < 0) {dataset->n_logL_warning++;};
		if(verb>0) printf("dvc analytic current = %1.4e\t%1.4e \n",analytic_flux,current_flux);
	}


// Make the exponential disc, also truncated

	i3_add_real_space_sersic_truncated_radius(high_resolution_image_pad_exp, ab_exp, p->e1, p->e2, A_exp, x0, y0, sersic_exp, truncation_factor);

	{
		i3_flt r0 = i3_sqrt(ab_exp);
		i3_flt truncation = truncation_factor*r0;
		i3_flt analytic_flux = i3_sersic_flux_to_radius(sersic_exp, A_exp, ab_exp, truncation);
		i3_flt current_flux = i3_image_sum(high_resolution_image_pad_exp);
		int peak_row = (int) x0;
		int peak_col = (int) y0;
		if (MILESTONE_CENTRAL_PIXEL_TRICK) if ( peak_row>=0 && peak_row<high_resolution_image_pad_exp->nx && peak_col>=0 && peak_col<high_resolution_image_pad_exp->ny ) high_resolution_image_pad_exp->row[peak_row][peak_col] += (analytic_flux-current_flux);
		if(verb>0) printf("exp analytic current = %1.4e\t%1.4e \n",analytic_flux,current_flux);
	}

// Scale

	i3_flt sum_dvc = i3_image_sum( high_resolution_image_pad_dvc );	i3_image_scale( high_resolution_image_pad_dvc, 1./sum_dvc * A_dvc );
	i3_flt sum_exp = i3_image_sum( high_resolution_image_pad_exp );	i3_image_scale( high_resolution_image_pad_exp, 1./sum_exp * A_exp );

	//if(verb>0) printf("sum_dvc=%2.4f sum_exp=%2.4f\n",sum_dvc,sum_exp);

	sum_dvc = i3_image_sum( high_resolution_image_pad_dvc );
	sum_exp = i3_image_sum( high_resolution_image_pad_exp );

	//if(verb>0) printf("sum_dvc=%2.4f sum_exp=%2.4f\n",sum_dvc,sum_exp);

// noise_bias_calibration testing section

 	if(verb>10){ snprintf( filename, 128, "image_gal_%03d_nopsf_hires_dvc.fits", proc_id );	i3_image_save_fits( high_resolution_image_pad_dvc, filename );}
 	if(verb>10){ snprintf( filename, 128, "image_gal_%03d_nopsf_hires_exp.fits", proc_id );	i3_image_save_fits( high_resolution_image_pad_exp, filename );}

// Convolve with the pre-prepared point spread function and Sinc kernel

	//if(verb>0) printf("convolving\n");

	i3_image_convolve_fourier( high_resolution_image_pad_dvc, dataset->psf_downsampler_kernel );
	i3_image_convolve_fourier( high_resolution_image_pad_exp, dataset->psf_downsampler_kernel );

// cut out the middle (remove padding) -- cutting and downsampling could be done in one step, I will do it soon TK20110809

	i3_image * high_resolution_image_dvc = i3_image_copy_part( high_resolution_image_pad_dvc , (n_pad/2)*n_sub, (n_pad/2)*n_sub, n_pix*n_sub, n_pix*n_sub );
	i3_image * high_resolution_image_exp = i3_image_copy_part( high_resolution_image_pad_exp , (n_pad/2)*n_sub, (n_pad/2)*n_sub, n_pix*n_sub, n_pix*n_sub );

// downsample

	i3_image_dsample_into( high_resolution_image_dvc, low_resolution_image_dvc );
	i3_image_dsample_into( high_resolution_image_exp, low_resolution_image_exp );

// this bit is only for noise_bias_calibration

	i3_image_add_images_into( low_resolution_image_dvc, low_resolution_image_exp, model_image );

// this is just testing

	//if(verb>0) printf("saving images\n");

	if(verb>10) {snprintf( filename, 128, "image_gal_%03d_hires_dvc.fits", proc_id );	i3_image_save_fits( high_resolution_image_dvc, filename );}
	if(verb>10) {snprintf( filename, 128, "image_gal_%03d_hires_exp.fits", proc_id );	i3_image_save_fits( high_resolution_image_exp, filename );}

	if(verb>10) {snprintf( filename, 128, "image_gal_%03d_dvc.fits", proc_id );	i3_image_save_fits( low_resolution_image_dvc, filename );}
	if(verb>10) {snprintf( filename, 128, "image_gal_%03d_exp.fits", proc_id );	i3_image_save_fits( low_resolution_image_exp, filename );}

	//if(verb>0) printf("high_resolution_image_dvc %d %d\n",(int)high_resolution_image_dvc->nx,(int)high_resolution_image_dvc->ny);
	//if(verb>0) printf("low_resolution_image_dvc %d %d\n",(int)low_resolution_image_dvc->nx,(int)low_resolution_image_dvc->ny);
	//if(verb>0) printf("model_image %d %d\n",(int)model_image->nx,(int)model_image->ny);


// clean up

	i3_image_destroy(high_resolution_image_dvc);
	i3_image_destroy(high_resolution_image_exp);

	i3_image_destroy(high_resolution_image_pad_exp);
	i3_image_destroy(high_resolution_image_pad_dvc);

	i3_image_destroy(low_resolution_image_dvc);
	i3_image_destroy(low_resolution_image_exp);
}

int main(int argc, char *argv[]){

// init

	i3_fftw_load_wisdom();
	atexit(i3_fftw_save_wisdom);

	char * filename_ini = "ini/i3_central_pixel_test.ini";
	int psf_type = 0;
	i3_flt psf_beta = 3;
	i3_flt psf_fwhm = 3.33;
	i3_flt psf_e1 = 0.0;
	i3_flt psf_e2 = 0.0;
	i3_flt gal_r = 3;
	i3_flt gal_A = 0.5;
	i3_flt gal_e1 = 0.0;
	i3_flt gal_e2 = 0.3;
	i3_flt snr = atof(argv[15]);
	int proc_id = 0;


// init the dataset

	i3_data_set * dataset = (i3_data_set*)malloc(sizeof(i3_data_set));

// load the options

	i3_options * options = i3_options_default();

	i3_options_read(options, filename_ini);

	int n_pix = options->stamp_size;
	int n_sub = options->upsampling;
	int n_pad = options->padding;
	int n_all = n_pix + n_pad; // total number of pixels, including padding, NOT including subpixels

	int verb  = 12;

	// load the options
	
	i3_model * model = i3_model_create(options->model_name,options);

	dataset->upsampling=n_sub;
	dataset->padding=n_pad;


	i3_init_rng_multiprocess(proc_id);

// make psf

	i3_fourier * fourier_psf = i3_fourier_create( n_all*n_sub, n_all*n_sub );
	i3_flt psf_r_trunc = psf_fwhm*2.;
	i3_fourier * fourier_ker = i3_fourier_conv_kernel_moffat(n_pix, n_sub, n_pad, psf_beta, psf_fwhm, psf_e1, psf_e2, psf_r_trunc,psf_type);
	dataset->psf_downsampler_kernel = fourier_ker;

// create the images centered on x + delta_x

	int n_delta_x =10;
	i3_flt delta_x =1./n_sub/n_delta_x;
	
	for(int idx = 0; idx<n_delta_x; idx++){

		proc_id++;

		printf("%2.2f\n",delta_x*idx);
		
		i3_milestone_parameter_set * params_gal = malloc(model->nbytes);;
		params_gal->x0= n_pix/2 + delta_x*idx;
		params_gal->y0= n_pix/2 + delta_x*idx;;
		//params_gal->x0= n_pix/2 + i3_random_normal()*0.5;
		//params_gal->y0= n_pix/2 + i3_random_normal()*0.5;
		params_gal->e1 = gal_e1;
		params_gal->e2 = gal_e2;
		params_gal->radius= gal_r;
		i3_image * image_gal = i3_image_create( n_pix, n_pix );
		i3_milestone_image(params_gal,dataset,image_gal,gal_A,proc_id,verb);

		if(verb > 10) {char filename[128]; snprintf( filename, 128, "image_gal_%03d.fits", proc_id ); i3_image_save_fits( image_gal, filename );}
	}

}