/* $Id: naco_img_checkfocus.c,v 1.59 2011-12-22 11:09:36 llundin Exp $
 *
 * This file is part of the NACO Pipeline
 * Copyright (C) 2002,2003 European Southern Observatory
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */

/*
 * $Author: llundin $
 * $Date: 2011-12-22 11:09:36 $
 * $Revision: 1.59 $
 * $Name: not supported by cvs2svn $
 */

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

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

#include "naco_recipe.h"
#include "naco_strehl.h"
#include "irplib_strehl.h"

/*-----------------------------------------------------------------------------
                                Define
 -----------------------------------------------------------------------------*/

#define STREHL_DEF_LOCATE_SX            512
#define STREHL_DEF_LOCATE_SY            512
#define ENERGY_RADIUS_PIX               11

#define RECIPE_STRING "naco_img_checkfocus"

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

static cpl_error_code naco_img_checkfocus_reduce(const cpl_parameterlist *,
                                                 const irplib_framelist *, int,
                                                 const cpl_image *, double *,
                                                 double *, double *, double *,
                                                 double *);

static cpl_error_code naco_img_checkfocus_qc(cpl_propertylist *,
                                             const irplib_framelist *);

static cpl_error_code naco_img_checkfocus_save(cpl_frameset *,
                                               const cpl_parameterlist *,
                                               const cpl_propertylist *);

NACO_RECIPE_DEFINE(naco_img_checkfocus,
    NACO_PARAM_PLOT    |                        
    NACO_PARAM_STAR_R  |                        
    NACO_PARAM_BG_RINT |                        
    NACO_PARAM_BG_REXT,
    "Focus check recipe",
    RECIPE_STRING " -- The focus checking recipe\n"        
    "The Set Of Frames (sof-file) must specify at least four "  
    "files and they must be tagged\n"                           
    "NACO-raw-file.fits "NACO_IMG_CHECKFOCUS_RAW"\n"            
    "The first of the files is used as a dark frame.\n");

/*----------------------------------------------------------------------------*/
/**
 * @defgroup naco_img_checkfocus   Focus Check
 */
/*----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------
                                Functions code
 -----------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------*/
/**
  @brief    The recipe data reduction part is implemented here 
  @param    framelist   the frames list
  @param    parlist     the parameters list
  @return   0 iff everything is ok
 */
