/*
 * 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+size_x*size_y*z)
@param im input image
@param X  point X
@param Y  point Y
@param Z  point Z
@return  (x+size_x*y+size_x*size_y*z)
*/
int sinfo_im_xyz(cpl_image* im, int X, int Y, int Z)
{
  int res=0;
   res = X+
         Y*cpl_image_get_size_x(im)+
         Z*cpl_image_get_size_x(im)*
           cpl_image_get_size_y(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_cu_xyz
@brief computes (x+size_x*y+size_x*size_y*z)
@param im input image
@param X  point X
@param Y  point Y
@param Z  point Z
@return  (x+size_x*y+size_x*size_y*z)
*/

int sinfo_cu_xyz(cpl_imagelist* cu, int X, int Y, int Z)
{
  int res=0;
   res = X+
         Y*cpl_image_get_size_x(cpl_imagelist_get(cu,0))+
         Z*cpl_image_get_size_x(cpl_imagelist_get(cu,0))*
           cpl_image_get_size_y(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;

    cpl_image* sc_img=NULL;
    cpl_image* drs_img=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++) {
      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++) {
      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;

    cpl_image* sc_img=NULL;
    cpl_image* drs_img=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++) {
      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++) {
      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[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,distance,selected_distance;
    new_Dim *point=NULL;
    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);
    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 )
{
   int k, kn, nn, nkn;
   double blend, muk;

   muk = 1;
   for ( k = 0; k <= n; k++ ) {
      nn = n;
      kn = k;
      nkn = n - k;
      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 );
}

int
sinfo_new_c_create_XYZ( new_Lookup *l )
{
    cpl_image *imX,*imY,*imZ,*imcX;
    short i,j,k,indexI,indexJ,x,y,z;
    int size;
    int idlx=0;
    int idly=0;
    int idnp=0;
    float* piddata=NULL;
    float* pjddata=NULL;
    float* pXdata=NULL;
    float* pYdata=NULL;
    float* pZdata=NULL;
    float* phXdata=NULL;

    cpl_image* i_img=NULL;
    cpl_image* j_img=NULL;

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

    size = idlx*idly;
    /* allocate memory */
    if ( NULL == (imX = cpl_image_new(size, size, CPL_TYPE_FLOAT)) )
        {
    sinfo_msg_error(" could not allocate memory for X !\n") ;
    return -1 ;
        }
    if ( NULL == (imY = cpl_image_new(size, size, CPL_TYPE_FLOAT)) )
        {
    sinfo_msg_error(" could not allocate memory for Y !\n") ;
    return -1 ;
        }
    if ( NULL == (imZ = cpl_image_new(size, size, CPL_TYPE_FLOAT)) )
        {
    sinfo_msg_error(" could not allocate memory for Z !\n") ;
    return -1 ;
        }
    if ( NULL == (imcX = cpl_image_new(size, size, CPL_TYPE_FLOAT)) )
        {
    sinfo_msg_error(" could not allocate memory for cX !\n") ;
    return -1 ;
        }

    l -> X  = imX;
    l -> Y  = imY;
    l -> Z  = imZ;
    l -> hX = imcX;

    /*Round id*/
    for( i = 0; i < idlx; i++)
    {
    for( j = 0; j < idly; j++)
        {
        for( k = 0; k < idnp; k++)
        {
          i_img=cpl_imagelist_get(l->id,k);
          piddata=cpl_image_get_data_float(i_img);
          piddata[sinfo_cu_xy(l->id,i,j)] =
                  (float)sinfo_new_nint(piddata[sinfo_cu_xy(l->id,i,j)]);
        }
        }
    }

    /*Round jd*/
    for( i = 0; i < idlx; i++)
    {
    for( j = 0; j < idly; j++)
        {
        for( k = 0; k < idnp; k++)
        {
          j_img=cpl_imagelist_get(l->jd,k);
          pjddata=cpl_image_get_data_float(j_img);
          pjddata[sinfo_cu_xy(l->jd,i,j)] =
                 (float)sinfo_new_nint(pjddata[sinfo_cu_xy(l->jd,i,j)]);
        }
        }
    }

    /*Fill with -1 X Y Z*/
    for( i = 0; i < cpl_image_get_size_x(l -> X); i++)
    {
    for( j = 0; j < cpl_image_get_size_y(l -> X); j++)
        {
          pXdata=cpl_image_get_data_float(l->X);
          pYdata=cpl_image_get_data_float(l->Y);
          pZdata=cpl_image_get_data_float(l->Z);

          pXdata[sinfo_im_xy(l->X,i,j)] = ZERO;
          pYdata[sinfo_im_xy(l->Y,i,j)] = ZERO;
          pZdata[sinfo_im_xy(l->Z,i,j)] = ZERO;
        }
    }
#define  FORW
#ifdef BACK
    for( x = idlx - 1;x>=0;x--)
    {
    for( y = idly - 1;y>=0;y--)
        {
        for( z = idnp - 1;z>=0;z--)
#endif
#ifdef FORW
    for( x = 0; x <  idlx; x++)
    {
    for( y = 0; y < idly; y++)
        {
        for( z = 0; z < idnp; z++)
#endif
          {
        i_img=cpl_imagelist_get(l->id,z);
        piddata=cpl_image_get_data_float(i_img);
        j_img=cpl_imagelist_get(l->jd,z);
        pjddata=cpl_image_get_data_float(j_img);
        indexI = piddata [sinfo_cu_xy(l->id,x,y)];
        indexJ = pjddata [sinfo_cu_xy(l->jd,x,y)];
        if ( indexI > 0.0  && indexI < size  &&
             indexJ > 0.0  && indexJ < size )
            {
            /*sinfo_msg_debug("%d %d %d = %f, %f\n",
                      x,y,z,(float)ICube(x,y,z),(float)JCube(x,y,z));*/
              pXdata=cpl_image_get_data_float(l->X);
              pYdata=cpl_image_get_data_float(l->Y);
              pZdata=cpl_image_get_data_float(l->Z);
              phXdata=cpl_image_get_data_float(l->hX);

              pXdata[sinfo_im_xy(l->X ,indexI,indexJ)] = x;
              phXdata[sinfo_im_xy(l->hX,indexI,indexJ)] =
                         phXdata[sinfo_im_xy(l->hX,indexI,indexJ)] + 1;

            pYdata[sinfo_im_xy(l->Y ,indexI,indexJ)] = y;
            /*phXdata[sinfo_im_xy(l->hX,indexI,indexJ)] =
                      phXdata[sinfo_im_xy(l->hX,indexI,indexJ)] + 1;*/

            pZdata[sinfo_im_xy(l->Z ,indexI,indexJ)] = z;
            /*phXdata[sinfo_im_xy(l->hX,indexI,indexJ)] =
                      phXdata[sinfo_im_xy(l->hX,indexI,indexJ)] + 1;*/
            }
        }
        }
    }


    sinfo_msg("Filled X Y Z , cX cY cZ 2D frames\n");
    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_find_cosmic
@brief Find cosmic rays in an image
@param im pointer to image
@param mask pointer to bad pixel map
@param look pointer to look-up table
@param rx radius on X
@param ry radius on Y
@param rz radius on Z
@param lowerI  lower I
@param highI   higher I
@param highJ   higher J
@param factor  threshold factor

@return if succes 0, else -1
*/
cpl_image *
sinfo_new_c_bezier_find_cosmic( cpl_image *im,
                                      cpl_image *mask,
                                      new_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;


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



    int ilx=0;
    int ily=0;

    int mlx=0;
    int mly=0;




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

    ilx=cpl_image_get_size_x(im);
    ily=cpl_image_get_size_y(im);
    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++){
      o_img=cpl_image_new(szx,szy,CPL_TYPE_FLOAT);
      cpl_imagelist_set(sc_im,o_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++){
      o_img=cpl_image_new(szx,szy,CPL_TYPE_FLOAT);
      cpl_imagelist_set(drs_sc_mask,o_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_cosmic( i, j, im,
                                                              mask, sc_im,
                                                              drs_sc_mask,
                                                              look,
                                                              rx_loop,
                                                              ry_loop,
                                                              rz_loop,
                                                              &med,
                                                              &stdev,
                                                              factor );
            /* if no valid neighboors are found, increase size of
                   sub cube until max radius is reached */
        while ( newValue == cubeNONEIGHBOR && rx_loop < rx &&
                        ry_loop < ry && rz_loop < rz )
            {
              rx_loop++ ; ry_loop++; rz_loop++;
              /*sinfo_msg_debug("Increasing radius to %d, in %d %d",
                          rx_loop, i, j) ;  */
                   newValue = sinfo_new_c_bezier_correct_cosmic( i, j, im,
                                                                    mask,
                                                                    sc_im,
                                                                    drs_sc_mask,
                                                                    look,
                                                                    rx_loop,
                                                                    ry_loop,
                                                                    rz_loop,
                                                                    &med,
                                                                    &stdev,
                                                                     factor );
            }
        /*give up on increasing the size*/
        if ( isnan(newValue) || newValue == cubeNONEIGHBOR )
                /*<= -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_debug("[%d,%d]=%f -> %f, med= %f, stdev=%f",
                      i,j, old, newValue, med, stdev ); */
            count++;
            }
        }
        }
    }


    sinfo_msg_debug("bad pixels count: %d",count);


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


/**
@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)]);


}

/**@}*/
