/*                                                                            *
 *   This file is part of the ESO SINFO Pipeline                              *
 *   Copyright (C) 2004,2005 European Southern Observatory                    *
 *                                                                            *
 *   This library 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     *
 *                                                                           */
 
/*
 * $Author: amodigli $
 * $Date: 2009-06-05 06:06:11 $
 * $Revision: 1.6 $
 * $Name: not supported by cvs2svn $
 */

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

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
#include <stdio.h>
#include <cpl.h>
#include <cpl_test.h>
#include <sinfo_pro_save.h>
#include <sinfo_error.h>
#include <sinfo_msg.h>
#include <sinfo_utils_wrappers.h>
#include <sinfo_new_cube_ops.h>
/*-----------------------------------------------------------------------------
                                Defines
 -----------------------------------------------------------------------------*/

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


/*----------------------------------------------------------------------------*/
/**
 * @defgroup sinfo_cube_measure_centroid_test  SINFO library unit tests
 */
#define SIZE 256
/*----------------------------------------------------------------------------*/
/**@{*/

/*----------------------------------------------------------------------------*/
/**
 *   @internal
 *     @brief    Find the aperture(s) with the greatest flux
 *       @param    self   The aperture object
 *         @param    ind  The aperture-indices in order of decreasing flux
 *           @param    nfind  Number of indices to find
 *             @return   CPL_ERROR_NONE or the relevant _cpl_error_code_ on error
 *
 *               nfind must be at least 1 and at most the size of the aperture object.
 *
 *                 The ind array must be able to hold (at least) nfind integers.
 *                   On success the first nfind elements of ind point to indices of the
 *                     aperture object.
 *
 *                       To find the single ind of the aperture with the maximum flux use simply:
 *                         int ind;
 *                           apertures_find_max_flux(self, &ind, 1);
 *
 *                            */
