/* $Id: visir_spc_distortion.c,v 1.45 2011/11/29 09:28:59 llundin Exp $
 *
 * This file is part of the VISIR 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., 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */

/*
 * $Author: llundin $
 * $Date: 2011/11/29 09:28:59 $
 * $Revision: 1.45 $
 * $Name: visir-3_5_1 $
 */

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

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

#include <string.h>
#include <math.h>

#include <cpl.h>

#include "visir_spc_distortion.h"
#include "visir_utils.h"

/*----------------------------------------------------------------------------*/
/**
 * @defgroup visir_spc_distortion   Distortion estimation and correction
 */
/*----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------
                             Private Functions prototypes
 -----------------------------------------------------------------------------*/

static cpl_error_code visir_spc_det_warp_xy(int, double, double, double, double,
                                            double, double, double, double,
                                            double *, double*);

/*-----------------------------------------------------------------------------
                                   Functions code
 -----------------------------------------------------------------------------*/
/**@{*/

/*----------------------------------------------------------------------------*/
/**
  @brief    Correction of various VISIR detector deficiencies
  @param    images    Array of either INTERM or Half-Cycle images to correct
  @param    nimages   (Positive) number of images in array
  @param    is_interm True iff the images are INTERM
  @param    wlen      Central wavelength
  @param    resol     Resolution
  @param    phi       The skew of slit [degrees]
  @param    ksi       The skew of spectrum [degrees]
  @param    eps       The horizontal curvature [pixel]
  @param    delta     The vertical curvature [pixel]
  @param    doplot    Plotting level (zero for none)
  @return   CPL_ERROR_NONE, or the relevant CPL_ERROR.
  @note Currently, nimages must be 1 for Half-Cycle images


  Half-cycle images are extrapolated into dead rows of pixels at top and bottom.

  Corrects the detector warping

  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_ILLEGAL_INPUT
  - CPL_ERROR_NULL_INPUT

 */
