/*
 * 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 ;
}

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;
}
/**
 * @name sinfo_atm_dispersion_cube
 *
 * @short correct object position in cube for atmospheric dispersion
 *
 * @param pCube        input imagelist
 * @param centpix      central plane in the cube CRPIX3
 * @param centlambda   wavelength of the central plane CRVAL3
 * @param Tc           temperature in Celsius TEL.AMBI.TEMP
 * @param Rh           relative humidity in % TEL.AMBI.RHUM
 * @param airm         airmass for the moment of observation TEL.AMBI.PRES
 * @param p            atmospheric pressure TEL.AMBI.PRES
 * @param parallactic  TEL.PARANG
 * @param pixelscale   could be for SINFONI 0.025, 0.100, 0.250
 * @param pixelsz      microns per pixel CDELT3
 *
 * @doc Performs atmospheric correction using polynomial fit coefficients
 *
 * @return CPL_ERROR_NONE if successful else CPL_ERROR_CODE
 */

cpl_error_code 
sinfo_atm_dispersion_cube(cpl_imagelist* pCube,
                          int centpix,
                          double centlambda,
                          double Tc,
                          double Rh,
                          double airm,
                          double p,
                          double parallactic,
                          double pixelscale,
                          double pixelsz
)
{
    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;

}

/**
 * @name sinfo_atmo_load_polynom
 *
 * @short set polynomial dispersion coefficients from input table (filename)
 *
 * @param filename        input table filename
 *
 * @return cpl polynomial with dispersion coefficients
 */

cpl_polynomial* 
sinfo_atmo_load_polynom(const char* filename)
{

    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]));
        const char* COL_NAME_TEMPLATE = "col_%d";
        const char* COL_NAME_VALUE = "value";
        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;
}

/**
 * @name sinfo_atmo_apply_cube_polynomial_shift
 *
 * @short Applies polynomial dispersion correction to input cube
 *
 * @param poly        polynomial correction
 * @param pCube       cube to correct
 * @param lambda0     wavelength setting value
 * @param airmass     airmass value
 * @param parallactic parallactic coordinate
 * @param pixelsz     pixel size
 * @param centpix     central pixel position
 *
 * @return cpl polynomial with dispersion coefficients
 */


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_vector* vparams = NULL;
    double * kernel = sinfo_generate_interpolation_kernel("default");
    int cubesize = 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);

    if (CPL_ERROR_NONE == cpl_error_get_code())
    {

        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);

    }
    if (CPL_ERROR_NONE == cpl_error_get_code())
    {
        retcube = cpl_imagelist_new();
        for (int i = 0; i < cubesize; i++)
        {
            // calculate the wavelength
            double lambda = lambda0 - (centpix - i) * pixelsz;


            cpl_vector_set(vparams, 1, lambda);
            // calc the shift

            double shift_y = cpl_polynomial_eval(poly, vparams); // North - South
            double shift_x = 0;
            if (CPL_ERROR_NONE == cpl_error_get_code())
            {
                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 (CPL_ERROR_NONE == cpl_error_get_code())
                {
                    cpl_imagelist_set(retcube, pimresult, i);
                }
                else
                {
                    sinfo_msg_error("Error sinfo_new_shift_image, %s",
                                    cpl_error_get_where());
                }
                if (CPL_ERROR_NONE != cpl_error_get_code())
                    break;
            }
            else
            {
                sinfo_msg_error("Error polynomial_eval, %s",
                                cpl_error_get_where());
            }
            if (CPL_ERROR_NONE != cpl_error_get_code())
                break;
        }
    }
    if (CPL_ERROR_NONE != cpl_error_get_code())
    {
        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;
}
