/* 
 * 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 ifu_nr       The number of the IFU being processed.
  @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,
        int                     ifu_nr,
        const char          *   method,
        const enum extrapolationType    extrapolate,
        int                     wcs_only)
{
    double          flux_in             = 0.0,
                    flux_out            = 0.0,
                    xshift_sub          = 0.0,
                    yshift_sub          = 0.0,
                    precision           = 1e-6,
                    crpix1              = 0.0,
                    crpix2              = 0.0,
                    crpix3              = 0.0,
                    crpix1_new          = 0.0,
                    crpix2_new          = 0.0,
                    crval1_new          = 0.0,
                    crval2_new          = 0.0,
                    mode_noise          = 0.0;
    int             xshift_int          = 0,
                    yshift_int          = 0,
                    mode_sigma          = 1000;
    cpl_imagelist   *data_out           = NULL,
                    *noise_out          = NULL;
    cpl_error_code  ret_error           = CPL_ERROR_NONE;
    cpl_wcs         *wcs                = NULL;
    cpl_matrix      *phys               = NULL,
                    *world              = NULL;
    cpl_array       *status             = NULL;

    KMO_TRY
    {
        /* Check inputs */
        KMO_TRY_ASSURE((data != NULL) && (header_data != NULL) &&
                (*data != NULL) && (*header_data != NULL),
                CPL_ERROR_NULL_INPUT, "Not all input data is provided!");

        KMO_TRY_ASSURE((wcs_only == TRUE) || (wcs_only == FALSE),
                CPL_ERROR_NULL_INPUT, "wcs_only must be TRUE or FALSE!");

        if ((noise != NULL) && (*noise != NULL)) {
            KMO_TRY_ASSURE((header_noise != NULL) && (*header_noise != NULL),
                    CPL_ERROR_NULL_INPUT, "header_noise isn't provided!");
        }

        KMO_TRY_ASSURE((flux == TRUE) || (flux == FALSE),
                CPL_ERROR_ILLEGAL_INPUT, "flux must be TRUE or FALSE!");

        /* 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 = xshift_sub+xshift_int;
        yshift = 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);
        KMO_TRY_CHECK_ERROR_STATE();

        crpix1_new = crpix1 - xshift;
        crpix2_new = crpix2 + yshift;

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

        KMO_TRY_EXIT_IF_NULL(wcs = cpl_wcs_new_from_propertylist(*header_data));

        KMO_TRY_EXIT_IF_ERROR(
            cpl_wcs_convert(wcs, phys, &world, &status, CPL_WCS_PHYS2WORLD));

        crval1_new = cpl_matrix_get(world, 1, 0);
        crval2_new = cpl_matrix_get(world, 1, 1);

        crpix1_new = crpix1-2*xshift;
        crpix2_new = crpix2+2*yshift;

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

        if ((noise != NULL) && (*noise != NULL)) {
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_noise, CRPIX1, 
                    crpix1_new, "[pix] Reference pixel in x"));
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_noise, CRPIX2, 
                    crpix2_new, "[pix] Reference pixel in y"));
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_noise, CRVAL1,
                    crval1_new, "[deg] RA at ref. pixel"));
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_noise, CRVAL2,
                    crval2_new, "[deg] DEC at ref. pixel"));
        }

        if (wcs_only == FALSE) {
            /* Apply shifts */
            /* sub-pixel-shift */
            if ((xshift_sub != 0.0) || (yshift_sub != 0.0)) {
                /* Calculate flux_in */
                if (flux == TRUE) {
                    KMO_TRY_EXIT_IF_ERROR(
                        kmo_calc_mode_for_flux_cube(*data, NULL, &mode_noise));

                    flux_in = kmo_imagelist_get_flux(*data);
                    KMO_TRY_CHECK_ERROR_STATE();

                    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 */
                KMO_TRY_EXIT_IF_NULL(
                    data_out = kmclipm_shift(*data, xshift_sub, -yshift_sub,
                                             method, extrapolate));
                cpl_imagelist_delete(*data); *data = data_out;

                if ((noise != NULL) && (*noise != NULL)) {
                    KMO_TRY_EXIT_IF_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_TRY_EXIT_IF_ERROR(
                        kmo_calc_mode_for_flux_cube(*data, NULL, &mode_noise));

                    flux_out = kmo_imagelist_get_flux(*data);
                    KMO_TRY_CHECK_ERROR_STATE();
                    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)) {
                        KMO_TRY_EXIT_IF_ERROR(
                            cpl_imagelist_multiply_scalar(*data, 
                                flux_in / flux_out));
                    }
                }
                if (ifu_nr > 0) {
                    cpl_msg_info(cpl_func, 
                            "Applied subpixel shift to IFU %d", ifu_nr);
                }
            }

            /* Whole-pixel-shift */
            if ((xshift_int != 0) || (yshift_int != 0)) {
                KMO_TRY_EXIT_IF_ERROR(
                    kmo_imagelist_shift(*data, -xshift_int, yshift_int));
                if ((noise != NULL) && (*noise != NULL)) {
                    KMO_TRY_EXIT_IF_ERROR(
                        kmo_imagelist_shift(*noise, -xshift_int, yshift_int));
                }
                if (ifu_nr > 0) {
                    cpl_msg_info(cpl_func, 
                            "Applied pixel shift to IFU %d", ifu_nr);
                }
            }
        } else {
            /* Update WCS only */
            if (ifu_nr > 0) {
                cpl_msg_info(cpl_func, "Updated WCS only on IFU %d", ifu_nr);
            }
        }
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        ret_error = cpl_error_get_code();
    }

    cpl_matrix_delete(phys); phys = NULL;
    cpl_matrix_delete(world); world = NULL;
    cpl_array_delete(status); status = NULL;
    cpl_wcs_delete(wcs); wcs = NULL;

    return ret_error;
}

/** @} */