/*----------------------------------------------------------------------------*/
cpl_error_code visir_spc_det_fix(cpl_image ** images, int nimages,
                                 cpl_boolean is_interm,
                                 double wlen, visir_spc_resol resol,
                                 double phi, double ksi, double eps, 
                                 double delta, int doplot)
{

    cpl_image * spectrum = NULL;

    int nx = 0;  /* Avoid (false) uninit warning */
    int ny = 0;  /* Avoid (false) uninit warning */
    int k;


    skip_if (0);

    skip_if (images == NULL);
    skip_if (nimages <= 0);

    skip_if (!is_interm && nimages != 1);

    for (k=0; k < nimages; k++) {
        if (images[k] == NULL) break;

        if (cpl_image_get_type(images[k]) != CPL_TYPE_FLOAT) {
            /* Done only with --fixcombi (used for backwards compatability */
            cpl_image * to_float = cpl_image_cast(images[k], CPL_TYPE_FLOAT);

            cpl_image_delete(images[k]);

            images[k] = to_float;

        }

        if (k == 0) {
            nx = cpl_image_get_size_x(images[0]);
            ny = cpl_image_get_size_y(images[0]);

            skip_if (nx < 1);
        } else {
            skip_if (nx != cpl_image_get_size_x(images[k]));
            skip_if (ny != cpl_image_get_size_y(images[k]));
        }

    }

    /* In HR only phi is used */
    /* HR Grism mode has its own recipe and needs no hard-coded changes here */
    if (resol == VISIR_SPC_R_HR) {

        visir_optmod ins_settings;

        if (visir_spc_optmod_init(VISIR_SPC_R_HR, wlen, &ins_settings)) {
            visir_error_set(CPL_ERROR_ILLEGAL_INPUT);
            skip_if(1);
        }

        if (visir_spc_optmod_side_is_A(&ins_settings)) {
            /* Line curvature is zero in HR-A */
            eps   = 0;
            /* Spectrum curvature is 1.5 times bigger in HR-A than in LMR */
            delta *= 1.5;
        } else {
            /* Line curvature is 2.21 times bigger in HR-B than in LR */
            eps   *= 0.115/0.052;
            /* Spectrum curvature is 2 times smaller in HR-B than in LMR */
            delta *= 0.5;
        }
        ksi = 0; /* ksi is zero */
    } else if (resol == VISIR_SPC_R_MR) {
        eps *=  0.25; /* Line curvature is 4 times smaller in MR than in LR */
    }

    cpl_msg_info(cpl_func, "Skew and Curvature: %g %g %g %g", phi, ksi, eps,
                 delta);

    /* The angles are coded in degrees and then converted to radians */
    ksi *= CPL_MATH_RAD_DEG; 
    phi *= CPL_MATH_RAD_DEG;

    if (!is_interm) {

        float * px;
        const double fdead = 0.25;
        int ndead = 0; /* Number of rows of dead pixels */
        const int mdeadmax = 4; /* Check 3 outermost rows */
        int i,j;

        skip_if (visir_image_reject_hot(images[0], NULL));

        /* Check that the top row of pixels are not too small */

        spectrum = cpl_image_collapse_create(images[0], 1);
        skip_if (0);

        px = cpl_image_get_data(spectrum);

        if (doplot > 1) visir_image_col_plot("", "t 'The collapsed 1/2-cycle "
                                             "spectrum'", "", spectrum, 1,1,1);

        for (j = 1; j < mdeadmax; j++) {
            const float g0 = px[ny  -j] - px[ny-1-j];
            const float g1 = px[ny-1-j] - px[ny-2-j];

            cpl_msg_debug(cpl_func, "GRAD(%g): %g <> %g", ny-j-0.5, g0, g1);

            if ((g1 < 0 && g0 * fdead < g1) || (g1 >=0 && g0 < g1 * fdead))
                ndead = j;
        }

        if (ndead) {
            cpl_msg_info(cpl_func, "Rejecting dead pixels %d -> %d. "
                            "GRAD: %g << %g", ny-ndead, ny-1,
                            px[ny  -ndead] - px[ny-1-ndead],
                            px[ny-1-ndead] - px[ny-2-ndead]);

            for (i = 0; i < nx; i++) {
                for (j = ny-ndead; j < ny; j++) {
                    skip_if (cpl_image_reject(images[0], i+1, j+1));
                }
            }
            ndead = 0;
        }

        px = cpl_image_get_data(spectrum);

        for (j = 1; j < mdeadmax; j++) {
            const float g0 = px[j-1] - px[j];
            const float g1 = px[j] - px[j+1];

            cpl_msg_debug(cpl_func, "GRAD(%g:%d): %g <> %g", j-0.5, ndead, g0, g1);
     
            if ((g1 < 0 && g0 * fdead < g1) || (g1 >=0 && g0 < g1 * fdead))
                ndead = j;
        }

        if (ndead) {
            cpl_msg_info(cpl_func, "Rejecting dead pixels 1 -> %d. GRAD: "
                            "%g << %g", ndead, px[ndead-1] - px[ndead],
                            px[ndead] - px[ndead+1]);

            for (i = 0; i < nx; i++) {
                for (j = 0; j < ndead; j++) {
                    skip_if (cpl_image_reject(images[0], i+1, j+1));
                }
            }
        }
        cpl_image_delete(spectrum);
        spectrum = NULL;

    }

    /* Apply the distortion correction  */
    skip_if (visir_spc_det_warp(images, nimages, phi, ksi, eps, delta));

    if (doplot > 1) visir_image_plot("", "t 'The first corrected image'",
                                     "", images[0]);

    end_skip;

    cpl_image_delete(spectrum);

    return cpl_error_get_code();

}


/*----------------------------------------------------------------------------*/
/**
  @brief    The warping of the spectral VISIR detector
  @param    images  Array of float images to correct
  @param    nimages   (Positive) number of images in array
  @param    phi    The skew of slit [radian]
  @param    ksi    The skew of spectrum [radian]
  @param    eps    The horizontal curvature [pixel]
  @param    delta  The vertical curvature [pixel]
  @return   CPL_ERROR_NONE, or the relevant CPL_ERROR.

  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_ILLEGAL_INPUT
  - CPL_ERROR_NULL_INPUT

 */
