/*
 * This file is part of the ESO SINFONI Pipeline
 * Copyright (C) 2004,2005 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, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */
/*****************************************************************************
 * M.P.E. - SPIFFI project
 *
 *
 *
 * who       when      what

 * --------  --------  ----------------------------------------------
 * rabuter 2004-11-04 Fixed error on Find Cosmic when no valid neighboors
 where found in the subcube
 * rabuter 10/07/03  created
 */

/************************************************************************
 *   NAME
 *        sinfo_new_bezier.c -
 *        procedures to correct for bad pixels using bezier splines
 *
 *   SYNOPSIS
 *
 *   DESCRIPTION
 *
 *   FILES
 *
 *   ENVIRONMENT
 *
 *   RETURN VALUES
 *
 *   CAUTIONS
 *
 *   EXAMPLES
 *
 *   SEE ALSO
 *
 *   BUGS
 *
 *------------------------------------------------------------------------
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
#define POSIX_SOURCE 1
#include "sinfo_vltPort.h"

/*
 * System Headers
 */

/*
 * Local Headers
 */

#include "sinfo_new_bezier.h"
#include "sinfo_msg.h"
/**@{*/
/**
 * @addtogroup sinfo_new_bezier Bezier Functions
 *
 * TBD
 */

/*----------------------------------------------------------------------------
 *                            Function codes
 *--------------------------------------------------------------------------*/

/**
 @name sinfo_im_xy
 @brief computes  (x+size_x*y)
 @param im input image
 @param X  point X
 @param Y  point Y
 @return  (x+size_x*y)
 */
int
sinfo_im_xy(cpl_image* im, int X, int Y)
{
    int res = 0;

    res = X + Y * cpl_image_get_size_x(im);
    return res;
}

/**
 @brief computes (x+size_x*y)
 @param cu input imagelist
 @param X  point X
 @param Y  point Y
 @return  (x+size_x*y)
 */

int
sinfo_cu_xy(cpl_imagelist* cu, int X, int Y)
{
    int res = 0;

    res = X + Y * cpl_image_get_size_x(cpl_imagelist_get(cu, 0));
    return res;
}

/**
 @name sinfo_new_c_bezier_interpolate_image(
 @brief interpolates an image with a bezier function (TBV)
 @param im         input image
 @param mask       input bad pixel map
 @param look       input look-up table
 @param rx         input radius on X
 @param ry         input radius on Y
 @param rz         input radius on Z
 @param max_rad    max radius
 @param slit_edges slitlet edges

 */

cpl_image *
sinfo_new_c_bezier_interpolate_image(cpl_image *im, cpl_image *mask,
                                     new_Lookup *look, short rx, short ry,
                                     short rz, int max_rad, float ** slit_edges)
{

    int i, j, count;
    cpl_imagelist * sc_im, *drs_sc_mask;
    cpl_image /**auxMask,*/*auxImage;
    cpl_image *tempMask;
    short szx, szy, szz;
    short rx_loop, ry_loop, rz_loop;
    /*float ant,new,dif;*/

    int ilx = 0;
    int ily = 0;
    int mlx = 0;
    int mly = 0;

    float* pidata = NULL;
    float* pmdata = NULL;
    float* ptdata = NULL;
    float* padata = NULL;


    mlx = cpl_image_get_size_x(mask);
    mly = cpl_image_get_size_y(mask);
    ilx = cpl_image_get_size_x(im);
    ily = cpl_image_get_size_y(im);

    pmdata = cpl_image_get_data_float(mask);
    pidata = cpl_image_get_data_float(im);

    if (mlx != ilx || mly != ily) {
        sinfo_msg_error(" data & mask images not compatible in size\n");
        return NULL ;
    }

    /* allocate memory for sub cubes*/
    szx = (rx * 2) + 1;
    szy = (ry * 2) + 1;
    szz = (rz * 2) + 1;

    if (NULL == (sc_im = cpl_imagelist_new())) {
        sinfo_msg_error(" could not allocate memory for data subcube\n");
        return NULL ;
    }

    for (i = 0; i < szz; i++) {
        cpl_image* sc_img = cpl_image_new(szx, szy, CPL_TYPE_FLOAT);
        cpl_imagelist_set(sc_im, sc_img, i);
    }

    if (NULL == (drs_sc_mask = cpl_imagelist_new())) {
        sinfo_msg_error(" could not allocate memory for mask subcube\n");
        return NULL ;
    }
    for (i = 0; i < szz; i++) {
        cpl_image* drs_img = cpl_image_new(szx, szy, CPL_TYPE_FLOAT);
        cpl_imagelist_set(drs_sc_mask, drs_img, i);
    }

    if (NULL == (tempMask = cpl_image_new(mlx, mly, CPL_TYPE_FLOAT))) {
        sinfo_msg_error("could not allocate memory for temporary "
                        "dead pixel mask\n");
        return NULL ;
    }
    ptdata = cpl_image_get_data_float(tempMask);

    count = 0;
    for (i = 0; i < mlx; i++) {
        for (j = 0; j < mly; j++) {
            if (pmdata[sinfo_im_xy(im, i, j)] == cubePT_BADPIXEL) {
                rx_loop = 1;
                ry_loop = 1;
                rz_loop = 1;
                pidata[sinfo_im_xy(im, i, j)] =
                                sinfo_new_c_bezier_correct_pixel(i, j, im, mask,
                                                sc_im, drs_sc_mask, look,
                                                rx_loop, ry_loop, rz_loop);
                /* if not enough neighbors found, increase size of sub
                 cube until max radius is reached */
                while (pidata[sinfo_im_xy(im, i, j)] == cubeNONEIGHBOR
                                && rx_loop < rx && ry_loop < ry && rz_loop < rz) {
                    rx_loop++;
                    ry_loop++;
                    rz_loop++;
                    /* sinfo_msg_warning("Increasing radius to %d, in %d %d",
                     rx_loop, i, j) ; */

                    pidata[sinfo_im_xy(im, i, j)] =
                                    sinfo_new_c_bezier_correct_pixel(i, j, im,
                                                    mask, sc_im, drs_sc_mask,
                                                    look, rx_loop, ry_loop,
                                                    rz_loop);
                }
                /* If still not enough neighbors, make result NaN = ZERO
                 in spred convention */
                if (pidata[sinfo_im_xy(im, i, j)] == cubeNONEIGHBOR) {
                    pidata[sinfo_im_xy(im, i, j)] = ZERO;
                }
                count++;
            }
            if (pidata[sinfo_im_xy(im, i, j)] == ZERO) {
                ptdata[sinfo_im_xy(tempMask, i, j)] = 0;
            }
            else {
                ptdata[sinfo_im_xy(tempMask, i, j)] = 1;
            }
        }
    }

    sinfo_msg("Replacing NaN\n");
    auxImage = sinfo_interpol_source_image(im, tempMask, max_rad, slit_edges);
    padata = cpl_image_get_data_float(auxImage);
    for (i = 0; i < mlx; i++) {
        for (j = 0; j < mly; j++) {

            if (isnan(pidata[sinfo_im_xy(im,i,j)])) /*<= -2e10ZERO )*/
            {
                /* sinfo_msg_warning("Replacing NaN -> %d %d %f\n",
                 i,j, padata[sinfo_im_xy(im,i,j)] ); */
                pidata[sinfo_im_xy(im, i, j)] = padata[sinfo_im_xy(im, i, j)];
            }
        }
    }
    cpl_image_delete(auxImage);
    cpl_imagelist_delete(sc_im);
    cpl_imagelist_delete(drs_sc_mask);

    sinfo_msg("bad pixels count: %d\n", count);

    return im;
}

