/* $Id: kmo_priv_rotate.c,v 1.7 2013-06-17 12:58:47 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 12:58:47 $
 * $Revision: 1.7 $
 * $Name: not supported by cvs2svn $
 */

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

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

#include <math.h>

#include <cpl.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_rotate.h"

/*----------------------------------------------------------------------------*/
/**
    @defgroup kmos_priv_rotate     Helper functions for recipe kmo_rotate.

    @{
 */
/*----------------------------------------------------------------------------*/

/**
    @brief
        Rotates a a cube in spatial direction.

    If the rotation angle is a multiple of 90 degrees , only the WCS keywords in
    the headers are updated.
    If this is not the case, the data will be interpolated. The spatial size of
    the cube will grow accordingly.

    @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 rotation     The spatial rotation (counterclockwise) [degrees]
    @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_rotate(cpl_imagelist **data,
                              cpl_imagelist **noise,
                              cpl_propertylist **header_data,
                              cpl_propertylist **header_noise,
                              double rotation,
                              int flux,
                              int ifu_nr,
                              const char *method,
                              const enum extrapolationType extrapolate)
{
    double          flux_in             = 0.0,
                    flux_out            = 0.0,
                    rotation_sub        = 0.0,
                    cdelt1              = 0.0,
                    cdelt2              = 0.0,
                    nx_old              = 0.0,
                    ny_old              = 0.0,
                    nx_new              = 0.0,
                    ny_new              = 0.0,
                    cd1_1               = 0.0,
                    cd1_2               = 0.0,
                    old_ang             = 0.0,
                    new_ang             = 0.0,
                    new_ang_rad         = 0.0,
                    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;

    cpl_image       *tmp_img            = NULL;

    int             rotation_multiple   = 0,
                    mode_sigma          = 1000,
                    direction           = -1;    // CCW rotation direction
//                    direction           = 1;    // CW rotation direction

    cpl_imagelist   *tmp_imglist        = 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(cpl_imagelist_get_size(*data) > 0,
                               CPL_ERROR_ILLEGAL_INPUT,
                               "Input data is empty!");

        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(cpl_imagelist_get_size(*noise) > 0,
                                   CPL_ERROR_ILLEGAL_INPUT,
                                   "Input noise is empty!");
        }

        KMO_TRY_ASSURE((flux == 1) || (flux == 0),
                       CPL_ERROR_ILLEGAL_INPUT,
                       "flux must be 1 or 0!");

        // check if CRPIX is in the center of the cube
        KMO_TRY_EXIT_IF_NULL(
            tmp_img = cpl_imagelist_get(*data, 0));
        nx_old = cpl_image_get_size_x(tmp_img);
        ny_old = cpl_image_get_size_y(tmp_img);

        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 = nx_old/2.0 + 0.5;
        crpix2_new = ny_old/2.0 + 0.5;

        if ((fabs(crpix1_new - crpix1) > 0.001) ||
            (fabs(crpix2_new - crpix2) > 0.001))
        {
            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);

            // set CRPIX to the center of the cube
            crpix1 = crpix1_new;
            crpix2 = crpix2_new;

            // update CRVAL accordingly
            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, 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"));
            }

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

        // change WCS
        rotation_multiple = rotation/90;
        rotation_sub = rotation-(rotation_multiple*90);

        if ((fabs(rotation_sub) > 0.0001)) {
            // apply interpolated rotation

            // calculate flux_in if desired
            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);
                }
            }

            KMO_TRY_EXIT_IF_NULL(
                tmp_imglist = kmclipm_rotate(*data, -1*rotation*CPL_MATH_PI/180, method,
                                             extrapolate));
            cpl_imagelist_delete(*data);
            *data = tmp_imglist;

            if ((noise != NULL) && (*noise != NULL)) {
                KMO_TRY_EXIT_IF_NULL(
                    tmp_imglist = kmclipm_rotate(*noise, -1*rotation*CPL_MATH_PI/180,
                                                 method, extrapolate));
                cpl_imagelist_delete(*noise);
                *noise = tmp_imglist;
            }

            // apply flux conservation if desired
            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));
                }
            }
        } else {
            // apply rotation in changing WCS
            // (rotation is a multiple of 90 degrees)

            KMO_TRY_EXIT_IF_ERROR(
                kmo_imagelist_turn(*data, direction*rotation_multiple));

            if ((noise != NULL) && (*noise != NULL)) {
                KMO_TRY_EXIT_IF_ERROR(
                    kmo_imagelist_turn(*noise, direction*rotation_multiple));
            }
        }
        cpl_msg_info(cpl_func, "Applied rotation to IFU %d", ifu_nr);

        // update headers
        cd1_1 = cpl_propertylist_get_double(*header_data, CD1_1);
        cd1_2 = cpl_propertylist_get_double(*header_data, CD1_2);
        old_ang = atan(cd1_2/ cd1_1)*180/CPL_MATH_PI;
        new_ang = old_ang - direction*rotation;
        KMO_TRY_CHECK_ERROR_STATE();

        // get rid of rounding errors due to atan()
        if (fabs(floor(new_ang+0.5)-new_ang) < 1e-10) {
            new_ang = floor(new_ang+0.5);
        }

        new_ang_rad = new_ang * CPL_MATH_PI / 180;
        KMO_TRY_CHECK_ERROR_STATE();

        cdelt1 = cpl_propertylist_get_double(*header_data, CDELT1);
        KMO_TRY_CHECK_ERROR_STATE();

        cdelt2 = cpl_propertylist_get_double(*header_data, CDELT2);
        KMO_TRY_CHECK_ERROR_STATE();

        KMO_TRY_EXIT_IF_ERROR(
            kmclipm_update_property_double(*header_data, CD1_1,
                                           cdelt1 * cos(new_ang_rad),
                                           "[deg] Pixel resolution in x"));
        KMO_TRY_EXIT_IF_ERROR(
            kmclipm_update_property_double(*header_data, CD1_2,
                                           -cdelt2 * sin(new_ang_rad),
                                           "rotation in xy-plane, no skew"));
        KMO_TRY_EXIT_IF_ERROR(
            kmclipm_update_property_double(*header_data, CD2_1,
                                           cdelt1 *sin(new_ang_rad),
                                           "rotation in xy-plane, no skew"));
        KMO_TRY_EXIT_IF_ERROR(
            kmclipm_update_property_double(*header_data, CD2_2,
                                           cdelt2 * cos(new_ang_rad),
                                           "[deg] Pixel resolution in y"));

        // update CRPIX if cube got larger in x/y
        KMO_TRY_EXIT_IF_NULL(
            tmp_img = cpl_imagelist_get(*data, 0));
        nx_new = cpl_image_get_size_x(tmp_img);
        ny_new = cpl_image_get_size_y(tmp_img);

        KMO_TRY_EXIT_IF_ERROR(
            kmclipm_update_property_double(*header_data, CRPIX1,
                                           (nx_new-nx_old)/2+crpix1,
                                           "[pix] Reference pixel in x"));

        KMO_TRY_EXIT_IF_ERROR(
            kmclipm_update_property_double(*header_data, CRPIX2,
                                           (ny_new-ny_old)/2+crpix2,
                                           "[pix] Reference pixel in y"));

        if ((noise != NULL) && (*noise != NULL)) {
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_noise, CD1_1,
                                               cdelt1 * cos(new_ang_rad),
                                               "[deg] Pixel resolution in x"));
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_noise, CD1_2,
                                               -cdelt2 * sin(new_ang_rad),
                                               "rotation in xy-plane, no skew"));
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_noise, CD2_1,
                                               cdelt1 *sin(new_ang_rad),
                                               "rotation in xy-plane, no skew"));
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_noise, CD2_2,
                                               cdelt2 * cos(new_ang_rad),
                                               "[deg] Pixel resolution in y"));
            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_data, CRPIX1,
                                               (nx_new-nx_old)/2+crpix1,
                                               "[pix] Reference pixel in x"));

            KMO_TRY_EXIT_IF_ERROR(
                kmclipm_update_property_double(*header_data, CRPIX2,
                                               (ny_new-ny_old)/2+crpix2,
                                               "[pix] Reference pixel in y"));
        }
    }
    KMO_CATCH
    {
        KMO_CATCH_MSG();
        ret_error = cpl_error_get_code();
    }

    return ret_error;
}

/** @} */