/*----------------------------------------------------------------------------*/
cpl_error_code visir_spc_det_warp(cpl_image ** images, int nimages,
                                         double phi, double ksi,
                                         double eps, double delta)
{


    const double radius = CPL_KERNEL_DEF_WIDTH;
    const int tabsperpix = CPL_KERNEL_TABSPERPIX;
    const int nx = cpl_image_get_size_x(images[0]);
    const int ny = cpl_image_get_size_y(images[0]);
    const cpl_boolean do_warp = phi != 0 || ksi != 0 || eps != 0 || delta != 0;
    int iu, iv;
    int iimage;
    int ipix;

    cpl_image  * copy = NULL;
    cpl_vector * xyprofile = NULL;
    cpl_image  * coord_x = NULL;
    cpl_image  * coord_y = NULL;
    double * piv = NULL; /* Avoid (false) uninit warning */
    double * piu = NULL;
    cpl_error_code didfail = CPL_ERROR_NONE;
    cpl_errorstate cleanstate = cpl_errorstate_get();


    skip_if (0);

    if (do_warp) {
        double x = 0.0; /* Avoid (false) uninit warning */ 
        double y = 0.0; /* Avoid (false) uninit warning */ 

        /* Avoid unnecessary interpolation */

        xyprofile = cpl_vector_new(1 + radius * tabsperpix);
        cpl_vector_fill_kernel_profile(xyprofile, CPL_KERNEL_DEFAULT, radius);
        coord_x = cpl_image_new(nx, ny, CPL_TYPE_DOUBLE);
        coord_y = cpl_image_new(nx, ny, CPL_TYPE_DOUBLE);

        piu = cpl_image_get_data(coord_x);
        piv = cpl_image_get_data(coord_y);

        skip_if (0);

        for (ipix = 0, iv = 1; iv <= ny; iv++) {
            for (iu = 1; iu <= nx; iu++, ipix++) {

                skip_if (visir_spc_det_warp_xy(1, nx, ny, iu, iv, phi, ksi, eps,
                                               delta, &x, &y));
                piu[ipix] = x;
                piv[ipix] = y;
            }
        }

    }

#ifdef _OPENMP
#pragma omp parallel for private(iimage, ipix, iv, iu, copy)
#endif
    for (iimage = 0 ; iimage < nimages; iimage++) {
        cpl_image * image = images[iimage];
        cpl_mask  * rejected = NULL;
        float     * px    = cpl_image_get_data(image);
        double fluxprev, fluxpost;
        int nbad = 0; /* Avoid (false) uninit warning */
        cpl_binary * preject = NULL;


        if (!do_warp || cpl_msg_get_level() <= CPL_MSG_DEBUG) {
            nbad = cpl_image_count_rejected(image);

            cpl_msg_debug(cpl_func, "Raw image has %d bad pixels", nbad);
        }

        if (cpl_error_get_code()) {
            didfail = cpl_error_set(cpl_func, cpl_error_get_code());
            irplib_error_recover(cleanstate, "Failure for image %d", iimage+1);
            continue;
        }

        if (!do_warp && nbad == 0) continue;

        fluxprev = cpl_image_get_flux(image);

        if (cpl_error_get_code()) {
            didfail = cpl_error_set(cpl_func, cpl_error_get_code());
            irplib_error_recover(cleanstate, "Failure for image %d", iimage+1);
            continue;
        }

        if (do_warp) {
            copy = cpl_image_duplicate(image);

            if (cpl_error_get_code()) {
                didfail = cpl_error_set(cpl_func, cpl_error_get_code());
                irplib_error_recover(cleanstate, "Failure for image %d",
                                     iimage+1);
                continue;
            }

            nbad = 0;
            for (ipix = 0, iv = 1; iv <= ny; iv++) {
                for (iu = 1; iu <= nx; iu++, ipix++) {
                    double value;
                    double confidence;

                    value = cpl_image_get_interpolated(copy, piu[ipix], piv[ipix],
                                                       xyprofile, radius,
                                                       xyprofile, radius,
                                                       &confidence);
                    if (confidence < 0) {
                        cpl_mask_delete(rejected);
                        didfail = cpl_error_set(cpl_func,
                                                CPL_ERROR_ILLEGAL_INPUT);
                        irplib_error_recover(cleanstate, "Failure for image %d",
                                             iimage+1);
                        continue;
                    }

                    if (confidence < 0.5) {
                        nbad++;
                        if (rejected == NULL) {
                            rejected = cpl_mask_new(nx, ny);
                            preject = cpl_mask_get_data(rejected);
                        }
                        preject[ipix] = CPL_BINARY_1;
                    } else {
                        /* The equivalent of at least 2 of the
                           nearest 4 pixels are good */
                        px[ipix] = value;
                    }
                }
            }

            cpl_image_delete(copy);
            copy = NULL;
        }

        if (nbad > 0) {
            if (do_warp) {
                const cpl_error_code error
                    = cpl_image_reject_from_mask(image, rejected);

                cpl_mask_delete(rejected);
                rejected = NULL;
                preject = NULL;

                if (error) {
                    didfail = cpl_error_set(cpl_func, cpl_error_get_code());
                    irplib_error_recover(cleanstate, "Failure for image %d",
                                         iimage+1);
                    continue;
                }
            }
            if (iimage == 0)
                cpl_msg_info(cpl_func, "Cleaning %d bad pixels in "
                             "each of the %d images", nbad, nimages);
            if (cpl_detector_interpolate_rejected(image)) {
                didfail = cpl_error_set(cpl_func, cpl_error_get_code());
                irplib_error_recover(cleanstate, "Failure for image %d",
                                     iimage+1);
                continue;
            }

        }

        fluxpost = cpl_image_get_flux(image);

        if (cpl_error_get_code()) {
            didfail = cpl_error_set(cpl_func, cpl_error_get_code());
            irplib_error_recover(cleanstate, "Failure for image %d", iimage+1);
            continue;
        }

        cpl_msg_info(cpl_func, "Corrected distortion in image %d, flux: %g => %g",
                     1+iimage, fluxprev, fluxpost);

    }
    skip_if(didfail);

    end_skip;

    if (cpl_error_get_code())
        cpl_msg_error(cpl_func, "Detector unwarp failed (at %s): %s",
                      cpl_error_get_where(), cpl_error_get_message());

    cpl_vector_delete(xyprofile);
    cpl_image_delete(copy);
    cpl_image_delete(coord_x);
    cpl_image_delete(coord_y);

    return cpl_error_get_code();
}

