/* $Id: kmo_priv_shift.c,v 1.7 2013-06-17 08:01:40 aagudo Exp $
 *
 * 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
 */

/*
 * $Author: aagudo $
 * $Date: 2013-06-17 08:01:40 $
 * $Revision: 1.7 $
 * $Name: not supported by cvs2svn $
 */

#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.

    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.

    @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
        The function returns CPL_ERROR_NONE on success or a CPL error code
        otherwise.

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

        // check if subpixel-shift is <= 0.5, if not subtract 1 from it and increase
        // the whole pixel shift by one
        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
        //
//        crpix1 = cpl_propertylist_get_double(*header_data, CRPIX1);
//        KMO_TRY_CHECK_ERROR_STATE();

        KMO_TRY_EXIT_IF_ERROR(
            kmclipm_update_property_double(*header_data, CRPIX1,
                                           crpix1_new,
                                           "[pix] Reference pixel in x"));

//        crpix2 = cpl_propertylist_get_double(*header_data, CRPIX2);
//        KMO_TRY_CHECK_ERROR_STATE();

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

                    // apply flux conservation
                    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;
}

/** @} */