cpl_image *
sinfo_new_c_bezier_find_bad(cpl_image *im, cpl_image *mask,
/* Lookup *look,*/
short rx,
                            short ry, short rz, short lowerI, short highI,
                            short lowerJ, short highJ, float factor)
{

    int i, j, count;
    cpl_imagelist * sc_im, *drs_sc_mask;
    short szx, szy, szz;
    float /*ant,*/newValue, old/*,dif,porcentage,distance*/;
    double med, stdev;
    /*cpl_image *out;*/
    short rx_loop, ry_loop, rz_loop;

    int ilx = 0;
    int ily = 0;
    int mlx = 0;
    int mly = 0;

    float* pidata = NULL;
    //float* pmdata=NULL;


    mlx = cpl_image_get_size_x(mask);
    mly = cpl_image_get_size_y(mask);
    ilx = cpl_image_get_size_x(im);
    ily = cpl_image_get_size_y(im);

    //pmdata=cpl_image_get_data_float(mask);
    pidata = cpl_image_get_data_float(im);

    if (mlx != ilx || mly != ily) {
        sinfo_msg_error(" data & mask images not compatible in size\n");
        return NULL ;
    }

    /* allocate memory for sub cubes*/
    szx = (rx * 2) + 1;
    szy = (ry * 2) + 1;
    szz = (rz * 2) + 1;

    if (NULL == (sc_im = cpl_imagelist_new())) {
        sinfo_msg_error(" could not allocate memory for data subcube\n");
        return NULL ;
    }
    for (i = 0; i < szz; i++) {
        cpl_image* sc_img = cpl_image_new(szx, szy, CPL_TYPE_FLOAT);
        cpl_imagelist_set(sc_im, sc_img, i);
    }

    if (NULL == (drs_sc_mask = cpl_imagelist_new())) {
        sinfo_msg_error(" could not allocate memory for mask subcube\n");
        return NULL ;
    }
    for (i = 0; i < szz; i++) {
        cpl_image* drs_img = cpl_image_new(szx, szy, CPL_TYPE_FLOAT);
        cpl_imagelist_set(drs_sc_mask, drs_img, i);
    }

    count = 0;
    for (i = 0; i < mlx; i++) {
        for (j = 0; j < mly; j++) {
            if (i >= lowerI && i < highI && j >= lowerJ && j < highJ) {

                rx_loop = 1;
                ry_loop = 1;
                rz_loop = 1;
                newValue = sinfo_new_c_bezier_correct_pixel_2D(i, j, im, mask,
                                sc_im, drs_sc_mask,
                                /* look,*/
                                rx_loop, ry_loop, rz_loop, &med, &stdev,
                                factor);
                /* if NaN returned, increase size of sub cube
                 until max radius is reached */
                while (newValue == ZERO && rx_loop < rx && ry_loop < ry
                                && rz_loop < rz) {
                    rx_loop++;
                    ry_loop++;
                    rz_loop++;
                    /*sinfo_msg_warning("Increasing radius to %d,
                     in %d %d", rx_loop, i, j) ;  */
                    newValue = sinfo_new_c_bezier_correct_pixel_2D(i, j, im,
                                    mask, sc_im, drs_sc_mask,
                                    /*, look*/
                                    rx_loop, ry_loop, rz_loop, &med, &stdev,
                                    factor);
                }
                if (isnan(newValue)) /*<= -3.e10 ZERO )*/
                    continue;

                old = pidata[sinfo_im_xy(im, i, j)];
                if (newValue != old) {
                    pidata[sinfo_im_xy(im, i, j)] = newValue;
                    /*sinfo_msg_warning("[%d,%d]=%f -> %f, med= %f,
                     stdev=%f\n",i,j, old, newValue, med, stdev );*/
                    count++;
                }
            }
        }
    }

    sinfo_msg("bad pixels count: %d\n", count);

    cpl_imagelist_delete(sc_im);
    cpl_imagelist_delete(drs_sc_mask);
    return im;
}