/**@}*/

/*----------------------------------------------------------------------------*/
/**
  @brief    The warping of the spectral VISIR detector
  @param    inv    Non-zero iff the inverse is to be computed
  @param    nx     The detector size in the field-direction [pixel]
  @param    ny     The detector size in the wavelength-direction [pixel]
  @param    x      The unwarped field-coordinate [pixel] (1..npix)
  @param    y      The unwarped wavelength-coordinate [pixel] (1..npix)
  @param    phi    The skew of slit [radian] (positive counter-clockwise)
  @param    ksi    The skew of spectrum [radian]  (positive counter-clockwise)
  @param    eps    The horizontal curvature [pixel] (positive downwards)
  @param    delta  The vertical curvature [pixel] (positive towards right)
  @param    pu     The warped field-coordinate [pixel]
  @param    pv     The warped wavelength-coordinate [pixel]
  @return   CPL_ERROR_NONE, or the relevant CPL_ERROR.

  In case of the combined image, nx may be different from the detector size.

  The warping has four elements:
  1) A skew of the horizontal lines (phi)
  2) A skew of the vertical lines (ksi)
  3) Curvature of the horizontal lines (eps)
  4) Curvature of the vertical lines (delta)

  The curvature is assumed to be a circle segment.
  The detector center is chosen as fix-point for the transformation.

  Possible #_cpl_error_code_ set in this function:
  - CPL_ERROR_ILLEGAL_INPUT
  - CPL_ERROR_NULL_INPUT

 */
/*----------------------------------------------------------------------------*/
static cpl_error_code visir_spc_det_warp_xy(int inv, double nx, double ny,
                                     double x, double y,
                                     double phi, double ksi,
                                     double eps, double delta,
                                     double *pu, double *pv)
{


#ifdef VISIR_DET_WARP_ROTATE
    double z;
    const double skew = phi + ksi;
#endif

    cpl_ensure_code(pu,       CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(pv,       CPL_ERROR_NULL_INPUT);

    cpl_ensure_code(nx > 0, CPL_ERROR_ILLEGAL_INPUT);
    cpl_ensure_code(ny > 0, CPL_ERROR_ILLEGAL_INPUT);


    /* Transform Origo to center of detector */
    x -= (nx+1)/2;
    y -= (ny+1)/2;

    if (inv) {

        if (delta != 0) {
            /* Correct vertical curvature */
            const double R = ((ny/2)*(ny/2) + delta*delta) / fabs(2 * delta);

            cpl_ensure_code(R >= y, CPL_ERROR_ILLEGAL_INPUT);

            x -= (R - sqrt(R*R - y * y)) * (delta > 0 ? 1 : -1);
        }

        if (eps != 0) {
            /* Correct horizontal curvature */
            const double R = ((ny/2)*(ny/2) + eps*eps) / fabs(2 * eps);

            cpl_ensure_code(R >= x, CPL_ERROR_ILLEGAL_INPUT);

            y += (R - sqrt(R*R - x * x)) * (eps > 0 ? 1 : -1);
        }

        /* Skew into horizontal lines */
        y -= x * tan(phi);

        /* Skew into vertical lines */
        x -= y * tan(ksi);

    } else {

        /* Skew into vertical lines */
        x += y * tan(ksi);

        /* Skew into horizontal lines */
        y += x * tan(phi);

        if (eps != 0) {
            /* Correct horizontal curvature */
            const double R = ((ny/2)*(ny/2) + eps*eps) / fabs(2 * eps);

            cpl_ensure_code(R >= x, CPL_ERROR_ILLEGAL_INPUT);

            y -= (R - sqrt(R*R - x * x)) * (eps > 0 ? 1 : -1);
        }
        if (delta != 0) {
            /* Correct vertical curvature */
            const double R = ((ny/2)*(ny/2) + delta*delta) / fabs(2 * delta);

            cpl_ensure_code(R >= y, CPL_ERROR_ILLEGAL_INPUT);

            x += (R - sqrt(R*R - y * y)) * (delta > 0 ? 1 : -1);
        }
    }

    /* Transform Origo back from center of detector */
    x += (nx+1)/2;
    y += (ny+1)/2;

    *pu = x;
    *pv = y;

    return cpl_error_get_code();

}
