/*
 * This file is part of the ESO SINFONI Pipeline
 * Copyright (C) 2004-2009 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, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */

/*
 * $Author: amodigli $
 * $Date: 2011/12/08 16:15:40 $
 * $Revision: 1.7 $
 */

#ifdef HAVE_CONFIG_H
#include <config.h>          /* allows the program compilation */
#endif

#include <cpl.h>
#include <string.h>
#include <math.h>
#include <sinfo_msg.h>
#include <sinfo_utils_wrappers.h>
#include "sinfo_atmo_disp.h"
#include "sinfo_resampling.h"
#include "sinfo_image_ops.h"

struct _disp_data
{
	double p1;
	double d1;
	double d2;
	double N0;
	double predelta;
	double parallactic_shiftX;
	double parallactic_shiftY;
};
typedef struct _disp_data disp_data;
static double sinfo_disp_calc_N(const disp_data* pdata, double lambda);
static void 
sinfo_disp_prepare_data(disp_data* pdata,
			double lambda0,
			double Tc,
			double rh,
			double airm,
			double p,
			double parallactic,
			double pixelscale );

static void 
sinfo_disp_calc(disp_data* pdata, double lambda, double *shiftx, double *shiftY);
static void 
sinfo_atmo_rotate_point(double* x_value, double * y_value, double rot_angle);
/*------------------------------------------------------------------------------------*/

static double 
sinfo_disp_calc_N(const disp_data* pdata, double lambda)
{
	double s = 1.0 /lambda;
	double s2 = s  * s;
	double s3 = s2 * s;
	double s4 = s3 * s;
	double s5 = s4 * s;
	double s6 = s5 * s;

	double a = 83939.7/(130   - s2);
	double b =   4547.3/(38.99 - s2);
	double c = 6487.31 + 58.058*s2 - 0.71150*s4 + 0.08851*s6;
	double N =  1e-8 * ( ((2371.34 + a + b) * pdata->d1)  +  ( c * pdata->d2));
	return N;

}
static void 
sinfo_disp_prepare_data(disp_data* pdata,
		double lambda0,
		double Tc,
        double rh,
        double airm,
        double p,
        double parallactic,
        double pixelscale )
{
    double ps,p2,p1,T,T2,T3;
    double zenith;
//    const double PI_NUMBer = 3.1415926535;

    T  = Tc + 273.15;T2 = T  * T; T3 = T2 * T;
    ps = -10474 + (116.43 * T) - (0.43284 *T2) + (0.00053840 * T3);
    p2 = (rh/100)*ps;
    p1 = p - p2;
    pdata->d1 = (p1/T)*(1+p1*( (57.90e-8) - ((9.3250e-4)/T) + (0.25844/T2)));
    pdata->d2 = (p2/T)*(1+p2*(1+3.7e-4*p2)*( (-2.37321e-3) + (2.23366/T) - (710.792/T2) + ((7.75141e-4)/T3) )) ;
	pdata->N0 = sinfo_disp_calc_N(pdata, lambda0);
    zenith     = acos(1/airm);
    pdata->predelta      = ((tan(zenith)) / (PI_NUMB/180)) * 3600;
    pdata->parallactic_shiftX = sin ( (parallactic)* (PI_NUMB/180) ) / pixelscale;
    pdata->parallactic_shiftY = cos ( (parallactic)* (PI_NUMB/180) ) / pixelscale;
    sinfo_msg_warning("atm disp N0[%f] d1[%f] d2[%f] pshiftX[%f] pshiftY[%f]",
    		pdata->N0,pdata->d1, pdata->d2,  pdata->parallactic_shiftX ,
    		pdata->parallactic_shiftY);
}
static void 
sinfo_disp_calc(disp_data* pdata, double lambda, double *shiftx, double *shifty)
{
	double n = sinfo_disp_calc_N(pdata, lambda);
	double delta = pdata->predelta * (n - pdata->N0);
	*shiftx = -delta * pdata->parallactic_shiftX;
	*shifty = delta * pdata->parallactic_shiftY ;
}

cpl_error_code 
sinfo_atm_dispersion_cube(cpl_imagelist* pCube,
		int centpix, // central plane in the cube CRPIX3
		double centlambda, // wavelength of the central plane CRVAL3
		double Tc, // temperature in Celsius TEL.AMBI.TEMP
		double Rh, // relative humidity in % TEL.AMBI.RHUM
		double airm, // airmass for the moment of observation TEL.AMBI.PRES
		double p, // atmospheric pressure TEL.AMBI.PRES
		double parallactic, // TEL.PARANG
		double pixelscale, // could be for SINFONI 0.025, 0.100, 0.250
		double pixelsz // microns per pixel CDELT3
		)
{
	cpl_error_code err = CPL_ERROR_NONE;
	int cubesize = cpl_imagelist_get_size(pCube);
	double * kernel = sinfo_generate_interpolation_kernel("default");
	disp_data ddata;
	int i = 0;

	sinfo_disp_prepare_data(&ddata, centlambda, Tc, Rh, airm, p, parallactic, pixelscale);

	for (i = 0; i < cubesize; i++)
	{
		double shiftx = 0;
		double shifty = 0;

		cpl_image* pnewImage = 0;
		// 1. get an image
		cpl_image* plane = cpl_imagelist_get(pCube, i);

		// 2. calculate dispersion and shift
		double lambda = centlambda - (centpix - i) * pixelsz;
		sinfo_disp_calc(&ddata, lambda, &shiftx, &shifty);
		// 3. aplly shift
//		int szx = cpl_image_get_size_x(plane);
		//int szy = cpl_image_get_size_y(plane);
		//if (i % 10 == 0)
			sinfo_msg_warning(" shift image #%d, dx[%f] dy[%f]", i, shiftx, shifty);
		pnewImage = sinfo_new_shift_image(
		    plane,
		    shiftx,
		    shifty,
		    kernel);
		err = cpl_imagelist_set(pCube, pnewImage, i);
		if (err != CPL_ERROR_NONE)
			break;
	}
	cpl_free(kernel);
	return err;

}