float
sinfo_new_c_bezier_correct_pixel(int ipos, int jpos, cpl_image * im,
                                 cpl_image * mask, cpl_imagelist * sc_im,
                                 cpl_imagelist * drs_sc_mask, new_Lookup * look,
                                 short rx, short ry, short rz)
{
    short ic, jc, kc, ii, jj, kk/*, sjj, skk*/, is, js, ks;
    short i, j, k, indexJ, indexI, lx, ly, lz, szx, szy, szz;
    /*float indexIf,indexJf,sp;*/
    cpl_image * X, *Y, *Z, *hX;
    cpl_imagelist * id, *jd;

    int idlx = 0;
    int idly = 0;
    int idnp = 0;

    int drslx = 0;
    int drsly = 0;
    int drsnp = 0;

    float* pXdata = NULL;
    float* pYdata = NULL;
    float* pZdata = NULL;
    float* phXdata = NULL;
    float* pidata = NULL;
    float* pmdata = NULL;
    float* piddata = NULL;
    float* pjddata = NULL;
    float* pscdata = NULL;
    float* pdrsdata = NULL;

    cpl_image* id_img = NULL;
    cpl_image* jd_img = NULL;
    cpl_image* sc_img = NULL;
    cpl_image* drs_img = NULL;

    X = look->X;
    Y = look->Y;
    Z = look->Z;
    hX = look->hX;
    id = look->id;
    jd = look->jd;

    /*
     phXdata=cpl_image_get_data_float(hX);
     if ( phXdata[sinfo_im_xy( hX, ipos, jpos)] > 1 )
     {
     sinfo_msg_error(" double hit in position [%d,%d]=%f, "
     "can not correct\n",
     ipos,jpos,phXdata[sinfo_im_xy(hX,ipos,jpos)]) ;
     return ( -2e10 );
     }*/

    pidata = cpl_image_get_data_float(im);
    pmdata = cpl_image_get_data_float(mask);

    phXdata = cpl_image_get_data_float(hX);
    if (phXdata[sinfo_im_xy(hX, ipos, jpos)] < 1) {
        /*sinfo_msg_error("no lookup in position [%d,%d]=%f, can not correct",
         ipos,jpos,phXdata[sinfo_im_xy(hX,ipos,jpos)]) ;*/
        return ( ZERO);
    }
    pXdata = cpl_image_get_data_float(X);
    pYdata = cpl_image_get_data_float(Y);
    pZdata = cpl_image_get_data_float(Z);

    ic = pXdata[sinfo_im_xy(X, ipos, jpos)];
    jc = pYdata[sinfo_im_xy(Y, ipos, jpos)];
    kc = pZdata[sinfo_im_xy(Z, ipos, jpos)];
    /*if ( !(ipos % 16 )  )*/
#ifdef DEBUG
    sinfo_msg_debug("Correcting bad pixel : ipos=%d,jpos=%d, "
                    "in Cube -> ic=%d, jc=%d, kc=%d\n",
                    ipos,jpos, ic, jc, kc );
#endif
    /*limit to start not before the beginning of the cube*/
    ii = ic - rx;
    if (ii < 0)
        ii = 0;
    jj = jc - ry;
    if (jj < 0)
        jj = 0;
    kk = kc - rz;
    if (kk < 0)
        kk = 0;

#ifdef DEBUG
    sinfo_msg_debug("Start Point in Cube -> ii=%d,jj=%d,kk=%d\n", ii, jj, kk );
#endif

    /*limit to end not outside of the cube */
    szx = (rx * 2) + 1;
    szy = (ry * 2) + 1;
    szz = (rz * 2) + 1;

    idlx = cpl_image_get_size_x(cpl_imagelist_get(id, 0));
    idly = cpl_image_get_size_y(cpl_imagelist_get(id, 0));
    idnp = cpl_imagelist_get_size(id);

    lx = idlx;
    ly = idly;
    lz = idnp;

    if ((ic + rx) >= idlx)
        szx = szx - ((ic + rx) - (lx - 1));

    if ((jc + ry) >= idly)
        szy = szy - ((jc + ry) - (ly - 1));

    if ((kc + rz) >= idnp)
        szz = szz - ((kc + rz) - (lz - 1));

    drslx = cpl_image_get_size_x(cpl_imagelist_get(drs_sc_mask, 0));
    drsly = cpl_image_get_size_y(cpl_imagelist_get(drs_sc_mask, 0));
    drsnp = cpl_imagelist_get_size(drs_sc_mask);
#ifdef DEBUG

    sinfo_msg_error("Size of subcube: szx=%d,szy=%d,szz=%d\n", szx, szy, szz );
    /*fill whole mask with not available*/
    sinfo_msg_error("Fill Mask subcube of size: %d,%d,%d, with NOINFO\n",
                    drslx, drsly, drsnp);
#endif
    for (i = 0; i < drslx; i++) {
        for (j = 0; j < drsly; j++) {
            for (k = 0; k < drsnp; k++) {
                drs_img = cpl_imagelist_get(drs_sc_mask, k);
                pdrsdata = cpl_image_get_data_float(drs_img);
                pdrsdata[sinfo_cu_xy(drs_sc_mask, i, j)] = cubePT_NOINFO;
            }
        }
    }

    for (i = ii, is = 0; i < ii + szx; i++, is++) {
        for (j = jj, js = 0; j < jj + szy; j++, js++) {
            for (k = kk, ks = 0; k < kk + szz; k++, ks++) {
#ifdef DEBUG
                sinfo_msg_debug("i=%d j=%d k=%d is=%d ij=%d ik=%d",
                                i,j,k,is,js,ks);
#endif
                id_img = cpl_imagelist_get(id, k);
                jd_img = cpl_imagelist_get(jd, k);
                piddata = cpl_image_get_data_float(id_img);
                pjddata = cpl_image_get_data_float(jd_img);

                drs_img = cpl_imagelist_get(drs_sc_mask, ks);
                pdrsdata = cpl_image_get_data_float(drs_img);
                sc_img = cpl_imagelist_get(sc_im, ks);
                pscdata = cpl_image_get_data_float(sc_img);

                indexI = sinfo_new_nint(piddata[sinfo_cu_xy(id, i, j)]);
                indexJ = sinfo_new_nint(pjddata[sinfo_cu_xy(jd, i, j)]);
                if (indexJ <= -1 || indexJ >= 2048 || indexI == -1) {
                    pdrsdata[sinfo_cu_xy(drs_sc_mask, is, js)] = cubePT_NOINFO;
                    continue;
                }
                pscdata[sinfo_cu_xy(sc_im, is, js)] = pidata[sinfo_im_xy(im,
                                indexI, indexJ)];
                pdrsdata[sinfo_cu_xy(drs_sc_mask, is, js)] = pmdata[sinfo_im_xy(
                                mask, indexI, indexJ)];
                /*
                 #ifdef DEBUG
                 sinfo_msg_debug("Cube i=%d, j=%d, k=%d  ;"
                 "  Sub is=%d, js=%d, ks=%d  ;"
                 "  Plane I=%d,J=%d ; mask %f ; im %f",
                 i, j, k, is, js, ks, indexI, indexJ,
                 pmdata[sinfo_im_xy(mask,indexI,indexJ)],
                 pidata[sinfo_im_xy(im,indexI,indexJ)]);
                 #endif
                 */
            }
        }
    }

    /*signal to correct this pixel*/
    drs_img = cpl_imagelist_get(drs_sc_mask, rz);
    pdrsdata = cpl_image_get_data_float(drs_img);
    pdrsdata[sinfo_cu_xy(drs_sc_mask, rx, ry)] = cubePT_FIND;
    return (sinfo_new_c_bezier_interpol(sc_im, drs_sc_mask));
}