/*----------------------------------------------------------------------------*/
cpl_error_code apertures_find_max_flux(const cpl_apertures * self,
                                              int * ind, int nfind)
{
    const int    nsize = cpl_apertures_get_size(self);
    int          ifind;


    cpl_ensure_code(nsize > 0,      cpl_error_get_code());
    cpl_ensure_code(ind,          CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(nfind > 0,      CPL_ERROR_ILLEGAL_INPUT);
    cpl_ensure_code(nfind <= nsize, CPL_ERROR_ILLEGAL_INPUT);

    for (ifind=0; ifind < nfind; ifind++) {
        double maxflux = -1;
        int maxind = -1;
        int i;
        for (i=1; i <= nsize; i++) {
            int k;

            /* The flux has to be the highest among those not already found */
            for (k=0; k < ifind; k++) if (ind[k] == i) break;

            if (k == ifind) {
                /* i has not been inserted into ind */
                const double flux = cpl_apertures_get_flux(self, i);

                if (maxind < 0 || flux > maxflux) {
                    maxind = i;
                    maxflux = flux;
                }
            }
        }
        ind[ifind] = maxind;
    }

    return CPL_ERROR_NONE;

}


/*----------------------------------------------------------------------------*/
/**
 *   @internal
 *   @brief    Find the peak flux, peak sum and position of a Gaussian
 *   @param    self        Image to process
 *   @param    sigma       The initial detection level  [ADU]
 *   @param    pxpos       On success, the refined X-position [pixel]
 *   @param    pypos       On success, the refined Y-position [pixel]
 *   @param    ppeak       On success, the refined peak flux  [ADU]
 *   @return CPL_ERROR_NONE or the relevant CPL error code on error
 *
 *   */
/*----------------------------------------------------------------------------*/
static cpl_error_code
gaussian_maxpos(const cpl_image * self,
                double sigma,
                double  * pxpos,
                double  * pypos,
                double  * ppeak)
{
    /* copied from irplib_strehl.c r163170 */
    const cpl_size  nx = cpl_image_get_size_x(self);
    const cpl_size  ny = cpl_image_get_size_y(self);
    int             iretry = 3; /* Number retries with decreasing sigma */
    int             ifluxapert = 0;
    double          med_dist;
    const double    median = cpl_image_get_median_dev(self, &med_dist);
    cpl_mask      * selection;
    cpl_size        nlabels = 0;
    cpl_image     * labels = NULL;
    cpl_apertures * aperts;
    cpl_size        npixobj;
    double          objradius;
    cpl_size        winsize;
    cpl_size        xposmax, yposmax;
    double          xposcen, yposcen;
    double          valmax, valfit = -1.0;
    cpl_array     * gauss_parameters = NULL;
    cpl_errorstate  prestate = cpl_errorstate_get();
    cpl_error_code  code = CPL_ERROR_NONE;


    cpl_ensure_code( sigma > 0.0, CPL_ERROR_ILLEGAL_INPUT);

    selection = cpl_mask_new(nx, ny);


    /* find aperture with signal larger than sigma * median deviation */
    for (; iretry > 0 && nlabels == 0; iretry--, sigma *= 0.5) {

        /* Compute the threshold */
        const double threshold = median + sigma * med_dist;


        /* Select the pixel above the threshold */
        code = cpl_mask_threshold_image(selection, self, threshold, DBL_MAX,
                                        CPL_BINARY_1);

        if (code) break;

        /* Labelise the thresholded selection */
        cpl_image_delete(labels);
        labels = cpl_image_labelise_mask_create(selection, &nlabels);
    }
    sigma *= 2.0; /* reverse last iteration that found no labels */

    cpl_mask_delete(selection);

    if (code) {
        cpl_image_delete(labels);
        return cpl_error_set_where(cpl_func);
    } else if (nlabels == 0) {
        cpl_image_delete(labels);
        return cpl_error_set(cpl_func, CPL_ERROR_DATA_NOT_FOUND);
    }

    aperts = cpl_apertures_new_from_image(self, labels);

    /* Find the aperture with the greatest flux */
    code = apertures_find_max_flux(aperts, &ifluxapert, 1);

    if (code) {
        cpl_apertures_delete(aperts);
        cpl_image_delete(labels);
        return cpl_error_set(cpl_func, CPL_ERROR_DATA_NOT_FOUND);
    }

    npixobj = cpl_apertures_get_npix(aperts, ifluxapert);
    objradius = sqrt((double)npixobj * CPL_MATH_1_PI);
    winsize = CX_MIN(CX_MIN(nx, ny), (3.0 * objradius));

    xposmax = cpl_apertures_get_maxpos_x(aperts, ifluxapert);
    yposmax = cpl_apertures_get_maxpos_y(aperts, ifluxapert);
    xposcen = cpl_apertures_get_centroid_x(aperts, ifluxapert);
    yposcen = cpl_apertures_get_centroid_y(aperts, ifluxapert);
    valmax  = cpl_apertures_get_max(aperts, ifluxapert);

    cpl_apertures_delete(aperts);
    cpl_image_delete(labels);

    cpl_msg_debug(cpl_func, "Object radius at S/R=%g: %g (window-size=%u)",
                  sigma, objradius, (unsigned)winsize);
    cpl_msg_debug(cpl_func, "Object-peak @ (%d, %d) = %g", (int)xposmax,
                  (int)yposmax, valmax);

    /* fit gaussian to get subpixel peak position */

    gauss_parameters = cpl_array_new(7, CPL_TYPE_DOUBLE);
    cpl_array_set_double(gauss_parameters, 0, median);

    code = cpl_fit_image_gaussian(self, NULL, xposmax, yposmax,
                                  winsize, winsize, gauss_parameters,
                                  NULL, NULL, NULL,
                                  NULL, NULL, NULL,
                                  NULL, NULL, NULL);
    if (!code) {
        const double M_x = cpl_array_get_double(gauss_parameters, 3, NULL);
        const double M_y = cpl_array_get_double(gauss_parameters, 4, NULL);

        valfit = cpl_gaussian_eval_2d(gauss_parameters, M_x, M_y);

        if (!cpl_errorstate_is_equal(prestate)) {
            code = cpl_error_get_code();
        } else {
            *pxpos        = M_x;
            *pypos        = M_y;
            *ppeak        = valfit;

            cpl_msg_debug(cpl_func, "Gauss-fit @ (%g, %g) = %g",
                          M_x, M_y, valfit);
        }
    }
    cpl_array_delete(gauss_parameters);

    if (code || valfit < valmax) {
        cpl_errorstate_set(prestate);
        *pxpos   = xposcen;
        *pypos   = yposcen;
        *ppeak   = valmax;
    }

    return code ? cpl_error_set_where(cpl_func) : CPL_ERROR_NONE;
}

   
/**
   @brief      test cube load
 */
/*---------------------------------------------------------------------------*/
/* TODO: find a way to generate a cube with the unit test */
static void
test_load_data(void)
{
  cpl_imagelist* cube_tst=NULL;
  cpl_image* img=NULL;
  cpl_propertylist* plist=NULL;
  char name[SIZE];
  int i=0;
  int sz=0;
  //char* src_data="/data2/sinfoni/sinfo_demo/pro/sinfo_rec_jitter_objnod_358/";
  char* src_data="/data2/sinfoni/spr/mariya/";
  //char* src_data="/data2/sinfoni/sinfo_demo/sof/";

  //We use input data generated by the pipeline
  //sprintf(name,"%s%s%2.2d%s",src_data,"out_cube_obj",i,".fits");
  sprintf(name,"%s%s",src_data,"jitter_std0013.fits");
  sinfo_msg("name=%s",name);
  sinfo_msg_warning("name=%s",name);
  plist=cpl_propertylist_load(name,0);
  //double crpix3=sinfo_pfits_get_crpix3(plist);
  double crval3=sinfo_pfits_get_crval3(plist);
  double cdelt3=sinfo_pfits_get_cdelt3(plist);
  cpl_propertylist_delete(plist);
  check_nomsg(cube_tst = cpl_imagelist_load(name,CPL_TYPE_FLOAT,0));
  sinfo_imagelist_reject_value(cube_tst,CPL_VALUE_NAN);
  sz=cpl_imagelist_get_size(cube_tst);

  double xc,yc,peak,wav;
  int margin=300;
  int nrow=0;
  cpl_table* trace=cpl_table_new(sz-2*margin);
  nrow=cpl_table_get_nrow(trace);
  cpl_table_new_column(trace,"wave",CPL_TYPE_DOUBLE);
  cpl_table_new_column(trace,"xc",CPL_TYPE_DOUBLE);
  cpl_table_new_column(trace,"yc",CPL_TYPE_DOUBLE);
  cpl_table_new_column(trace,"peak",CPL_TYPE_DOUBLE);

  cpl_table_fill_column_window_double(trace,"wave",0,nrow,0);
  cpl_table_fill_column_window_double(trace,"xc",0,nrow,0);
  cpl_table_fill_column_window_double(trace,"yc",0,nrow,0);
  cpl_table_fill_column_window_double(trace,"peak",0,nrow,0);
  double* pw=NULL;
  double* pxc=NULL;
  double* pyc=NULL;
  double* ppeak=NULL;
  int j=0;
  int rad=10;
  cpl_image* sum;
  pw=cpl_table_get_data_double(trace,"wave");
  pxc=cpl_table_get_data_double(trace,"xc");
  pyc=cpl_table_get_data_double(trace,"yc");
  ppeak=cpl_table_get_data_double(trace,"peak");

  for(i=margin;i<sz-margin;i++) {
     for(j=-rad;j<rad;j++) {
       check_nomsg(img=cpl_imagelist_get(cube_tst,i+j));
       if(j==-rad) {
         sum=cpl_image_duplicate(img);
       } else {
         cpl_image_add(sum,img);
       }
    }
    cpl_image_divide_scalar(sum,2*rad);
    sprintf(name,"%s%4.4d%s","ima_tst",i,".fits");
    check_nomsg(gaussian_maxpos(sum,5,&xc,&yc,&peak));
    cpl_image_delete(sum);
    wav=crval3+(i-crval3-1)*cdelt3;
    pw[i-margin]=wav;
    pxc[i-margin]=xc;
    pyc[i-margin]=yc;
    ppeak[i-margin]=peak;
    sinfo_msg_warning("plane=%d max=%d xc=%g yc=%g int=%g",i,sz-margin,xc,yc,peak);
    /*
    check_nomsg(cpl_image_save(img,name,CPL_BPP_IEEE_FLOAT,
			       NULL,CPL_IO_DEFAULT));
    */
  }

  sprintf(name,"%s","cub_tst.fits");
  check_nomsg(cpl_imagelist_save(cube_tst,name,CPL_BPP_IEEE_FLOAT,
				 NULL,CPL_IO_DEFAULT));
  sprintf(name,"%s","trace.fits");
  sinfo_msg_warning("tab=%p name=%s",trace,name);
  cpl_table_save(trace,NULL,NULL,name,CPL_IO_DEFAULT);
  sinfo_msg_warning("Note that fails before this line");
 cleanup:
  sinfo_msg_warning("about to exit");

  sinfo_free_imagelist(&cube_tst);
  sinfo_free_table(&trace);

  return;

}

/*----------------------------------------------------------------------------*/
/**
  @brief   SINFONI pipeline unit test for cube load

**/
/*----------------------------------------------------------------------------*/

int main(void)
{
  cpl_test_init(PACKAGE_BUGREPORT, CPL_MSG_WARNING);

    /* Initialize CPL + SINFO messaging */
  //test_load_data();

  return cpl_test_end(0);

}


/**@}*/