/*----------------------------------------------------------------------------*/
static int naco_img_checkfocus(cpl_frameset            * framelist,
                               const cpl_parameterlist * parlist)
{
    cpl_errorstate cleanstate = cpl_errorstate_get();
    irplib_framelist * allframes = NULL;
    irplib_framelist * rawframes = NULL;
    cpl_propertylist * qclist = cpl_propertylist_new();
    cpl_image        * dark = NULL;
    cpl_vector       * strehl_vec = NULL;
    cpl_matrix       * focus_mat = NULL;
    cpl_vector       * focus_res = NULL;
    cpl_polynomial   * fit_poly = NULL;
    const char       * darkfile;
    int                nframes;
    int                nb_good;
    const cpl_size     degree1 = 1;
    const cpl_size     degree2 = 2;
    double             best_strehl = DBL_MAX; /* Avoid (false) uninit warning */
    double             c1, c2;
    double             optimal_focus, optimal_strehl, mse2, mse1;
    int                i;

    /* Identify the RAW and CALIB frames in the input frameset */
    skip_if (naco_dfs_set_groups(framelist));

    allframes = irplib_framelist_cast(framelist);
    skip_if(allframes == NULL);

    rawframes = irplib_framelist_extract(allframes, NACO_IMG_CHECKFOCUS_RAW);
    skip_if(rawframes == NULL);
    irplib_framelist_empty(allframes);

    nframes = irplib_framelist_get_size(rawframes);
    irplib_ensure(nframes >= 4, CPL_ERROR_DATA_NOT_FOUND,
                  "Must have at least 4 (not %d) frames to check the focus",
                   nframes);
   
    skip_if(irplib_framelist_load_propertylist(rawframes, 0, 0, "^("
                                               NACO_PFITS_REGEXP_CHECKFOCUS "|"
                                               NACO_PFITS_REGEXP_CHECKFOCUS_PAF
                                               ")$", CPL_FALSE));

    skip_if(irplib_framelist_load_propertylist_all(rawframes, 0, "^("
                                                   NACO_PFITS_REGEXP_CHECKFOCUS
                                                   ")$", CPL_FALSE));

    /* The dark is the first frame */
    cpl_msg_info(cpl_func, "The first frame is used as a dark");
    darkfile = cpl_frame_get_filename(irplib_framelist_get_const(rawframes, 0));
    skip_if (0);

    irplib_check(dark = cpl_image_load(darkfile, CPL_TYPE_FLOAT, 0, 0),
                 "Could not load the dark from %s", darkfile);

    /* Allocate vectors to store results */
    strehl_vec = cpl_vector_new(nframes-1);
    focus_mat  = cpl_matrix_new(1, nframes-1);
    
    skip_if (naco_img_checkfocus_qc(qclist, rawframes));

    /* Reduce each frame */
    nb_good = 0;
    for (i=1 ; i < nframes ; i++) {
        double focus = DBL_MAX; /* Avoid (false) uninit warning */
        double energy = DBL_MAX; /* Avoid (false) uninit warning */
        double fwhm = DBL_MAX; /* Avoid (false) uninit warning */
        double strehl, strehl_err;

        cpl_msg_info(cpl_func, "Reducing frame %d of %d\n", i+1, nframes);

        /* Reduce frame nb i */
        if (naco_img_checkfocus_reduce(parlist, rawframes, i, dark,
                                      &fwhm, &strehl, &strehl_err,
                                       &energy, &focus)) {
            naco_error_reset("Could not compute focus for this frame:");
            continue;
        }

        /* Keep only the values where strehl_err < 10% */
        if (strehl_err >= 0.1) continue;

        /* Assign the results in the vectors */
        bug_if (cpl_vector_set(strehl_vec,   nb_good, strehl));
        bug_if (cpl_matrix_set(focus_mat, 0, nb_good, focus));

        nb_good++;

        if (nb_good > 1 && strehl <= best_strehl) continue;

        /* Found the best FOCUS */
        best_strehl = strehl;

        /* Add/Update QC parameters */
        bug_if(cpl_propertylist_update_double(qclist, "ESO QC STREHL", strehl));
        bug_if(cpl_propertylist_update_double(qclist, "ESO QC STREHL ERROR",
                                              strehl_err));
        bug_if(cpl_propertylist_update_double(qclist, "ESO QC FWHM PIX", fwhm));
        bug_if(cpl_propertylist_update_double(qclist, "ESO QC ENERGY", energy));
        bug_if(cpl_propertylist_update_double(qclist, "ESO QC FOCUS", focus));


    }
    cpl_image_delete(dark);
    dark = NULL;
    irplib_framelist_empty(rawframes);

    skip_if_lt (nb_good, 3, "frames with a Strehl error less than 0.1");

    bug_if (cpl_vector_set_size(strehl_vec,   nb_good));
    bug_if (cpl_matrix_set_size(focus_mat, 1, nb_good));

    /* Fit the optimal focus - and make sure it is well-defined */
    focus_res  = cpl_vector_new(nb_good);
    fit_poly = cpl_polynomial_new(1);
    skip_if(cpl_polynomial_fit(fit_poly, focus_mat, NULL, strehl_vec, NULL,
                               CPL_FALSE, NULL, &degree1));

    bug_if(cpl_vector_fill_polynomial_fit_residual(focus_res, strehl_vec, NULL,
                                                   fit_poly, focus_mat, NULL));
    mse1 = cpl_vector_product(focus_res, focus_res) / nb_good;

    skip_if(cpl_polynomial_fit(fit_poly, focus_mat, NULL, strehl_vec, NULL,
                               CPL_FALSE, NULL, &degree2));

    bug_if(cpl_vector_fill_polynomial_fit_residual(focus_res, strehl_vec, NULL,
                                                   fit_poly, focus_mat, NULL));
    mse2 = cpl_vector_product(focus_res, focus_res) / nb_good;

    cpl_vector_delete(focus_res);
    focus_res = NULL;
    cpl_vector_delete(strehl_vec);
    strehl_vec = NULL;
    cpl_matrix_delete(focus_mat);
    focus_mat = NULL;

    c1 = cpl_polynomial_get_coeff(fit_poly, &degree1);
    c2 = cpl_polynomial_get_coeff(fit_poly, &degree2);

    irplib_ensure(mse2 < mse1, CPL_ERROR_DATA_NOT_FOUND,
                  "Ill-defined optimal focus, the strehl ratio "
                   "appears to be a linear function of the focus value: "
                   "mse(2)=%g >= mse(1)=%g (c1=%g, c2=%g)",
                   mse2, mse1, c1, c2);

    bug_if (c2 == 0.0);

    irplib_ensure(c2 < 0.0, CPL_ERROR_DATA_NOT_FOUND,
                  "Ill-defined optimal focus, the strehl ratio "
                   "does not have a single optimal value: mse(2)=%g, c1=%g, "
                   "c2=%g > 0", mse2, c1, c2);

    optimal_focus = -c1/(2.0*c2);

    /* Could compute derivate as well, to check that it is close to zero */
    optimal_strehl = cpl_polynomial_eval_1d(fit_poly, optimal_focus, NULL);

    cpl_polynomial_delete(fit_poly);
    fit_poly = NULL;

    cpl_msg_info(cpl_func, "Strehl ratio with optimal Focus(%g): %g "
                 "(mse(2)=%g < mse(1)=%g)", optimal_focus, optimal_strehl,
                 mse2, mse1);

    bug_if(cpl_propertylist_append_double(qclist, "ESO QC FOCUSOPT",
                                           optimal_focus));

    /* PRO.CATG */
    bug_if(cpl_propertylist_append_string(qclist, CPL_DFS_PRO_CATG,
                                          NACO_IMG_CHECKFOCUS));

    skip_if (naco_img_checkfocus_save(framelist, parlist, qclist));

    end_skip;
    
    cpl_propertylist_delete(qclist);
    irplib_framelist_delete(allframes);
    irplib_framelist_delete(rawframes);
    cpl_image_delete(dark);

    cpl_vector_delete(focus_res);
    cpl_vector_delete(strehl_vec);
    cpl_matrix_delete(focus_mat);

    cpl_polynomial_delete(fit_poly);

    return cpl_error_get_code();
}