float
sinfo_new_c_bezier_correct_pixel_2D(int ipos, int jpos, cpl_image * im,
                                    cpl_image * mask, cpl_imagelist * sc_im,
                                    cpl_imagelist * drs_sc_mask,
                                    /* Lookup * look,*/
                                    short rx,
                                    short ry, short rz, double *med,
                                    double *stdev, float factor)
{
    short ic, jc, kc, ii, jj, kk/*, sjj, skk*/, is, js, ks;
    short i, j, k, indexJ, indexI, lx, ly, lz, szx, szy, szz;
    double sum;
    int counter;
    float sumarr[100];

    int ilx = 0;
    int ily = 0;

    int drslx = 0;
    int drsly = 0;
    int drsnp = 0;

    float* pidata = 0;
    float* pmdata = 0;
    float* pscdata = 0;
    float* pdrsdata = 0;

    cpl_image* drs_img = NULL;
    cpl_image* sc_img = NULL;

    jc = 0;
    ic = ipos;
    kc = jpos;
    sinfo_msg_debug("Correcting bad pixel : ipos=%d,jpos=%d, "
                    "in Cube -> ic=%d, jc=%d, kc=%d", ipos, jpos, ic, jc, kc);
    /*limit to start not before the beginning of the cube*/
    ii = ic - rx;
    if (ii < 0)
        ii = 0;
    jj = jc - ry;
    if (jj < 0)
        jj = 0;
    kk = kc - rz;
    if (kk < 0)
        kk = 0;

    sinfo_msg_debug("Start Point in Cube -> ii=%d,jj=%d,kk=%d", ii, jj, kk);

    ilx = cpl_image_get_size_x(im);
    ily = cpl_image_get_size_y(im);

    /*limit to end not outside of the cube */
    szx = (rx * 2) + 1;
    szy = (ry * 2) + 1;
    szz = (rz * 2) + 1;
    lx = ilx;
    ly = ily;
    lz = ily;
    if ((ic + rx) >= ilx)
        szx = szx - ((ic + rx) - (lx - 1));

    if ((jc + ry) >= ily)
        szy = szy - ((jc + ry) - (ly - 1));

    if ((kc + rz) >= ily)
        szz = szz - ((kc + rz) - (lz - 1));

#ifdef DEBUG
    drslx=cpl_image_get_size_x(cpl_imagelist_get(drs_sc_mask,0));
    drsly=cpl_image_get_size_y(cpl_imagelist_get(drs_sc_mask,0));
    drsnp=cpl_imagelist_get_size(drs_sc_mask);
    sinfo_msg_debug("Size of subcube : szx=%d,szy=%d,szz=%d", szx, szy, szz );
    /*fill whole mask with not available*/
    sinfo_msg_debug("Fill Mask subcube of size:%d,%d,%d, with NOINFO",
                    drslx, drsly, drsnp);
#endif
    for (i = 0; i < drslx; i++) {
        for (j = 0; j < drsly; j++) {
            for (k = 0; k < drsnp; k++) {
                drs_img = cpl_imagelist_get(drs_sc_mask, k);
                pdrsdata = cpl_image_get_data_float(drs_img);
                pdrsdata[sinfo_cu_xy(drs_sc_mask, i, j)] = cubePT_NOINFO;
            }
        }
    }
    counter = 0;
    sum = 0;
    memset(sumarr, 0x00, sizeof(sumarr));
    pidata = cpl_image_get_data(im);
    pmdata = cpl_image_get_data(mask);

    for (i = ii, is = 0; i < ii + szx; i++, is++) {
        for (j = jj, js = 0; j < jj + szy; j++, js++) {
            for (k = kk, ks = 0; k < kk + szz; k++, ks++) {
#ifdef DEBUG
                sinfo_msg_debug("i=%d j=%d k=%d is=%d ij=%d ik=%d",
                                i,j,k,is,js,ks);
#endif
                indexI = i;
                indexJ = k;
                if (isnan(pidata[sinfo_im_xy(mask,indexI,indexJ)]))
                    pmdata[sinfo_im_xy(mask, indexI, indexJ)] = 0;

                if (pmdata[sinfo_im_xy(mask, indexI, indexJ)] == 1
                                && (indexI != ipos || indexJ != jpos)) {
                    /*sumarr[counter] = pidata[sinfo_im_xy(im,indexI,indexJ)];*/
                    sum = sum + pidata[sinfo_im_xy(im, indexI, indexJ)];
                    counter++;
                }
                sc_img = cpl_imagelist_get(sc_im, ks);
                pscdata = cpl_image_get_data_float(sc_img);
                pscdata[sinfo_cu_xy(sc_im, is, js)] = pidata[sinfo_im_xy(im,
                                indexI, indexJ)];
                drs_img = cpl_imagelist_get(drs_sc_mask, ks);
                pdrsdata = cpl_image_get_data_float(drs_img);
                pdrsdata[sinfo_cu_xy(drs_sc_mask, is, js)] = pmdata[sinfo_im_xy(
                                mask, indexI, indexJ)];
#ifdef DEBUG
                sinfo_msg_debug("Cube i=%d, j=%d, k=%d  ;  "
                                "Sub is=%d, js=%d, ks=%d  ; "
                                " Plane I=%d,J=%d ; mask %f ; im %f",
                                i, j, k, is, js, ks, indexI, indexJ,
                                pmdata[sinfo_im_xy(mask,indexI,indexJ)],
                                pidata[sinfo_im_xy(im,indexI,indexJ)]);
#endif

            }
        }
    }

    /*signal to correct this pixel*/
    drs_img = cpl_imagelist_get(drs_sc_mask, rz);
    pdrsdata = cpl_image_get_data_float(drs_img);
    pdrsdata[sinfo_cu_xy(drs_sc_mask, rx, ry)] = cubePT_FIND;
    if (counter)
        /**med = sinfo_median(sumarr,counter);*/
        *med = sum / counter;
    else
        return (pidata[sinfo_im_xy(im, ipos, jpos)]);

    /*sinfo_msg_debug("%f %f %d\n",
     sum ,pidata[sinfo_im_xy(im,ipos,jpos)], counter);*/

    sum = 0;
    counter = 0;
    for (i = ii, is = 0; i < ii + szx; i++, is++) {
        for (j = jj, js = 0; j < jj + szy; j++, js++) {
            for (k = kk, ks = 0; k < kk + szz; k++, ks++) {
                drs_img = cpl_imagelist_get(drs_sc_mask, ks);
                pdrsdata = cpl_image_get_data_float(drs_img);
                indexI = i;
                indexJ = k;
                if (pdrsdata[sinfo_cu_xy(drs_sc_mask, is, js)] == 1
                                && (indexI != ipos || indexJ != jpos)) {
                    sc_img = cpl_imagelist_get(sc_im, ks);
                    pscdata = cpl_image_get_data_float(sc_img);

                    sum =
                                    sum
                                                    + ((pscdata[sinfo_cu_xy(
                                                                    drs_sc_mask,
                                                                    is, js)]
                                                                    - *med)
                                                                    * (pscdata[sinfo_cu_xy(
                                                                                    drs_sc_mask,
                                                                                    is,
                                                                                    js)]
                                                                                    - *med));
                    counter++;
                }
            }
        }
    }
    /*
     double aux;
     aux = sum;
     */
    sum = sum / (counter - 1);
    *stdev = sqrt(sum);

    if ((fabs(pidata[sinfo_im_xy(im, ipos, jpos)] - *med) > factor * *stdev)
                    || isnan(pidata[sinfo_im_xy(im,ipos,jpos)])) {
        /*sinfo_msg_debug("[%d,%d]: distance to mean = %f,"
         " thres =%f sum=%f, stdev=%f, counter=%d, aux= %f",
         ipos,jpos, fabs( pidata[sinfo_im_xy(im,ipos,jpos)] - *med),
         factor * *stdev, sum,*stdev, counter,aux );
         pmdata[sinfo_im_xy(mask,ipos,jpos)] = 0;*/
        return (sinfo_new_c_bezier_interpol(sc_im, drs_sc_mask));
    }
    return (pidata[sinfo_im_xy(im, ipos, jpos)]);
}

