/* 
 * This file is part of the KMOS Pipeline
 * Copyright (C) 2002,2003 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, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

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

/*-----------------------------------------------------------------------------
 *                              Includes
 *----------------------------------------------------------------------------*/

#include <math.h>

#include <cpl.h>

#include "kmclipm_constants.h"

#include "kmo_debug.h"
#include "kmo_error.h"
#include "kmo_dfs.h"
#include "kmo_functions.h"
#include "kmo_cpl_extensions.h"
#include "kmo_priv_shift.h"

/*----------------------------------------------------------------------------*/
/**
    @defgroup kmos_priv_shift     Helper functions for recipe kmo_shift.
 */
/*----------------------------------------------------------------------------*/

/**@{*/

/*----------------------------------------------------------------------------*/
/**
  @brief Shifts a a cube in spatial direction.
  @param data         The image data (will be altered).
  @param noise        The image noise (will be altered).
  @param header_data  The image data header (will be altered).
  @param header_noise The image noise header (will be altered).
  @param xshift       The shift in x-direction (positive to the left) [pixel]
  @param yshift       The shift in y-direction (positive to the top) [pixel]
  @param flux         1 if flux should be conserved, 0 otherwise.
                      Flux changes only if a subpixel shift is applied.
  @param method       The interpolation method (BCS only).
  @param extrapolate  The extrapolation method (BCS_NATURAL, BCS_ESTIMATED,
                      NONE_NANS, RESIZE_BCS_NATURAL, RESIZE_BCS_ESTIMATED,
                      RESIZE_NANS)
  @return CPL_ERROR_NONE on success or a CPL error code otherwise.

  If the shift is a multiple of CDELT (either in x- or y-direction), only the
  WCS keywords in the headers are updated. If this is not the case, the data
  will be interpolated.
  Interpolation will be performed only on a subpixel basis. If the shift is
  bigger than CDELT, first WCS will be updated and then the subpixel shift is
  performed.

  Possible cpl_error_code set in this function:
  @c CPL_ERROR_NULL_INPUT    if any of the inputs is NULL.
  @c CPL_ERROR_ILLEGAL_INPUT if flux is anything other than 0 or 1.
*/
/*----------------------------------------------------------------------------*/
cpl_error_code kmo_priv_shift(
        cpl_imagelist       **  data,
        cpl_imagelist       **  noise,
        cpl_propertylist    **  header_data,
        cpl_propertylist    **  header_noise,
        double                  xshift,
        double                  yshift,
        int                     flux,
        const char          *   method,
        const enum extrapolationType    extrapolate)
{
    cpl_imagelist   *   data_out ;
    cpl_imagelist   *   noise_out ;
    cpl_wcs         *   wcs ;
    cpl_matrix      *   phys ;
    cpl_matrix      *   world ;
    cpl_array       *   status ;
    double              flux_in, flux_out, xshift_loc, yshift_loc, xshift_sub, 
                        yshift_sub, precision, crpix1, crpix2, crpix3, 
                        crpix1_new, crpix2_new, crval1_new, crval2_new, 
                        mode_noise ;
    int                 xshift_int, yshift_int, mode_sigma ;

    /* Check inputs */
    if (data==NULL || header_data==NULL || *data==NULL || *header_data==NULL) {
        return CPL_ERROR_NULL_INPUT ;
    }
    if ((noise != NULL) && (*noise != NULL)) {
        if (header_noise==NULL || *header_noise==NULL) {
            return CPL_ERROR_NULL_INPUT ;
        }
    }

    /* Initialise */
    precision = 1e-6 ;
    mode_sigma = 1000 ;

    /* Calculate amount of sub-/whole-pixel-shifts */

    /* Whole-pixel-shift */
    if (xshift >= 0.0)  xshift_int = xshift + precision;
    else                xshift_int = xshift - precision;
    if (yshift >= 0.0)  yshift_int = yshift + precision;
    else                yshift_int = yshift - precision;

    /* Sub-pixel-shift */
    xshift_sub = xshift-xshift_int;
    if (fabs(xshift_sub) < precision)   xshift_sub = 0.0;
    yshift_sub = yshift-yshift_int;
    if (fabs(yshift_sub) < precision)   yshift_sub = 0.0;

    /* One pixel shift  */
    if (xshift_sub > 0.5) {
        xshift_sub -= 1;
        xshift_int +=1;
    }
    if (yshift_sub > 0.5) {
        yshift_sub -= 1;
        yshift_int +=1;
    }

    xshift_loc = xshift_sub+xshift_int;
    yshift_loc = yshift_sub+yshift_int;

    crpix1 = cpl_propertylist_get_double(*header_data, CRPIX1);
    crpix2 = cpl_propertylist_get_double(*header_data, CRPIX2);
    crpix3 = cpl_propertylist_get_double(*header_data, CRPIX3);

    crpix1_new = crpix1 - xshift_loc;
    crpix2_new = crpix2 + yshift_loc;

    phys = cpl_matrix_new (2, 3);
    cpl_matrix_set(phys, 0, 0, crpix1);
    cpl_matrix_set(phys, 0, 1, crpix2);
    cpl_matrix_set(phys, 0, 2, crpix3);
    cpl_matrix_set(phys, 1, 0, crpix1_new);
    cpl_matrix_set(phys, 1, 1, crpix2_new);
    cpl_matrix_set(phys, 1, 2, crpix3);

    if ((wcs = cpl_wcs_new_from_propertylist(*header_data)) == NULL) {
        cpl_matrix_delete(phys); 
        return CPL_ERROR_ILLEGAL_INPUT ;
    }

    /* WCS Computation */
    if (cpl_wcs_convert(wcs, phys, &world, &status, 
                CPL_WCS_PHYS2WORLD) == CPL_ERROR_NONE) {
        /* New Values */
        crval1_new = cpl_matrix_get(world, 1, 0);
        crval2_new = cpl_matrix_get(world, 1, 1);
        crpix1_new = crpix1-2*xshift_loc;
        crpix2_new = crpix2+2*yshift_loc;

        /* Update WCS */
        kmclipm_update_property_double(*header_data, CRPIX1, crpix1_new,
                "[pix] Reference pixel in x");
        kmclipm_update_property_double(*header_data, CRPIX2, crpix2_new,
                "[pix] Reference pixel in y");
        kmclipm_update_property_double(*header_data, CRVAL1, crval1_new,
                "[deg] RA at ref. pixel");
        kmclipm_update_property_double(*header_data, CRVAL2, crval2_new,
                "[deg] DEC at ref. pixel");

        if ((noise != NULL) && (*noise != NULL)) {
            kmclipm_update_property_double(*header_noise, CRPIX1, crpix1_new, 
                    "[pix] Reference pixel in x");
            kmclipm_update_property_double(*header_noise, CRPIX2, crpix2_new, 
                    "[pix] Reference pixel in y");
            kmclipm_update_property_double(*header_noise, CRVAL1, crval1_new, 
                    "[deg] RA at ref. pixel");
            kmclipm_update_property_double(*header_noise, CRVAL2, crval2_new, 
                    "[deg] DEC at ref. pixel");
        }
    } else {
        cpl_msg_warning(__func__, "Failed to compute the new WCS") ;
        cpl_error_reset() ;
    } 
    cpl_matrix_delete(phys); 
    cpl_array_delete(status);
    cpl_wcs_delete(wcs);
    cpl_matrix_delete(world);

    /* Apply shifts */
    /* sub-pixel-shift */
    if ((xshift_sub != 0.0) || (yshift_sub != 0.0)) {

        /* Calculate flux_in */
    	flux_in = 0.;

        if (flux == TRUE) {
            kmo_calc_mode_for_flux_cube(*data, NULL, &mode_noise);
            flux_in = kmo_imagelist_get_flux(*data);
            if (isnan(mode_noise) || flux_in < mode_sigma*mode_noise) {
                flux_in = 0./0.;
                cpl_msg_warning("","Flux in <  %d*noise", mode_sigma);
            }
        }

        /* Apply subpixel-shift */
        /* Sign of xshift_sub is inverted, since orientation of */
        /* x-axis goes from rigth to left */
        data_out = kmclipm_shift(*data, xshift_sub, -yshift_sub, method, 
                extrapolate);
        cpl_imagelist_delete(*data); 
        *data = data_out;

        if ((noise != NULL) && (*noise != NULL)) {
            noise_out = kmclipm_shift(*noise, xshift_sub, -yshift_sub, method, 
                    extrapolate);
            cpl_imagelist_delete(*noise); 
            *noise = noise_out;
        }

        /* Apply flux conservation */
        if (flux == TRUE) {
            kmo_calc_mode_for_flux_cube(*data, NULL, &mode_noise);

            flux_out = kmo_imagelist_get_flux(*data);
            if (isnan(mode_noise) || flux_out < mode_sigma*mode_noise) {
                flux_out = 0./0.;
                cpl_msg_warning("","Flux out <  %d*noise", mode_sigma);
            }
            if (!isnan(flux_in) && !isnan(flux_out)) {
                cpl_imagelist_multiply_scalar(*data, flux_in / flux_out);
            }
        }
    }

    /* Whole-pixel-shift */
    if ((xshift_int != 0) || (yshift_int != 0)) {
        kmo_imagelist_shift(*data, -xshift_int, yshift_int);
        if ((noise != NULL) && (*noise != NULL)) {
            kmo_imagelist_shift(*noise, -xshift_int, yshift_int);
        }
    }

    return CPL_ERROR_NONE ;
}

/** @} */