/*----------------------------------------------------------------------------*/
/**
  @brief    The strehl is computed here
  @param    parlist   The list of recipe parameters
  @param    rawframe  the list of raw frames
  @param    iframe    The frame number to reduce
  @param    dark        the dark image
  @param    fwhm        the computed fwhm
  @param    strehl      the computed strehl
  @param    strehl_err  the computed strehl_err
  @param    energy      the computed energy
  @param    focus       the computed focus
  @return   0 iff everything is ok
 */
/*----------------------------------------------------------------------------*/
static cpl_error_code naco_img_checkfocus_reduce(const cpl_parameterlist * parlist,
                                                 const irplib_framelist  * rawframes,
                                                 int                       iframe,
                                                 const cpl_image *   dark,
                                                 double          *   fwhm,
                                                 double          *   strehl,
                                                 double          *   strehl_err,
                                                 double          *   energy,
                                                 double          *   focus)
{
    const cpl_propertylist * plist;
    const char          *   filter;
    double                  pixscale;
    cpl_image           *   ima = NULL;
    cpl_vector          *   sigmas = NULL;
    cpl_apertures       *   apert = NULL;
    const char          *   file;
    double                  psigmas[] = {5, 2, 1, 0.5};
    const int               nsigmas = (int)(sizeof(psigmas)/sizeof(double));
    cpl_size                isigma;
    double                  lam, dlam;
    double                  pos_x, pos_y;
    double                  star_bg, star_peak, star_flux, psf_peak, psf_flux, 
                            bg_noise;
    double                  fwhm_x, fwhm_y;
    int                     imax_flux;


    skip_if (0);

    bug_if (parlist == NULL);
    bug_if (rawframes == NULL);
    bug_if (dark == NULL);
    bug_if (fwhm == NULL);
    bug_if (strehl == NULL);
    bug_if (strehl_err == NULL);
    bug_if (energy == NULL);
    bug_if (focus == NULL);

    file = cpl_frame_get_filename(irplib_framelist_get_const(rawframes, iframe));

    /* Print out the filter and the pixel scale */
    plist = irplib_framelist_get_propertylist_const(rawframes, iframe);
    bug_if (0);

    filter = naco_pfits_get_filter(plist);
    pixscale = naco_pfits_get_pixscale(plist);
    *focus = naco_pfits_get_focus(plist);

    skip_if (0);

    /* Get lam and dlam from the filter name for the Strehl computation */
    irplib_check(naco_get_filter_infos(filter, &lam, &dlam),
                 "Cannot get filter infos [%s]", filter);

    /* Load input images */
    cpl_msg_info(cpl_func, "---> Load input image and subtract the dark");
    ima = cpl_image_load(file, CPL_TYPE_FLOAT, 0, 0);
    skip_if (0);
    
    bug_if (cpl_image_subtract(ima, dark));
    
    /* Detect a bright star around the center */
    cpl_msg_info(cpl_func, "---> Detecting a bright star using "
                 "%d sigma-levels ranging from %g down to %g", nsigmas,
                 psigmas[0], psigmas[nsigmas-1]);
    sigmas = cpl_vector_wrap(nsigmas, psigmas);
    apert = cpl_apertures_extract_window(ima, sigmas, 
            (int)(cpl_image_get_size_x(ima)-STREHL_DEF_LOCATE_SX)/2,
            (int)(cpl_image_get_size_y(ima)-STREHL_DEF_LOCATE_SY)/2,
            (int)(cpl_image_get_size_x(ima)+STREHL_DEF_LOCATE_SX)/2,
            (int)(cpl_image_get_size_y(ima)+STREHL_DEF_LOCATE_SY)/2,
            &isigma);
    if (apert == NULL) {
        cpl_msg_error(cpl_func, "Cannot detect any object");
        skip_if(1);
    }

    /* Find position of aperture with largest flux */
    skip_if (irplib_apertures_find_max_flux(apert, &imax_flux, 1));

    pos_x = cpl_apertures_get_centroid_x(apert, imax_flux);
    skip_if (0);
    pos_y = cpl_apertures_get_centroid_y(apert, imax_flux);
    skip_if (0);

    cpl_apertures_delete(apert);
    apert = NULL;

    cpl_msg_info(cpl_func, "Star detected at sigma=%g, at position: %g %g",
                 psigmas[isigma], pos_x, pos_y);
   
    /* Compute the strehl */
    cpl_msg_info(cpl_func, "---> Compute the strehl");
    irplib_check(naco_strehl_compute(ima, parlist, RECIPE_STRING, lam, dlam,
                                       pos_x, pos_y,  pixscale,
                                       strehl, strehl_err, &star_bg, &star_peak,
                                       &star_flux, &psf_peak,
                                       &psf_flux, &bg_noise),
                 "Cannot compute the strehl");

    /* Compute the energy */
    *energy = irplib_strehl_disk_flux(ima, pos_x, pos_y, ENERGY_RADIUS_PIX,
                                      0.0);

    skip_if (0);
    
    /* Compute the FHWM */
    skip_if (cpl_image_get_fwhm(ima, (int)pos_x, (int)pos_y, &fwhm_x, &fwhm_y));

    *fwhm = (fwhm_x+fwhm_y)/2.0;

    /* Display results */
    cpl_msg_info(cpl_func, "Strehl:                    %g", *strehl);
    cpl_msg_info(cpl_func, "Strehl error:              %g", *strehl_err);
    cpl_msg_info(cpl_func, "Energy:                    %g", *energy);
    cpl_msg_info(cpl_func, "FWHM:                      %g", *fwhm);
    cpl_msg_info(cpl_func, "Focus:                     %g", *focus);

    end_skip;

    cpl_image_delete(ima);
    cpl_vector_unwrap(sigmas);
    cpl_apertures_delete(apert);

    return cpl_error_get_code();
}