float
sinfo_new_c_bezier_interpol(cpl_imagelist * im, cpl_imagelist * action)
{
    short pos;
    unsigned short i, j, k;
    new_XYZW indata[1000];
    new_XYZW res;
    new_XYZW selected;
    float step, cumstep, selected_distance;
    new_Dim *point;
    double munk;
    int ilx = 0;
    int ily = 0;
    int inp = 0;
    float* padata = NULL;
    float* pidata = NULL;
    cpl_image* i_img = NULL;
    cpl_image* a_img = NULL;

    selected.w = 0;
    memset(indata, 0x00, 1000 * sizeof(new_XYZW));
    ilx = cpl_image_get_size_x(cpl_imagelist_get(im, 0));
    ily = cpl_image_get_size_y(cpl_imagelist_get(im, 0));
    inp = cpl_imagelist_get_size(im);

    pos = 0;
    for (i = 0; i < ilx; i++) {
        for (j = 0; j < ily; j++) {
#ifdef DEBUG
            int pix=0;
            pix=i+j*ilx;
#endif
            for (k = 0; k < inp; k++) {
                a_img = cpl_imagelist_get(action, k);
                padata = cpl_image_get_data_float(a_img);
                i_img = cpl_imagelist_get(action, k);
                pidata = cpl_image_get_data_float(i_img);
                if (padata[sinfo_cu_xy(action, i, j)] == cubePT_USE) {
#ifdef DEBUG
                    sinfo_msg_debug("Used im[%d,%d,%d]=%lf\n",
                                    i,j,k,pidata[pix]);
#endif
                    indata[pos].x = i;
                    indata[pos].y = j;
                    indata[pos].z = k;
                    indata[pos].w = pidata[sinfo_cu_xy(im, i, j)];
                    pos++;
                }
                else {
                    if (padata[sinfo_cu_xy(action, i, j)] == cubePT_FIND) {
                        (*point).x = i;
                        (*point).y = j;
                        (*point).z = k;
#ifdef DEBUG
                        sinfo_msg_debug("Find for im[%d,%d,%d]=%lf reason:%f",
                                        i,j,k,pidata[pix],
                                        padata[sinfo_cu_xy(action,i,j)]);
#endif
                    }
                    else {
#ifdef DEBUG
                        sinfo_msg_debug("Ignored im[%d,%d,%d]=%lf reason:%f",
                                        i,j,k,pidata[sinfo_im_xy(im,i,j)],
                                        padata[pix]);
#endif
                    }
                }
            }
        }
    }

    /**/
    if (pos < 2) {
#ifdef DEBUG
        sinfo_msg_debug("subcube contains no valid pixels "
                        "to use in iterpolation");
#endif
        /*i_img=cpl_imagelist_get((*point).z);
         pidata=cpl_image_get_data_float(i_img);
         return( pidata[sinfo_im_xy(im,(*point).x,(*point).y)] );*/
        return (cubeNONEIGHBOR);

    }

    step = 0.01;
    cumstep = 0.0;
    selected_distance = 1000;
    munk = pow(1.0 - cumstep, (double) pos - 1);
    for (i = 0; (i < 100) && (munk != 0.0); i++) {
        memset(&res, 0x00, sizeof(new_XYZW));
        sinfo_new_bezier(indata, pos - 1, cumstep, munk, &res);
        float distance = sqrt(
                        pow(((*point).x - res.x), 2)
                                        + pow(((*point).y - res.y), 2)
                                        + pow(((*point).z - res.z), 2));
        /*sinfo_msg_debug("%lf %lf %lf %lf %lf\n",
         res.x,res.y,res.z,res.w,distance);*/
        if (distance < selected_distance) {
            selected_distance = distance;
            selected.x = res.x;
            selected.y = res.y;
            selected.z = res.z;
            selected.w = res.w;
        }
        cumstep = cumstep + step;
        munk = pow(1.0 - cumstep, (double) pos - 1);

    }

#ifdef DEBUG
    sinfo_msg_debug("Selected %lf %lf %lf %lf, distance=%lf",
                    selected.x,selected.y,selected.z,
                    selected.w,selected_distance);
#endif
    i_img = cpl_imagelist_get(im, (*point).z);
    pidata = cpl_image_get_data_float(i_img);
    pidata[sinfo_cu_xy(im, (*point).x, (*point).y)] = selected.w;

    return selected.w;
}