/*----------------------------------------------------
 * Atmospheric correction using polynomial fit
 *----------------------------------------------------*/
cpl_polynomial* 
sinfo_atmo_load_polynom(const char* filename)
{
	const char* COL_NAME_TEMPLATE = "col_%d";
	const char* COL_NAME_VALUE = "value";
	cpl_polynomial* poly = NULL;
	cpl_table* ptable = NULL;

	ptable = cpl_table_load(filename, 1, 0);
	if (ptable)
	{
		int dim = 0;
		int nrows = 0;
		int i = 0;
		cpl_size* expo = NULL;

		dim = cpl_table_get_ncol(ptable) - 1;
		poly = cpl_polynomial_new(dim );
		nrows = cpl_table_get_nrow(ptable);
		expo = cpl_malloc(dim * sizeof(expo[0]));
		memset(&expo[0], 0, dim * sizeof(expo[0]));
		for (i = 0; i < nrows; i++)
		{
			int j = 0;
			int inull = 0;
			double value = 0;
			for (j = 0; j < dim; j++)
			{
				char col_name[255];
				sprintf(col_name, COL_NAME_TEMPLATE, j);
				expo[j] = cpl_table_get_int(ptable, col_name, i, &inull);
			}
			value = cpl_table_get(ptable, COL_NAME_VALUE, i, &inull);
			cpl_polynomial_set_coeff(poly, expo, value);
			if (cpl_error_get_code() != CPL_ERROR_NONE)
			{
				if (poly)
				{
					sinfo_free_polynomial(&poly);
				}
				break;
			}
		}
		cpl_free(expo);
	}
	sinfo_free_table(&ptable);
	return poly;
}

static void 
sinfo_atmo_rotate_point(double* x_value, double * y_value, double rot_angle)
{
	double newx = *x_value * cos(rot_angle) - *y_value * sin(rot_angle);
	double newy = *x_value * sin(rot_angle) + *y_value * cos(rot_angle);
	*x_value = newx;
	*y_value = newy;
}

cpl_imagelist* 
sinfo_atmo_apply_cube_polynomial_shift(
		cpl_polynomial* poly,
		cpl_imagelist* pCube,
		double lambda0,
		double airmass,
		double parallactic, // should be in radian
		double pixelsz,
		int centpix)
{
	cpl_imagelist* retcube = NULL;
	cpl_error_code 	err = CPL_ERROR_NONE;
	cpl_vector* vparams = NULL;
	double * kernel = sinfo_generate_interpolation_kernel("default");
	int cubesize = 0;
	int i = 0;
	// the following two parameters are necessary for computing the shift
	// in case when polynom for H+K band is used for H or K
	double l0_shift_x = 0; // shift for the central point by X
	double l0_shift_y = 0; // shift for the central point by Y

	vparams = cpl_vector_new(2);
	cpl_vector_set(vparams, 0, airmass);
	cpl_vector_set(vparams, 1, lambda0);
	err = cpl_error_get_code();
	if (err == CPL_ERROR_NONE)
	{
		l0_shift_y = cpl_polynomial_eval(poly, vparams); // North - South
		l0_shift_x = 0; // (EAST-WEST direction)
		// rotate the shift
		sinfo_atmo_rotate_point(&l0_shift_x, &l0_shift_y, parallactic);
		cubesize = cpl_imagelist_get_size(pCube);
		err =  cpl_error_get_code();
	}
	if (err == CPL_ERROR_NONE)
	{
		retcube = cpl_imagelist_new();
		for (i = 0; i < cubesize; i++)
		{
			// calculate the wavelength
			double lambda = lambda0 - (centpix - i) * pixelsz;
			double shift_y = 0;
			double shift_x = 0;

			cpl_vector_set(vparams, 1, lambda);
			// calc the shift
			shift_y = cpl_polynomial_eval(poly, vparams); // North - South
			err = cpl_error_get_code();
			if (err == CPL_ERROR_NONE)
			{
				double res_shift_x = -(shift_x - l0_shift_x);
				double res_shift_y = -(shift_y - l0_shift_y);
				cpl_image* plane = NULL;
				cpl_image* pimresult = NULL;
				// rotate the shift
				sinfo_atmo_rotate_point(&res_shift_x, &res_shift_y, parallactic);
				plane = cpl_imagelist_get(pCube, i);
				pimresult = sinfo_new_shift_image(
						plane,
						res_shift_x, // x shift
						res_shift_y, // y shift
						kernel);
				if (err == CPL_ERROR_NONE)
				{
					err = cpl_imagelist_set(retcube, pimresult, i);
				}
				else
				{
					sinfo_msg_error("Error sinfo_new_shift_image, %s",
										cpl_error_get_where());
				}
				if (err != CPL_ERROR_NONE)
					break;
			}
			else
			{
				sinfo_msg_error("Error polynomial_eval, %s",
						cpl_error_get_where());
			}
			if (err != CPL_ERROR_NONE)
				break;
		}
	}
	if (err != CPL_ERROR_NONE)
	{
		sinfo_free_imagelist(&retcube);
		sinfo_msg_error("Error during shift planes in the cube, %s",
				cpl_error_get_where());
	}
    sinfoni_free_vector(&vparams);
	cpl_free(kernel);
	return retcube;
}