/*----------------------------------------------------------------------------*/
/**
  @brief    Append the QC parameters to the propertylist
  @param    qclist    List of QC parameters
  @param    rawframes List of rawframes and their propertylists
  @return   0 iff everything is ok
 */
/*----------------------------------------------------------------------------*/
static cpl_error_code naco_img_checkfocus_qc(cpl_propertylist       * qclist,
                                             const irplib_framelist * rawframes)
{

    const cpl_propertylist * reflist
        = irplib_framelist_get_propertylist_const(rawframes, 0);


    bug_if (0);

    /* Get the keywords for the paf file */
    bug_if(cpl_propertylist_copy_property_regexp(qclist, reflist,
                                                 "^(" IRPLIB_PFITS_REGEXP_PAF
                                                 ")$", 0));
  
    end_skip;

    return cpl_error_get_code();
}

/*----------------------------------------------------------------------------*/
/**
  @brief    Save the recipe product on disk
  @param    self    The raw frames
  @param    parlist The input parameters list
  @param    qclist  List of QC parameters (modified ...)
  @return   0 iff everything is ok
 */
/*----------------------------------------------------------------------------*/
static
cpl_error_code naco_img_checkfocus_save(cpl_frameset * self,
                                        const cpl_parameterlist * parlist,
                                        const cpl_propertylist * qclist)
{


    skip_if(cpl_dfs_save_propertylist(self, NULL, parlist, self, NULL,
                                      RECIPE_STRING, qclist, NULL, naco_pipe_id,
                                      RECIPE_STRING CPL_DFS_FITS));

#ifdef NACO_SAVE_PAF
    skip_if (cpl_dfs_save_paf("NACO", RECIPE_STRING, qclist,
                             RECIPE_STRING CPL_DFS_PAF));
#endif

    end_skip;

    return cpl_error_get_code();
}