int
sinfo_new_bezier(new_XYZW *p, int n, double mu, double munk, new_XYZW *res)
{

    double muk = 1;
    for (int k = 0; k <= n; k++) {
        int nn = n;
        int kn = k;
        int nkn = n - k;
        double blend = muk * munk;
        muk *= mu;
        munk /= (1.0 - mu);
        while (nn >= 1) {
            blend *= (double) nn;
            nn--;
            if (kn > 1) {
                blend /= (double) kn;
                kn--;
            }
            if (nkn > 1) {
                blend /= (double) nkn;
                nkn--;
            }
        }
        res->x += p[k].x * blend;
        res->y += p[k].y * blend;
        res->z += p[k].z * blend;
        res->w += p[k].w * blend;
    }
    return (0);
}


/**
 @name sinfo_new_Lookup
 @brief computes a new look-up object
 @return pointer to new look-up object
 */
new_Lookup *
sinfo_new_lookup(void)
{
    new_Lookup *l;
    l = (new_Lookup*) cpl_calloc(1, sizeof(new_Lookup));
    return (l);
}
/**
 @name sinfo_new_destroy_lookup
 @brief frees a new Lookup object
 @param l pointer to look-up object
 @return void
 */
void
sinfo_new_destroy_lookup(new_Lookup *l)
{
    if (l)
        cpl_free(l);
}
/**
 @name sinfo_new_change_mask
 @brief frees a new Lookup object
 @param mask pointer to bad pixel map
 @param im pointer to image
 @return if succes 0, else -1
 */
int
sinfo_new_change_mask(cpl_image * mask, cpl_image * im)
{
    int i;
    //int mlx=0;
    //int mly=0;
    int ilx = 0;
    int ily = 0;
    float* pidata = NULL;
    float* pmdata = NULL;

    if (mask == NULL || im == NULL )
        return -1;
    ilx = cpl_image_get_size_x(im);
    ily = cpl_image_get_size_y(im);
    pidata = cpl_image_get_data_float(im);

    //mlx=cpl_image_get_size_x(mask);
    //mly=cpl_image_get_size_y(mask);
    pmdata = cpl_image_get_data_float(mask);

    for (i = 0; i < (int) ilx * ily; i++) {
        if (isnan(pidata[i])) {
            pmdata[i] = 0.;
        }
    }
    return 0;
}

/**
 @name sinfo_new_c_bezier_correct_cosmic
 @brief Correct cosmic rays in an image
 @param im pointer to image
 @param mask pointer to bad pixel map
 @param sc_im image list
 @param drs_sc_mask list of image maps
 @param look pointer to look-up table
 @param rx radius on X
 @param ry radius on Y
 @param rz radius on Z
 @param med  output median
 @param stdev output standard deviation
 @param factor  threshold factor

 @return if succes 0, else -1
 */
float
sinfo_new_c_bezier_correct_cosmic(int ipos, int jpos, cpl_image * im,
                                  cpl_image * mask, cpl_imagelist * sc_im,
                                  cpl_imagelist * drs_sc_mask,
                                  new_Lookup * look, short rx, short ry,
                                  short rz, double *med, double *stdev,
                                  float factor)
{
    short ic, jc, kc, ii, jj, kk/*, sjj, skk*/, is, js, ks;
    short i, j, k, indexJ, indexI, lx, ly, lz, szx, szy, szz;
    /*float indexIf,indexJf,sp;*/
    cpl_image * X, *Y, *Z, *hX;
    cpl_imagelist * id, *jd;
    short counter;
    double sum;
    float* phXdata = NULL;
    float* pXdata = NULL;
    float* pYdata = NULL;
    float* pZdata = NULL;

    float* pimdata = NULL;
    float* pscdata = NULL;
    float* pdrsdata = NULL;
    float* piddata = NULL;
    float* pjddata = NULL;
    float* pmaskdata = NULL;

    int idlx = 0;
    int idly = 0;
    int idnp = 0;

    int drslx = 0;
    int drsly = 0;
    int drsnp = 0;

    X = look->X;
    Y = look->Y;
    Z = look->Z;
    hX = look->hX;
    id = look->id;
    jd = look->jd;

    phXdata = cpl_image_get_data_float(hX);
    /*if ( phXdata[sinfo_im_xy( hX, ipos, jpos)] > 1 )
     {
     sinfo_msg_error("double hit in position [%d,%d]=%f, can not correct",
     ipos,jpos,phXdata[sinfo_im_xy(hX,ipos,jpos)]) ;
     return ( -2e10 );
     }*/
    if (phXdata[sinfo_im_xy(hX, ipos, jpos)] < 1) {
        /*sinfo_msg_error("no lookup  in position [%d,%d]=%f, can not correct",
         ipos,jpos,phXdata[sinfo_im_xy(hX,ipos,jpos)]) ;*/
        return ( ZERO);
    }

    pXdata = cpl_image_get_data_float(X);
    pYdata = cpl_image_get_data_float(Y);
    pZdata = cpl_image_get_data_float(Z);

    ic = pXdata[sinfo_im_xy(X, ipos, jpos)];
    jc = pYdata[sinfo_im_xy(Y, ipos, jpos)];
    kc = pZdata[sinfo_im_xy(Z, ipos, jpos)];
    /*if ( !(ipos % 16 )  )*/
#ifdef DEBUG
    sinfo_msg_debug("Correcting bad pixel : ipos=%d,jpos=%d, "
                    "in Cube -> ic=%d, jc=%d, kc=%d", ipos,jpos, ic, jc, kc );
#endif
    /*limit to start not before the beginning of the cube*/
    ii = ic - rx;
    if (ii < 0)
        ii = 0;
    jj = jc - ry;
    if (jj < 0)
        jj = 0;
    kk = kc - rz;
    if (kk < 0)
        kk = 0;

#ifdef DEBUG
    sinfo_msg_debug("Start Point in Cube -> ii=%d,jj=%d,kk=%d", ii, jj, kk );
#endif

    /*limit to end not outside of the cube */
    szx = (rx * 2) + 1;
    szy = (ry * 2) + 1;
    szz = (rz * 2) + 1;

    idlx = cpl_image_get_size_x(cpl_imagelist_get(id, 0));
    idly = cpl_image_get_size_y(cpl_imagelist_get(id, 0));
    idnp = cpl_imagelist_get_size(id);

    lx = idlx;
    ly = idly;
    lz = idnp;
    if ((ic + rx) >= idlx)
        szx = szx - ((ic + rx) - (lx - 1));

    if ((jc + ry) >= idly)
        szy = szy - ((jc + ry) - (ly - 1));

    if ((kc + rz) >= idnp)
        szz = szz - ((kc + rz) - (lz - 1));

#ifdef DEBUG
    sinfo_msg_error("Size of subcube : szx=%d,szy=%d,szz=%d\n", szx, szy, szz );
    /*fill whole mask with not available*/
    drsnp=cpl_imagelist_get_size(drs_sc_mask);
    drslx=cpl_image_get_size_x(cpl_imagelist_get(drs_sc_mask,0));
    drsly=cpl_image_get_size_y(cpl_imagelist_get(drs_sc_mask,0));

    sinfo_msg_error("Fill Mask subcube of size: %d,%d,%d, with NOINFO",
                    drslx, drsly, drsnp);
#endif
    for (i = 0; i < drslx; i++) {
        for (j = 0; j < drsly; j++) {
            for (k = 0; k < drsnp; k++) {
                pdrsdata = cpl_image_get_data_float(
                                cpl_imagelist_get(drs_sc_mask, k));
                pdrsdata[sinfo_cu_xy(drs_sc_mask, i, j)] = cubePT_NOINFO;
            }
        }
    }
    pimdata = cpl_image_get_data_float(im);
    pmaskdata = cpl_image_get_data_float(mask);
    for (i = ii, is = 0; i < ii + szx; i++, is++) {
        for (j = jj, js = 0; j < jj + szy; j++, js++) {
            for (k = kk, ks = 0; k < kk + szz; k++, ks++) {
#ifdef DEBUG
                sinfo_msg_debug("i=%d j=%d k=%d is=%d ij=%d ik=%d",
                                i,j,k,is,js,ks);
#endif
                piddata = cpl_image_get_data_float(cpl_imagelist_get(id, k));
                pjddata = cpl_image_get_data_float(cpl_imagelist_get(id, k));
                pdrsdata = cpl_image_get_data_float(
                                cpl_imagelist_get(drs_sc_mask, ks));
                pscdata = cpl_image_get_data_float(
                                cpl_imagelist_get(sc_im, ks));

                indexI = sinfo_new_nint(piddata[sinfo_cu_xy(id, i, j)]);
                indexJ = sinfo_new_nint(pjddata[sinfo_cu_xy(jd, i, j)]);
                if (indexJ <= -1 || indexJ >= 2048 || indexI == -1) {
                    pdrsdata[sinfo_cu_xy(drs_sc_mask, is, js)] = cubePT_NOINFO;
                    continue;
                }
                pscdata[sinfo_cu_xy(sc_im, is, js)] = pimdata[sinfo_im_xy(im,
                                indexI, indexJ)];
                pdrsdata[sinfo_cu_xy(drs_sc_mask, is, js)] =
                                pmaskdata[sinfo_im_xy(mask, indexI, indexJ)];
#ifdef DEBUG
                sinfo_msg_debug("Cube i=%d, j=%d, k=%d  ; "
                                " Sub is=%d, js=%d, ks=%d  ; "
                                " Plane I=%d,J=%d ; mask %f ; im %f\n",
                                i, j, k, is, js, ks, indexI, indexJ,
                                pmaskdata[sinfo_im_xy(mask,indexI,indexJ)],
                                pimdata[sinfo_im_xy(im,indexI,indexJ)]);
#endif

            }
        }
    }

    /* ignoring the elements in the slitlet of the tested pixel */

    for (i = 0; i < szx; i++) {
        for (k = 0; k < szz; k++) {
            pdrsdata = cpl_image_get_data_float(
                            cpl_imagelist_get(drs_sc_mask, k));
            pdrsdata[sinfo_cu_xy(drs_sc_mask, i, ry)] = cubePT_NOINFO;
        }
    }
    /* now calculate mean and stdev in subcube */

    counter = 0;
    sum = 0;
    for (i = 0; i < szx; i++) {
        for (j = 0; j < szy; j++) {
            for (k = 0; k < szz; k++) {
                pdrsdata = cpl_image_get_data_float(
                                cpl_imagelist_get(drs_sc_mask, k));
                pscdata = cpl_image_get_data_float(cpl_imagelist_get(sc_im, k));
                if (pscdata[sinfo_cu_xy(sc_im, i, j)] != ZERO
                                && pdrsdata[sinfo_cu_xy(drs_sc_mask, i, j)]
                                                != cubePT_NOINFO) {
                    sum = sum + pscdata[sinfo_cu_xy(sc_im, i, j)];
                    counter++;
                }
            }
        }
    }

    *med = sum / counter;

    counter = 0;
    sum = 0;
    for (i = 0; i < szx; i++) {
        for (j = 0; j < szy; j++) {
            for (k = 0; k < szz; k++) {
                pscdata = cpl_image_get_data_float(cpl_imagelist_get(sc_im, k));
                pdrsdata = cpl_image_get_data_float(
                                cpl_imagelist_get(drs_sc_mask, k));
                if (pscdata[sinfo_cu_xy(sc_im, i, j)] != ZERO
                                && pdrsdata[sinfo_cu_xy(drs_sc_mask, i, j)]
                                            != cubePT_NOINFO) {
                    sum = sum + (pscdata[sinfo_cu_xy(
                                    sc_im, i, j)]
                                         - *med)
                                                   * (pscdata[sinfo_cu_xy(
                                                                   sc_im,
                                                                   i,
                                                                   j)]
                                                              - *med);
                    counter++;
                }
            }
        }
    }

    *stdev = sqrt(sum / (counter - 1));

    if ((fabs(pimdata[sinfo_im_xy(im, ipos, jpos)] - *med) > factor * *stdev)
                    || isnan(pimdata[sinfo_im_xy(im,ipos,jpos)])) {
        pdrsdata = cpl_image_get_data_float(cpl_imagelist_get(drs_sc_mask, rz));
        pdrsdata[sinfo_cu_xy(drs_sc_mask, rx, ry)] = cubePT_FIND;
        return (sinfo_new_c_bezier_interpol(sc_im, drs_sc_mask));
    }
    else
        return (pimdata[sinfo_im_xy(im, ipos, jpos)]);

}

/**@}*/
