/* $Id: visir_util_detect_shift.c,v 1.97 2013-05-14 12:59:23 jtaylor Exp $
 *
 * This file is part of the VISIR Pipeline
 * Copyright (C) 2012,2013,2014 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
 */

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

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

#include "visir_recipe.h"

#include "visir_spc_distortion.h"
#include "visir_utils.h"
#include "irplib_framelist.h"
#include <cxlist.h>
#include <string.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include <time.h>
#ifdef _OPENMP
#include <omp.h>
#endif
/*-----------------------------------------------------------------------------
                                Defines
 -----------------------------------------------------------------------------*/

#define RECIPE_STRING   "visir_util_detect_shift"

#define MAX_BEAMS 4

#define SQR(a) ((a)*(a))

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

typedef enum {
    VISIR_SHIFT_XCORRELATE,
    VISIR_SHIFT_BRIGHTEST
} visir_shift_method;

typedef enum {
    VISIR_PLANE_AOFF = 0,
    VISIR_PLANE_BON = 1,
    VISIR_PLANE_AON = 2,
    VISIR_PLANE_BOFF = 3,
    VISIR_PLANE_UNDEFINED = 4,
} visir_plane_type;

static const char * type2string[] = {"Aoff", "Bon", "Aon", "Boff", "full"};

typedef struct {
    cpl_size seqnb;
    cpl_image * img;
    cpl_propertylist * plist;
    visir_plane_type type;
    /* original detected or set beam position */
    double x;
    double y;
    /* detected shift on original position */
    double xshift;
    double yshift;
    double correlation;
    double median;
    double mad;
    double stdev;
    double fwhmx;
    double fwhmy;
    double min;
    double max;
    double crval1;
    double crval2;
} visir_plane;


typedef struct {
    double min_cor;
    double max_shift;
    double max_mad;
} stats_limits;

static
cpl_error_code visir_util_detect_shift_one(cpl_frameset *,
                                           irplib_framelist * , int,
                                           const cpl_parameterlist *,
                                           cpl_mask *,
                                           visir_plane **);

#ifdef VISIR_OMP3
static double t_loading = 0.;
static double t_loading_blocked = 0.;
static double t_handling_blocked_in = 0.;
static double t_handling = 0.;
static double t_handling_blocked_out = 0.;
static double t_writing = 0.;
static double t_writing_blocked = 0.;
#endif
static size_t nbytes_load = 0;
static size_t nbytes_load_eff = 0;
static size_t nbytes_save = 0;

static int is_aqu_data = -1;

#ifdef VISIR_CHAIN
#define cpl_plugin_get_info visir_util_detect_shift_get_info
#endif
cpl_recipe_define(visir_util_detect_shift, VISIR_BINARY_VERSION,
                  "Find beam positions and reject bad images",
                  "Julian Tayor", PACKAGE_BUGREPORT, "2012",
                  "Detects beams in a background subtracted multi-beam image,"
                  " determines the shifts between different images and their "
                  "correlation. Images can be rejected if their correlation "
                  "is bad.\n"
                  "The files listed in the Set Of Frames (sof-file) "
                  "must be tagged: \n"
                  "VISIR-chopnod-corrected-file.fits "
                  VISIR_UTIL_CORRECTED
                  "\nbad-pixel-mask.fits " VISIR_CALIB_STATIC_MASK
                  "\nThe output consists of single beam images of the size "
                  "of the throw. The shifts will be written int CRPIX[12].");

/*----------------------------------------------------------------------------*/
/**
 * @defgroup visir_util_detect_shift   Correct the spectral distortion
 */
/*----------------------------------------------------------------------------*/

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

static visir_plane * visir_plane_new(void)
{
    return cpl_calloc(1, sizeof(visir_plane));
}

static void visir_plane_delete(visir_plane * pl)
{
    if (pl) {
        cpl_image_delete(pl->img);
        cpl_propertylist_delete(pl->plist);
        cpl_free(pl);
    }
}

#ifdef _OPENMP
static inline int
pl_compare_seqnb(const visir_plane * a, const visir_plane * b)
{
    if (a->seqnb < b->seqnb)
        return -1;
    if (a->seqnb > b->seqnb)
        return 1;
    return 0;
}
#endif

static void planelist_delete(cx_list * l)
{
    if (l)
        cx_list_destroy(l, (cx_free_func)&visir_plane_delete);
}

static inline cpl_image *
window_extract(const cpl_image * img,
               const cpl_size x, const cpl_size y, const cpl_size window)
{
    const cpl_size nx = cpl_image_get_size_x(img);
    const cpl_size ny = cpl_image_get_size_y(img);
    if (x > nx || y > ny || window <= 0)
        return NULL;
    return cpl_image_extract(img,
                             CX_MAX(1,  x - window / 2),
                             CX_MAX(1,  y - window / 2),
                             CX_MIN(nx, x + window / 2),
                             CX_MIN(ny, y + window / 2));
}


static inline void
window_extract_p(visir_plane * pl, const cpl_image * img,
                 const double x, const double y, const cpl_size window)
{
    pl->img = window_extract(img, x, y, window);
    pl->x = x - CX_MAX(1,  (cpl_size)x - window / 2) + 1;
    pl->y = y - CX_MAX(1,  (cpl_size)y - window / 2) + 1;
}

/* ---------------------------------------------------------------------------*/
/**
 * @brief cut aqu image to illuminated area
 *
 * @param img  pointer to original image, replaced with smaller image
 */
/* ---------------------------------------------------------------------------*/
static void cut_aqu_illuminated(cpl_image ** img)
{
    assert(is_aqu_data >= 0);
    if (!is_aqu_data) {
        return;
    }
    /* only cut full sized aqu images, skip e.g. smaller aquisition images */
    cpl_size llx = cpl_image_get_size_x(*img) == 1024 ? 22 : 1;
    cpl_size lly = cpl_image_get_size_y(*img) == 1024 ? 92 : 1;
    cpl_size urx = cpl_image_get_size_x(*img) == 1024 ?
        878 : cpl_image_get_size_x(*img);
    cpl_size ury = cpl_image_get_size_y(*img) == 1024 ?
        948 : cpl_image_get_size_y(*img);

    cpl_image * tmp = cpl_image_extract(*img, llx, lly, urx, ury);
    cpl_image_delete(*img);
    *img = tmp;
}

static inline cpl_boolean
is_image_bad(const visir_plane * pl, const stats_limits * limits)
{
    return pl->correlation < limits->min_cor ||
        hypot(pl->xshift, pl->yshift) > limits->max_shift ||
        (limits->max_mad > 0 && pl->mad > limits->max_mad);
}


/*----------------------------------------------------------------------------*/
/**
  @internal
  @brief    Fill the recipe parameterlist
  @param    self  The parameterlist
  @return   0 iff everything is ok

 */
/*----------------------------------------------------------------------------*/
static cpl_error_code
visir_util_detect_shift_fill_parameterlist(cpl_parameterlist * self)
{
    const char * context = PACKAGE "." RECIPE_STRING;
    cpl_error_code err;

    cpl_ensure_code(self, CPL_ERROR_NULL_INPUT);

    /* Fill the parameters list */

    /* --template */
    err = irplib_parameterlist_set_string(self, PACKAGE, RECIPE_STRING,
                                          "template", VISIR_STR_PAR_EMPTY, NULL,
                                          context, "Correlation template used "
                                          "to detect shifts and reject bad "
                                          "images.\n If none given an "
                                          "averaged image is used");
    cpl_ensure_code(!err, err);

    /* --min-correlation */
    err = irplib_parameterlist_set_double(self, PACKAGE, RECIPE_STRING,
                                          "min-correlation", 0.5, NULL, context,
                                          "Minimal correlation to the template "
                                          "required to be accepted");
    cpl_ensure_code(!err, err);

    /* --max-shift */
    err = irplib_parameterlist_set_double(self, PACKAGE, RECIPE_STRING,
                                          "max-shift", 10., NULL, context,
                                          "Maximal allowed object shift for an "
                                          "image to be accepted");
    cpl_ensure_code(!err, err);

    /* --max-mad */
    err = irplib_parameterlist_set_double(self, PACKAGE, RECIPE_STRING,
                                          "max-mad", 0., NULL, context,
                                          "Maximal allowed median absolute "
                                          "deviation for an image to be "
                                          "accepted. <= 0 equals no limit");
    cpl_ensure_code(!err, err);

    /* --naverage */
    err = irplib_parameterlist_set_int(self, PACKAGE, RECIPE_STRING,
                                          "naverage", 1, NULL, context,
                                          "Number of planes to average before "
                                          "attempting to detect the shifts.");
    cpl_ensure_code(!err, err);

    /* --method */
    err = irplib_parameterlist_set_string(self, PACKAGE, RECIPE_STRING,
                                          "method", "xcorrelate", NULL, context,
                                          "Method to determine beam shifts.\n"
                                          " xcorrelate: use the cross "
                                          "correlation shift.\n"
                                          " brightest: use the position of "
                                          "the brightest pixel");
    cpl_ensure_code(!err, err);

    /* --no-shift */
    err = irplib_parameterlist_set_bool(self, PACKAGE, RECIPE_STRING,
                                        "no-shift", CPL_FALSE, NULL, context,
                                        "Sets whether to apply the "
                                        "determined shifts in following "
                                        "recipes. Set to TRUE of shifts "
                                        "appear too large.");
    cpl_ensure_code(!err, err);

    /* --no-reject */
    err = irplib_parameterlist_set_bool(self, PACKAGE, RECIPE_STRING,
                                        "no-reject", CPL_TRUE, NULL, context,
                                        "If true images with bad "
                                        "statistics will not be rejected "
                                        "and no shift correction is done");
    cpl_ensure_code(!err, err);

    /* --beampos */
    err = irplib_parameterlist_set_string(self, PACKAGE, RECIPE_STRING,
                                          "beampos", VISIR_STR_PAR_EMPTY,
                                          NULL, context,
                                          "Define the positions of the beams. "
                                          "These positions are cut by the "
                                          "window to extract the single "
                                          "beam images.\n Format: "
                                          "sign:x,y,window;sign:x,y,window;... "
                                          "\n where sign is \"pos\" or \"neg\" "
                                          "depending on whether the beam is "
                                          "positive or negative.\n "
                                          "The window is optional and defines "
                                          "the size of the cut image around "
                                          "the beam. The default window is "
                                          "the chop throw.\n E.g.: "
                                          "pos:50,60;neg:50,160;\n By default "
                                          "autodetection is attempted.\n");
    cpl_ensure_code(!err, err);

    err =
        irplib_parameterlist_set_double(self, PACKAGE, RECIPE_STRING, "eccmax",
                                      3.0, NULL, context,
                       "The maximum eccentricity allowed in the combination "
                       "of the three (in parallel nod/chopping) or four (in "
                       "perpendicular nod/chopping) beams. In parallel mode, "
                       "three perfectly aligned points spaced with the "
                       "chopnod throw will have eccentricity 0, while in "
                       "perpedicular mode a square with the chopnod throw as "
                       "the side length will have eccentricity 0"
                                      );
    cpl_ensure_code(!err, err);

    return CPL_ERROR_NONE;
}


/*----------------------------------------------------------------------------*/
/**
  @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 visir_util_detect_shift(cpl_frameset            * framelist,
                                   const cpl_parameterlist * parlist)
{
    irplib_framelist * allframes  = NULL;
    irplib_framelist * rawframes  = NULL;
    int                n;
    cpl_mask *         bpm = NULL;
    visir_plane *      template = NULL;

#ifdef _OPENMP
    omp_set_num_threads(visir_get_num_threads(CPL_FALSE));
#endif

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

    /* Objects observation */
    allframes = irplib_framelist_cast(framelist);
    skip_if(allframes == NULL);
    rawframes = irplib_framelist_extract_regexp(allframes, "^("
                                                VISIR_UTIL_CORRECTED
                                                ")$", CPL_FALSE);
    skip_if (rawframes == NULL);
    n = irplib_framelist_get_size(rawframes);


    if (cpl_frameset_find_const(framelist, VISIR_CALIB_STATIC_MASK)) {
        irplib_framelist * bpmframes =
            irplib_framelist_extract(allframes, VISIR_CALIB_STATIC_MASK);
        if (irplib_framelist_get_size(bpmframes) == 1) {
            const cpl_frame * bpmframe =
                irplib_framelist_get_const(bpmframes, 0);
            const char * bpmname = cpl_frame_get_filename(bpmframe);
            cpl_image * bpmimg  =
                cpl_image_load(bpmname, CPL_TYPE_UNSPECIFIED, 0, 0);
            cpl_error_code errori;

            bpm = cpl_mask_threshold_image_create(bpmimg, 0.1, DBL_MAX);
            cpl_image_delete(bpmimg);

            errori = cpl_error_set_where(cpl_func);
            error_if(errori, errori, "Could not load bad pixel map");
            cpl_msg_info(cpl_func, "Loaded bad pixel map");
        }
        else
            error_if(1, CPL_ERROR_ILLEGAL_INPUT,
                     "Unexpected number of bad pixel maps");
        irplib_framelist_delete(bpmframes);
    }

    for (cpl_size i = 0; i < n; i++) {
        skip_if(visir_util_detect_shift_one(framelist, rawframes, i,
                                            parlist, bpm, & template));
        cpl_fits_set_mode(CPL_FITS_RESTART_CACHING);
    }

    end_skip;

    irplib_framelist_delete(allframes);
    irplib_framelist_delete(rawframes);
    cpl_mask_delete(bpm);
    visir_plane_delete(template);

    return cpl_error_get_code();
}

/* ---------------------------------------------------------------------------*/
/**
 * @brief load images into a fifo buffer in parallel
 *
 * @param filename  filename to load from
 * @param iext      extension to load
 * @param lim       upper limit of extension
 * @param imgbuf    fifo buffer
 * @param nimgbuf   size of buffer (must be even number)
 *
 * uses openmp tasks to load data in parallel so it can use more cpu when the
 * worker thread is starving, readahead called before this will avoid access
 * pattern issues.
 */
/* ---------------------------------------------------------------------------*/
static void
parallel_load(const char * filename, cpl_size iext, cpl_size lim,
              cpl_image * imgbuf[], cpl_size nimgbuf)
{
    assert(nimgbuf % 2 == 0);
    for (cpl_size i = 0; i < nimgbuf; i+=2) {
        OMP3_PRAGMA(omp task firstprivate(i) if (iext + i < lim))
        {
            cpl_errorstate prestate = cpl_errorstate_get();
            if (iext + i < lim) {
                imgbuf[i] = cpl_image_load(filename, CPL_TYPE_FLOAT, 0,
                                           iext + i);
            }
            if (iext + i + 1 < lim) {
                imgbuf[i + 1] = cpl_image_load(filename, CPL_TYPE_FLOAT, 0,
                                               iext + i + 1);
            }
            cpl_errorstate_set(prestate);
        }
    }
    OMP3_PRAGMA(omp taskwait)
}


static visir_imglist *
load_range(const cpl_frame *frame, cpl_size * start, cpl_size nplanes,
           const cpl_size naverage, cpl_mask * bpm)
{
    const cpl_size next = cpl_frame_get_nextensions(frame);
    visir_imglist * range = visir_imglist_new(nplanes, NULL);
    const char * filename = cpl_frame_get_filename(frame);
    cpl_size iext;
    cpl_propertylist * plist = NULL;
    size_t *pbpm = NULL;
    size_t ndata;
    /* image fifo for parallel loading */
    cpl_image * imgbuf[20] = {NULL};
    size_t nimgbuf = sizeof(imgbuf) / sizeof(imgbuf[0]);

    nplanes = CX_MAX(nplanes, naverage);

    cpl_ensure(naverage > 0, CPL_ERROR_ILLEGAL_INPUT, NULL);
    cpl_imagelist * av = cpl_imagelist_new();

    for (iext = *start; iext < 1 + next; iext++) {
        cpl_errorstate prestate = cpl_errorstate_get();
        size_t nbytes;

        cpl_propertylist_delete(plist);

        plist = cpl_propertylist_load(filename, iext);
        if (plist == NULL) {
            cpl_msg_info(cpl_func, "No propertylist in extension %d",
                         (int)iext);
            cpl_errorstate_set(prestate);
            continue;
        }


        /* load more images if our fifo is empty
         * as the input can be compressed and is often still in the cache from
         * repack we profit from loading with more then one thread */
        if (imgbuf[0] == NULL) {
            parallel_load(filename, iext, next + 1, imgbuf, nimgbuf);
        }

        /* pop one image of the buffer */
        cpl_image * img = imgbuf[0];
        memmove(imgbuf, imgbuf + 1, sizeof(imgbuf) - sizeof(imgbuf[0]));
        imgbuf[nimgbuf - 1] = NULL;

        if (img == NULL) {
            cpl_msg_debug(cpl_func, "No image-data in extension %d",
                         (int)iext);
            cpl_propertylist_delete(plist);
            plist = NULL;
            cpl_errorstate_set(prestate);
            continue;
        }

        nbytes = visir_get_nbytes_plist(plist);

        /* readahead the file for optimal disk IO
         * start + 1 as first extension is normally empty */
        if (iext == *start + 1) {
            visir_readahead(filename, iext * nbytes, nplanes * nbytes);
        }

        nbytes_load += nbytes;
        /* uncompressed data loaded assuming bitpix = float */
        nbytes_load_eff += cpl_image_get_size_x(img) *
            cpl_image_get_size_y(img) * 4;
        cpl_imagelist_set(av, img, cpl_imagelist_get_size(av));

        if (cpl_imagelist_get_size(av) == naverage) {
            cpl_image * avimg = naverage > 1 ? cpl_imagelist_collapse_create(av)
                : cpl_imagelist_unset(av, 0);
            if (bpm) {
                cpl_image_reject_from_mask(avimg, bpm);
                /* cpl_detector_interpolate_rejected(avimg); too slow */
                visir_interpolate_rejected(avimg, &pbpm, &ndata);
            }

            cut_aqu_illuminated(&avimg);
            visir_imglist_append(range, avimg, plist);

            plist = NULL;
            cpl_imagelist_delete(av);
            av = cpl_imagelist_new();
        }

        if (visir_imglist_get_size(range) == nplanes/naverage)
            break;
    }

    if (cpl_imagelist_get_size(av) != 0) {
        cpl_image * avimg = cpl_imagelist_collapse_create(av);
        if (bpm) {
            cpl_image_reject_from_mask(avimg, bpm);
            /* cpl_detector_interpolate_rejected(avimg); */
            visir_interpolate_rejected(avimg, &pbpm, &ndata);
        }
        cut_aqu_illuminated(&avimg);
        visir_imglist_append(range, avimg, plist);
    }

    cpl_free(pbpm);
    cpl_imagelist_delete(av);
    *start = iext + 1;

    /* we don't need the input in the page cache anymore
     * TODO: we might not want to do this when called from reflex */
    if (visir_imglist_get_size(range) > 1) {
        /* approximation assuming non padded header and all extensions have
         * similar compression, take first extension as primary has no data */
        size_t nbytes = visir_get_nbytes_plist(visir_imglist_get_data(range, 1));
        visir_drop_cache(filename, 0, nbytes * (*start));
    }

    return range;
}

typedef struct {
    double x;
    double y;
    cpl_size window;
    int sign;
    /* weight of beam image compared to the original input beams (4 or 2)
     * in parallel mode one beam is the sum of two input beams -> weight = 2
     * parallel mode images in util_qc */
    float weight;
    int type;
} beam_info;


static cx_list *
parse_beampos_cmd(const char * bpstr)
{
    char *str, *token, *istr = cpl_strdup(bpstr);
    cx_list * bpl = cx_list_new();

    for (str = istr; ; str = NULL) {
        beam_info * bi;
        double w = 0;
        int ret;

        token = strtok(str, ";");
        if (token == NULL)
            break;

        bi = cpl_calloc(1, sizeof(beam_info));
        bi->type = cx_list_size(bpl);
        cx_list_push_back(bpl, bi);

        bi->weight = 1;
        if (strncmp(token, "pos", 3) == 0)
            bi->sign=1;
        else if (strncmp(token, "neg", 3) == 0)
            bi->sign=-1;
        else
            error_if(1, CPL_ERROR_ILLEGAL_INPUT,
                     "Invalid beam sign: %s", bpstr);

        ret = sscanf(token+4, "%lf,%lf,%lf", &bi->x, &bi->y, &w);
        if ((ret != 2 && ret != 3) || bi->x < 0 || bi->y < 0 || w < 0)
            error_if(1, CPL_ERROR_ILLEGAL_INPUT,
                     "Invalid beam position: %s", bpstr);

        if (ret == 3)
            bi->window = ceil(w);
        else
            bi->window = 0;
    }

    error_if(cx_list_size(bpl) < 1, CPL_ERROR_ILLEGAL_INPUT,
             "Invalid beam position: %s", bpstr);
    {
        beam_info * pos = NULL;
        int npos = 0, nneg = 0;
        FOR_EACH_T(beam_info * bi, bpl) {
            if (bi->sign > 0) {
                pos = bi;
                npos++;
            }
            else
                nneg--;
        }
        /* give positive beam in parallel case double weight */
        if (nneg == 2 && npos == 1)
            pos->weight *= 2.;
    }
    end_skip;

    cpl_free(istr);

    return bpl;
}

static cx_list *
get_beam_positions(cpl_image * average, const cpl_parameterlist * parlist,
                   cpl_propertylist * plist)
{
    cpl_errorstate cleanstate = cpl_errorstate_get();
    cpl_propertylist * qclist = cpl_propertylist_new();
    cpl_image * inverse = cpl_image_duplicate(average);
    double x[4], y[4];
    const cpl_boolean chop_on =
        cpl_propertylist_get_bool(plist, VISIR_PFITS_BOOL_CHOP_STATUS);
    cx_list * beamposl = cx_list_new();
    int n;

    skip_if(0);

    cpl_image_multiply_scalar(inverse, -1);

    visir_chopnod_mode mode =
        visir_img_find_beam(qclist, average, inverse,
                            plist, parlist, RECIPE_STRING,
                            x, y);
    if (cpl_error_get_code() != CPL_ERROR_NONE) {
        irplib_error_recover(cleanstate, "Could not detect objects,"
                             " using full image. Photometry "
                             "will not work without object positions.");
        beam_info * bi = cpl_malloc(sizeof(beam_info));
        const cpl_size nx = cpl_image_get_size_x(average);
        const cpl_size ny = cpl_image_get_size_y(average);
        bi->x = nx / 2;
        bi->y = ny / 2;
        bi->window = CX_MAX(nx, ny);
        bi->sign = 1;
        bi->weight = 1;
        bi->type = VISIR_PLANE_UNDEFINED;
        cx_list_push_back(beamposl, bi);
    }
    else {

    if (mode == VISIR_CHOPNOD_PERPENDICULAR)
        n = 4;
    else if (mode == VISIR_CHOPNOD_PARALLEL)
        n = 3;
    else
        error_if(1, CPL_ERROR_UNSUPPORTED_MODE, "Unsupported chop mode");

    if (chop_on == CPL_FALSE)
        n = 2;

    for (int i = 0; i < n; i++) {
        beam_info * bi = cpl_calloc(1, sizeof(beam_info));
        bi->x = x[i];
        bi->y = y[i];
        /* the first are always the positive apertures */
        if (n == 4 || n == 2) {
            bi->sign = i < n / 2 ? 1 : -1;
            bi->weight = 1.;
        }
        else {
            bug_if(n != 3);
            /* give positive beam in parallel case double weight */
            bi->sign = i < 1 ? 1 : -1;
            bi->weight = i < 1 ? 2. : 1.;
        }
        bi->type = i;
        cx_list_push_back(beamposl, bi);
    }
    }

    end_skip;
    cpl_image_delete(inverse);
    cpl_propertylist_delete(qclist);

    return beamposl;
}

static cx_list *
cut_image(const cpl_image * img, const cpl_propertylist * plist,
          const cx_list * beampos, cpl_size window)
{
    cx_list * l = cx_list_new();

    FOR_EACH_T(const beam_info * bi, beampos) {
        const cpl_size win = bi->window > 0 ? bi->window : window;

        visir_plane * pl = visir_plane_new();
        bug_if(0);

        pl->plist = plist ?
            cpl_propertylist_duplicate(plist) : cpl_propertylist_new();

        window_extract_p(pl, img, bi->x, bi->y, win);
        skip_if(pl->img == NULL);

        pl->type = bi->type;

        if (bi->sign != 1)
            cpl_image_multiply_scalar(pl->img, bi->sign);

        cx_list_push_back(l, pl);
    }

    end_skip;

    return l;
}


static void
generate_statistics(visir_plane * pl)
{
    cpl_size mx, my;
    cpl_errorstate prestate = cpl_errorstate_get();
    cpl_stats * stat =
        cpl_stats_new_from_image(pl->img, CPL_STATS_MAD |
                                 CPL_STATS_MEDIAN | CPL_STATS_MAXPOS |
                                 CPL_STATS_MAX | CPL_STATS_MIN |
                                 CPL_STATS_STDEV);
    pl->stdev = cpl_stats_get_stdev(stat);
    pl->median = cpl_stats_get_median(stat);
    pl->mad = cpl_stats_get_mad(stat);
    pl->max = cpl_stats_get_max(stat);
    pl->min = cpl_stats_get_min(stat);
    mx = cpl_stats_get_max_x(stat);
    my = cpl_stats_get_max_y(stat);
    if (cpl_image_get_fwhm(pl->img, mx, my,
                           &pl->fwhmx, &pl->fwhmy) != CPL_ERROR_NONE) {
        pl->fwhmx = 0;
        pl->fwhmy = 0;
        cpl_msg_warning(cpl_func, "Could not determine fwhm.");
        cpl_errorstate_set(prestate);
    }

    cpl_stats_delete(stat);
}


static cx_list *
handle_chunk(visir_imglist * chunk, const visir_plane * template,
             const cx_list * beampos,
             const visir_shift_method method,
             const stats_limits * limits)
{
    const cpl_size n = visir_imglist_get_size(chunk);
    cpl_propertylist * mplist = visir_imglist_get_mplist(chunk);
    double pthrow = visir_pfits_get_chop_pthrow(mplist);
    cx_list * res = cx_list_new();

    /* prepare the fft transformed template, one per beam */
    visir_fftx_cache * fft_cache[MAX_BEAMS];
    for (size_t i = 0; i < MAX_BEAMS; i++) {
        fft_cache[i] = visir_new_fftx_cache();
    }
    if (n > 0) {
        cx_list * beams = cut_image(visir_imglist_get_img(chunk, 0),
                                    NULL, beampos, pthrow);
        skip_if(0);
        for (cpl_size j = 0, m = cx_list_size(beams); j < m; j++) {
            visir_plane * pl = cx_list_pop_front(beams);
            visir_fftxcorrelate(template->img, pl->img, CPL_TRUE,
                                NULL, NULL, NULL, fft_cache[j]);
            visir_plane_delete(pl);
        }
        cx_list_delete(beams);
    }

    skip_if(0);

    /* tasks are used instead of a parallel for to yield resources to the
     * reader thread if necessary (e.g. to decompress data)
     * with 1k x 1k burst data the handling is already starved for data
     * with ~8 threads */
    for (cpl_size i = 0; i < n; i++)
OMP3_PRAGMA(omp task firstprivate(i))
        {
        cx_list * beams = cut_image(visir_imglist_get_img(chunk, i),
                                    visir_imglist_get_data(chunk, i),
                                    beampos, pthrow);

        for (cpl_size j = 0, m = cx_list_size(beams); j < m; j++) {
            double xshift = 0, yshift = 0, max_cor = 1;
            visir_plane * pl = cx_list_pop_front(beams);

            if (limits->min_cor > 0 || method == VISIR_SHIFT_XCORRELATE) {
                /* FIXME: cropping around beampos might improve robustness */
                visir_fftxcorrelate(template->img, pl->img, CPL_TRUE,
                                    &xshift, &yshift, &max_cor, fft_cache[j]);
                xshift = template->x + xshift;
                yshift = template->y + yshift;
            }

            if (method == VISIR_SHIFT_BRIGHTEST) {
                cpl_size ixshift, iyshift;
                double xsub, ysub;
                cpl_image_get_maxpos(pl->img, &ixshift, &iyshift);
                visir_get_subpixel_maxpos(pl->img, ixshift, iyshift,
                                          &xsub, &ysub);
                xshift = ixshift + xsub;
                yshift = iyshift + ysub;
            }

            cpl_msg_debug(cpl_func, "%4s: correlation %5.3f shift %5.2f %5.2f",
                         type2string[pl->type], max_cor, xshift, yshift);

            pl->seqnb = i;
            pl->xshift = xshift - pl->x;
            pl->yshift = yshift - pl->y;
            pl->correlation = max_cor;
            /* with shift the wcs are not same as the template */
            pl->crval1 = template->crval1;
            pl->crval2 = template->crval2;

            generate_statistics(pl);
OMP_PRAGMA(omp critical(chunk_lock))
            cx_list_push_back(res, pl);
        }

        cx_list_delete(beams);
    }
OMP3_PRAGMA(omp taskwait)

#ifdef _OPENMP
    /* a priority queue or per thread lists would be more efficient
     * but time spent here is negligible */
    cx_list_sort(res, (cx_compare_func)pl_compare_seqnb);
#endif

    end_skip;
    
    for (size_t i = 0; i < sizeof(fft_cache)/sizeof(fft_cache[0]); i++)
        visir_delete_fftx_cache(fft_cache[i]);

    return res;
}

static visir_plane *
improve_template(const cx_list *imgs, const stats_limits * limits)
{
    cpl_imagelist * l = cpl_imagelist_new();
    visir_plane * template = visir_plane_new();
    cx_list_iterator it = cx_list_begin(imgs);
    const cx_list_iterator end = cx_list_end(imgs);
    cpl_size nx = 0;
    cpl_size ny = 0;

    skip_if(cx_list_empty(imgs));

    while (it != end) {
        visir_plane * pl  = cx_list_get(imgs, it);
        cpl_image * img = cpl_image_duplicate(pl->img);
        nx = nx == 0 ? cpl_image_get_size_x(img) : nx;
        ny = ny == 0 ? cpl_image_get_size_y(img) : ny;

        /* sizes of different beams can diverge if they are close to the
         * detector edge */
        if (cpl_image_get_size_x(img) != nx ||
            cpl_image_get_size_y(img) != ny) {
            it = cx_list_next(imgs, it);
            continue;
        }

        // FIXME: use cpl_geom_img_offset_saa for subpixel shifting
        if (is_image_bad(pl, limits) == CPL_FALSE)
            cpl_image_shift(img, -visir_round_to_int(pl->xshift),
                            -visir_round_to_int(pl->yshift));

        cpl_imagelist_set(l, img, cpl_imagelist_get_size(l));

        it = cx_list_next(imgs, it);
        skip_if(0);
    }

    {
        /* average and shrink it a bit around the beam for better correlation*/
        cpl_image * tmp = visir_parallel_median_collapse(l);
        visir_plane * firstpl = cx_list_get(imgs, cx_list_begin(imgs));
        template->x = firstpl->x;
        template->y = firstpl->y;
        window_extract_p(template, tmp, firstpl->x, firstpl->y,
                         cpl_image_get_size_x(tmp) / 2);
        cpl_image_delete(tmp);
    }

    end_skip;
    cpl_imagelist_delete(l);

    return template;
}

static cpl_error_code
save_images(cx_list * imgs, const cpl_propertylist * mplist,
            const cpl_parameterlist * parlist, const int iframe,
            const stats_limits * limits)
{
    const cpl_boolean noshift =
        irplib_parameterlist_get_bool(parlist, PACKAGE,
                                      RECIPE_STRING, "no-shift");
    const cpl_boolean noreject =
        irplib_parameterlist_get_bool(parlist, PACKAGE,
                                      RECIPE_STRING, "no-reject");
    const int naverage =
        irplib_parameterlist_get_int(parlist, PACKAGE,
                                     RECIPE_STRING, "naverage");

    for (cpl_size j = 0, n = cx_list_size(imgs); j < n; j++) {
        char buffer[128];
        visir_plane * pl = cx_list_pop_front(imgs);

        cpl_boolean bad = is_image_bad(pl, limits);
        cpl_type save_type = CPL_TYPE_FLOAT;

        if (noreject == CPL_FALSE && bad) {
            visir_plane_delete(pl);
            continue;
        }

        if (pl->max <= CX_MAXSHORT && pl->min >= CX_MINSHORT &&
            naverage == 1 && cpl_propertylist_get_int(pl->plist, "BITPIX") > 0)
            save_type = CPL_TYPE_SHORT;

        cpl_propertylist_copy_property_regexp(pl->plist, mplist, "^("
                                              IRPLIB_PFITS_WCS_REGEXP ")$",
                                              CPL_FALSE);

        /* don't correct for shift if the stats are bad in no-reject mode */
        if (noshift || (noreject && bad)) {
            cpl_propertylist_append_double(pl->plist, "CRPIX1", pl->x);
            cpl_propertylist_append_double(pl->plist, "CRPIX2", pl->y);
        } else {
            cpl_propertylist_append_double(pl->plist, "CRPIX1",
                                           pl->x + pl->xshift);
            cpl_propertylist_append_double(pl->plist, "CRPIX2",
                                           pl->y + pl->yshift);
        }
        cpl_propertylist_append_double(pl->plist, "CRVAL1", pl->crval1);
        cpl_propertylist_append_double(pl->plist, "CRVAL2", pl->crval2);

        cpl_propertylist_append_double(pl->plist, "ESO QC CORR",
                                       pl->correlation);

        cpl_propertylist_append_double(pl->plist, "ESO QC MEDIAN",
                                       pl->median);
        cpl_propertylist_append_double(pl->plist, "ESO QC MAD",
                                       pl->mad);
        cpl_propertylist_append_double(pl->plist, "ESO QC STDEV",
                                       pl->stdev);
        cpl_propertylist_append_double(pl->plist, "ESO QC FWHMX",
                                       pl->fwhmx);
        cpl_propertylist_append_double(pl->plist, "ESO QC FWHMY",
                                       pl->fwhmy);

        sprintf(buffer, RECIPE_STRING"_%03d_%s"CPL_DFS_FITS,
                iframe+1, type2string[pl->type]);

        /* Update the headers before saving */
        visir_dfs_update_header(pl->plist);
        cpl_image_save(pl->img, buffer, save_type,
                       pl->plist, CPL_IO_EXTEND);
        nbytes_save += cpl_image_get_size_x(pl->img) *
            cpl_image_get_size_y(pl->img) * cpl_type_get_sizeof(save_type);

        visir_plane_delete(pl);
        skip_if(0);
    }

    end_skip;

    return cpl_error_get_code();
}


#ifdef VISIR_OMP3
static cpl_error_code
read_data(visir_queue * readq, cpl_propertylist * mplist,
          cpl_frame * frame, const cpl_size chunksize,
          const cpl_size naverage, cpl_mask * bpm, cpl_size * cur)
{
    visir_imglist * range = NULL;
    do {
        double t = cpl_test_get_walltime(), t2;
        range = load_range(frame, cur, chunksize,
                           naverage, bpm);
        skip_if(0);

        if (visir_imglist_get_size(range) == 0) {
            visir_imglist_delete(range, (visir_free)cpl_propertylist_delete);
            break;
        }

        visir_imglist_set_mplist(range, mplist);
        t2 = cpl_test_get_walltime();
        t_loading += t2 - t;
        skip_if(visir_queue_put(readq, range));
        t_loading_blocked += cpl_test_get_walltime() - t2;
    } while (visir_imglist_get_size(range) != 0);

    cpl_msg_info(cpl_func,
                 "Loading thread done, %.3g MB/s (%.3g MB/s uncompressed)",
                 (nbytes_load / 1024. / 1024.) / t_loading,
                 (nbytes_load_eff / 1024. / 1024.) / t_loading);

    end_skip;
    /* signal end of data */
    visir_queue_put(readq, NULL);

    cpl_error_code err = cpl_error_get_code();
    if (err != CPL_ERROR_NONE)
        visir_queue_set_error(readq, err);

    return err;
}

static cpl_error_code
handle_data(visir_queue * readq, visir_queue * writeq,
            const visir_plane * template,
            cx_list * beampos, const visir_shift_method method,
            const stats_limits * limits)
{
    visir_imglist * range = NULL;
    do {
        double t = cpl_test_get_walltime(), t2;
        range = visir_queue_pop(readq);
        t2 = cpl_test_get_walltime();
        t_handling_blocked_in += t2 - t;
        if (range == NULL)
            break;
        cx_list * res =  handle_chunk(range, template, beampos,
                                      method, limits);

        visir_imglist_delete(range, (visir_free)cpl_propertylist_delete);

        t = cpl_test_get_walltime();
        t_handling += t - t2;
        skip_if(visir_queue_put(writeq, res));
        t_handling_blocked_out += cpl_test_get_walltime() - t;
    } while(range != NULL);

    end_skip;
    /* signal end of data */
    visir_queue_put(writeq, NULL);

    cpl_error_code err = cpl_error_get_code();
    if (err != CPL_ERROR_NONE) {
        visir_queue_set_error(writeq, err);
        visir_queue_set_error(readq, err);
    }

    return err;
}


static cpl_error_code
write_data(visir_queue * writeq, cpl_propertylist * mplist,
           const cpl_parameterlist * parlist,
           const int iframe, const stats_limits * limits,
           const cpl_size chunksize, const cpl_size next)
{
    cx_list * res = NULL;
    cpl_size count = 1;
    do {
        double t = cpl_test_get_walltime(), t2;
        res = visir_queue_pop(writeq);
        t2 = cpl_test_get_walltime();
        t_writing_blocked += t2 - t;
        if (res == NULL)
            break;

        save_images(res, mplist, parlist, iframe, limits);
        planelist_delete(res);
        t_writing += cpl_test_get_walltime() - t2;
        skip_if(0);
        cpl_msg_info(cpl_func, "done: %d/%d",
                     (int)CX_MIN((count * chunksize), next + 1),
                     (int)next + 1);
        count++;
    } while(res != NULL);

    cpl_msg_info(cpl_func, "Writing thread done, %.3g MB /s",
                 (nbytes_save / 1024. / 1024.) / t_writing);

    end_skip;

    cpl_error_code err = cpl_error_get_code();
    if (err != CPL_ERROR_NONE)
        visir_queue_set_error(writeq, err);

    return err;
}
#endif

/*----------------------------------------------------------------------------*/
/**
  @brief    Process one frame
  @param    framelist       The frameset to append products to
  @param    i               The frame to process (0 for first)
  @param    parlist         The parameters list
  @return   CPL_ERROR_NONE iff OK, otherwise the relevant CPL error code.
  @see visir_detect_shift_image()
 */
/*----------------------------------------------------------------------------*/
static
cpl_error_code visir_util_detect_shift_one(cpl_frameset * framelist,
                                           irplib_framelist * rawframes,
                                           int iframe,
                                           const cpl_parameterlist * parlist,
                                           cpl_mask * bpm,
                                           visir_plane ** ptemplate)
{
    cpl_frameset * products = cpl_frameset_new();
    visir_plane * template = NULL;
    cpl_image * av = NULL;
    cpl_boolean bfirst = CPL_FALSE;
    cx_list * res = NULL;
    cpl_frame * frame  = irplib_framelist_get(rawframes, iframe);
    const char * filename = cpl_frame_get_filename(frame);
    const char * fn_template =
        irplib_parameterlist_get_string(parlist, PACKAGE,
                                     RECIPE_STRING, "template");
    const int naverage =
        irplib_parameterlist_get_int(parlist, PACKAGE,
                                     RECIPE_STRING, "naverage");
    cpl_size cur = 0;
    const cpl_size chunksize = 200 * naverage;
    cpl_frame_set_group(frame, CPL_FRAME_GROUP_RAW);

    /* load the first chunk for the template */
    cpl_size next;
    cpl_propertylist * mplist = cpl_propertylist_load(filename, 0);
    {
        visir_data_type datatype;
        visir_get_data_type(frame, mplist, &datatype, &next);
        is_aqu_data = visir_data_is_aqu(datatype);
    }
    visir_imglist * range = load_range(frame, &cur, chunksize,
                                       naverage, bpm);
    visir_imglist_set_mplist(range, mplist);
    double pthrow = visir_pfits_get_chop_pthrow(mplist);
    const char * sbeampos =
        irplib_parameterlist_get_string(parlist, PACKAGE,
                                        RECIPE_STRING, "beampos");
    cx_list * beampos = NULL;

    skip_if(range == NULL);

    if (cpl_frameset_find(framelist, VISIR_UTIL_REPACK_MEAN_PROCATG)) {
        cpl_frame * fn = cpl_frameset_find(framelist, VISIR_UTIL_REPACK_MEAN_PROCATG);
        av = cpl_image_load(cpl_frame_get_filename(fn),
                            CPL_TYPE_FLOAT, 0,0);
        cut_aqu_illuminated(&av);
        skip_if(0);

        if (bpm) {
            cpl_image * ibpm = cpl_image_new_from_mask(bpm);
            cut_aqu_illuminated(&ibpm);
            cpl_mask * nbpm = cpl_mask_threshold_image_create(ibpm, 0.1, DBL_MAX);

            cpl_image_reject_from_mask(av, nbpm);

            cpl_image_delete(ibpm);
            cpl_mask_delete(nbpm);

            skip_if(visir_interpolate_rejected(av, NULL, NULL));
        }
    }
    else {
        const cpl_imagelist * tmpl = visir_imglist_get_imglist(range);
        av = visir_parallel_median_collapse(tmpl);
        skip_if(0);
    }

    if (!visir_str_par_is_empty(sbeampos)) {
        beampos = parse_beampos_cmd(sbeampos);
        /* adapt input beampos to unilluminated cut on aqu data */
        if (is_aqu_data) {
            FOR_EACH_T(beam_info * bi, beampos) {
                bi->x -= cpl_image_get_size_x(av) == 1024 - 22 - (1024 - 878 - 1) ? 22 : 0;
                bi->y -= cpl_image_get_size_y(av) == 1024 - 92 - (1024 - 948 - 1) ? 92 : 0;
            }
        }
    }
    else
        beampos = get_beam_positions(av, parlist, mplist);

    skip_if(0);

    error_if(cx_list_size(beampos) > MAX_BEAMS, CPL_ERROR_ILLEGAL_INPUT,
             "Too many beams, maximum supported: %d", MAX_BEAMS);
    error_if(ptemplate == NULL, CPL_ERROR_NULL_INPUT, "Bug: ptemplate == NULL");

    /* when beam positions is determined once for all datasets we need to adapt
     * for the jitter */
    if ((!visir_str_par_is_empty(sbeampos) ||
        cpl_frameset_find(framelist, VISIR_UTIL_REPACK_MEAN_PROCATG)) &&
        cpl_propertylist_has(mplist, VISIR_DRS_CUMOFFSETXA)) {
        double dxa = cpl_propertylist_get_double(mplist, VISIR_DRS_CUMOFFSETXA);
        double dya = cpl_propertylist_get_double(mplist, VISIR_DRS_CUMOFFSETYA);
        double dxb = cpl_propertylist_get_double(mplist, VISIR_DRS_CUMOFFSETXB);
        double dyb = cpl_propertylist_get_double(mplist, VISIR_DRS_CUMOFFSETYB);
        FOR_EACH_T(beam_info * bi, beampos) {
            if (bi->type == VISIR_PLANE_AOFF || bi->type == VISIR_PLANE_AON ||
                bi->type == VISIR_PLANE_UNDEFINED) {
                bi->x += dxa;
                bi->y += dya;
            }
            else if (bi->type == VISIR_PLANE_BOFF || bi->type == VISIR_PLANE_BON) {
                bi->x += dxb;
                bi->y += dyb;
            }
        }
    }

    FOR_EACH_T(const beam_info * bi, beampos) {
        double window = bi->window > 0 ? bi->window : pthrow;
        cpl_msg_info(cpl_func, "Beam position: x %5.1f, y %5.1f, window %4.0f,"
                     " sign %2d", bi->x, bi->y, window, bi->sign);
    }

    stats_limits limits;
    limits.min_cor =
        irplib_parameterlist_get_double(parlist, PACKAGE,
                                        RECIPE_STRING, "min-correlation");
    limits.max_shift =
        irplib_parameterlist_get_double(parlist, PACKAGE,
                                        RECIPE_STRING, "max-shift");
    limits.max_mad =
        irplib_parameterlist_get_double(parlist, PACKAGE,
                                        RECIPE_STRING, "max-mad");

    const char * smethod =
        irplib_parameterlist_get_string(parlist, PACKAGE,
                                        RECIPE_STRING, "method");
    visir_shift_method method = VISIR_SHIFT_XCORRELATE;

    if (strncmp(smethod, "xcorrelate", sizeof("xcorrelate")) == 0)
        method = VISIR_SHIFT_XCORRELATE;
    else if (strncmp(smethod, "brightest", sizeof("brightest")) == 0)
        method = VISIR_SHIFT_BRIGHTEST;
    else
        cpl_msg_warning(cpl_func, "Unknown method %s. Using xcorrelate",
                        smethod);

    /* FIXME: add option for template per file but make sure
     * center is the same in all versions */
    if (*ptemplate != NULL)
        template = *ptemplate;
    else if (!visir_str_par_is_empty(fn_template)) {
        cpl_msg_info(cpl_func, "Loading %s as template", fn_template);
        template = visir_plane_new();
        template->plist = cpl_propertylist_load(fn_template, 0);
        template->img = cpl_image_load(fn_template, CPL_TYPE_UNSPECIFIED, 0, 0);
        skip_if(template->img == NULL || template->plist == NULL);
        if (cpl_propertylist_has(template->plist, "CRPIX1") &&
            cpl_propertylist_has(template->plist, "CRPIX2") &&
            cpl_propertylist_has(template->plist, "CRVAL1") &&
            cpl_propertylist_has(template->plist, "CRVAL2")) {
            template->x = cpl_propertylist_get_double(template->plist, "CRPIX1");
            template->y = cpl_propertylist_get_double(template->plist, "CRPIX2");
            template->crval1 = cpl_propertylist_get_double(template->plist, "CRVAL1");
            template->crval2 = cpl_propertylist_get_double(template->plist, "CRVAL2");
        }
        else {
            error_if(1, CPL_ERROR_ILLEGAL_INPUT,
                     "Input template must have WCS coordinates");

        }
        *ptemplate = template;
        bfirst = CPL_TRUE;
    }
    else {
        template = visir_plane_new();
        cpl_msg_info(cpl_func, "Using average as template");
        const cpl_boolean noshift =
            irplib_parameterlist_get_bool(parlist, PACKAGE,
                                          RECIPE_STRING, "no-shift");
        beam_info * bi = cx_list_get(beampos, cx_list_begin(beampos));
        /* crops a little extra to get less background in the template */
        const cpl_size window = bi->window > 0 ?
            bi->window * 2 / 3. : pthrow * 2 / 3.;
        window_extract_p(template, av, bi->x, bi->y, window);
        cpl_image_multiply_scalar(template->img, bi->sign);

        if (!noshift &&
            (limits.min_cor > 0 || method == VISIR_SHIFT_XCORRELATE)) {
            /* improve the template by doing a coarse shift correction */
            stats_limits lim;
            lim.min_cor = 0;
            lim.max_shift = 5000;
            lim.max_mad = 0;
OMP_PRAGMA(omp parallel)
OMP_PRAGMA(omp single)
            res =  handle_chunk(range, template, beampos, method, &lim);
            visir_plane_delete(template);
            template = improve_template(res, &limits);

            planelist_delete(res);
            res = NULL;
        }
        template->plist = cpl_propertylist_new();
        cpl_propertylist_append_double(template->plist, "CRPIX1", template->x);
        cpl_propertylist_append_double(template->plist, "CRPIX2", template->y);
        *ptemplate = template;

        /* get world coordinate of template center */
        cpl_wcs * wcs = cpl_wcs_new_from_propertylist(mplist);
        cpl_matrix * from = cpl_matrix_new(1, 2);
        cpl_matrix * to = NULL;
        cpl_matrix_set(from, 0, 0, bi->x);
        cpl_matrix_set(from, 0, 1, bi->y);
        cpl_array * status = NULL;
        skip_if(cpl_wcs_convert(wcs, from, &to, &status, CPL_WCS_PHYS2WORLD));
        template->crval1 = cpl_matrix_get(to, 0, 0);
        template->crval2 = cpl_matrix_get(to, 0, 1);
        cpl_wcs_delete(wcs);
        cpl_matrix_delete(from);
        cpl_matrix_delete(to);
        cpl_array_delete(status);

        bfirst = CPL_TRUE;
    }
    skip_if(0);

    if (bpm) {
        int i  = 0;
        cpl_frame * frm = cpl_frameset_find(framelist, VISIR_CALIB_STATIC_MASK);
        cpl_image * ibpm = cpl_image_new_from_mask(bpm);
        cut_aqu_illuminated(&ibpm);
        cx_list * cbpml = cut_image(ibpm, NULL, beampos, pthrow);
        cpl_frameset  * usedframes = cpl_frameset_new();
        cpl_frameset_insert(usedframes, cpl_frame_duplicate(frm));

        FOR_EACH(it, cbpml) {
            char buffer[128];
            cpl_image * img = ((visir_plane*)cx_list_get(cbpml, it))->img;
            cpl_image_abs(img);
            sprintf(buffer, RECIPE_STRING"_bpm_%03d_%s"CPL_DFS_FITS,
                    iframe+1, type2string[i++]);
            irplib_dfs_save_image(products, parlist,
                                          usedframes, img, CPL_BPP_8_UNSIGNED,
                                          RECIPE_STRING,
                                          VISIR_CALIB_BPM,
                                          NULL, NULL, visir_pipe_id,
                                          buffer);
        }
        planelist_delete(cbpml);
        cpl_image_delete(ibpm);
        cpl_frameset_delete(usedframes);
    }

    skip_if(0);
    
    if (bfirst) {
        char buffer[128];
        cpl_frameset  * usedframes = cpl_frameset_new();
        sprintf(buffer, RECIPE_STRING"_template_%03d"CPL_DFS_FITS,
                iframe+1);

        cpl_frameset_insert(usedframes, cpl_frame_duplicate(frame));
        skip_if(irplib_dfs_save_image(products, parlist,
                                      usedframes, template->img, CPL_TYPE_FLOAT,
                                      RECIPE_STRING,
                                      VISIR_UTIL_DETECT_SHIFT_TEMPLATE_PROCATG,
                                      template->plist, NULL, visir_pipe_id,
                                      buffer));
        cpl_frameset_delete(usedframes);
    }

    FOR_EACH_T(const beam_info * bi, beampos) {
        char buffer[128];
        cpl_frameset  * usedframes = cpl_frameset_new();
        cpl_propertylist * _mplist = cpl_propertylist_new();
        cpl_propertylist_copy_property_regexp(_mplist, mplist, ".*", 0);

        if (bi->type != VISIR_PLANE_UNDEFINED)
            sprintf(buffer, "%01d", bi->type);
        else
            sprintf(buffer, "UNDEFINED");
        cpl_propertylist_append_string(_mplist, "ESO QC BEAMID", buffer);
        cpl_propertylist_append_float(_mplist, "ESO DRS IMGWGT", bi->weight);
        cpl_propertylist_append_double(_mplist, "ESO QC BEAMX", bi->x);
        cpl_propertylist_append_double(_mplist, "ESO QC BEAMY", bi->y);

        sprintf(buffer, RECIPE_STRING"_%03d_%s"CPL_DFS_FITS,
                iframe+1, type2string[bi->type]);

        cpl_frameset_insert(usedframes, cpl_frame_duplicate(frame));
        irplib_dfs_save_propertylist(products, parlist,
                                     usedframes, RECIPE_STRING,
                                     VISIR_UTIL_DETECT_SHIFT_PROCATG,
                                     _mplist, NULL, visir_pipe_id, buffer);
        cpl_frameset_delete(usedframes);
        cpl_propertylist_delete(_mplist);
        skip_if(0);
    }

    cpl_msg_info(cpl_func, "Working on frame %d", iframe);
    /* first range already loaded
     * process it and load next range in parallel*/
#ifdef VISIR_OMP3
    cpl_error_code serr, werr, lerr;
    serr = werr = lerr = CPL_ERROR_NONE;
    visir_queue * readq = visir_queue_init(3);
    visir_queue * writeq = visir_queue_init(3);
    visir_queue_put(readq, range);
    range = NULL;
    /* Start a thread to read data, a thread to handle the data and a thread to
     * write it. Interleaving of reads and write not a big issue as  modern
     * file systems will delay the actual disk write for a very long time.
     * The reader and worker parallize by openmp tasks so no nested
     * parallization is required.
     * While the scheduling is more expensive than the simple static of the
     * nested case, this has the advantage that when the worker thread is
     * stalled for data the reader thread can take all resources it needs to
     * load new data as fast as possible. This is very relevant when reading
     * cached or compressed data which can be CPU bound on fast disks */

    /* oversubscribe by two so the queues can't deadlock */
    int nthreads = CX_MAX(visir_get_num_threads(CPL_FALSE) + 2, 3);
#pragma omp parallel num_threads(nthreads)
    {
#pragma omp single nowait
        lerr = read_data(readq, mplist, frame, chunksize, naverage, bpm, &cur);
#pragma omp single nowait
        serr = handle_data(readq, writeq, template, beampos, method, &limits);
#pragma omp single nowait
        werr = write_data(writeq, mplist, parlist, iframe, &limits,
                          chunksize, next);
    }
    visir_queue_delete(readq);
    visir_queue_delete(writeq);

    error_if(serr, serr, "Failed to save data");
    error_if(werr, werr, "Failed to handle data");
    error_if(lerr, lerr, "Failed to load data");

    cpl_msg_info(cpl_func, "Time spent loading data:                   %3.3gs",
                 t_loading);
    cpl_msg_info(cpl_func, "Time loading thread was blocked on output: %3.3gs",
                 t_loading_blocked < 1e-3 ? 0. : t_loading_blocked);
    cpl_msg_info(cpl_func, "Time working thread was blocked on input:  %3.3gs",
                 t_handling_blocked_in < 1e-3 ? 0. : t_handling_blocked_in);
    cpl_msg_info(cpl_func, "Time working on data:                      %3.3gs",
                 t_handling);
    cpl_msg_info(cpl_func, "Time working thread was blocked on output: %3.3gs",
                 t_handling_blocked_out < 1e-3 ? 0. : t_handling_blocked_out);
    cpl_msg_info(cpl_func, "Time saving thread was blocked on input:   %3.3gs",
                 t_writing_blocked < 1e-3 ? 0. : t_writing_blocked);
    cpl_msg_info(cpl_func, "Time spent saving data:                    %3.3gs",
                 t_writing);
#else
    /* without omp 3.0 just do it synchrounous */
    do {
        res =  handle_chunk(range, template, beampos,
                            method, &limits);

        skip_if(0);
        cpl_msg_info(cpl_func, "done: %d/%d",
                     (int)cur - 1, (int)next + 1);
        visir_imglist_delete(range,
                             (visir_free)cpl_propertylist_delete);
        skip_if(0);

        range = load_range(frame, &cur, chunksize,
                           naverage, bpm);
        skip_if(0);
        visir_imglist_set_mplist(range, mplist);

        save_images(res, mplist, parlist, iframe, &limits);
        planelist_delete(res);
        res = NULL;
        skip_if(0);
    } while (visir_imglist_get_size(range) != 0);
#endif

    /* only output products that have accepted images */
    FOR_EACH_FRAMESET_C(frm, products) {
        if (cpl_frame_get_nextensions(frm) > 0 ||
            strcmp(cpl_frame_get_tag(frm),
                   VISIR_UTIL_DETECT_SHIFT_TEMPLATE_PROCATG) == 0 ||
            strcmp(cpl_frame_get_tag(frm),
                   VISIR_CALIB_BPM) == 0 ||
            cpl_frame_get_group(frm) != CPL_FRAME_GROUP_PRODUCT) {
            cpl_frameset_insert(framelist, cpl_frame_duplicate(frm));
        }
    }

    end_skip;
    planelist_delete(res);
    cpl_image_delete(av);
    cx_list_destroy(beampos, (visir_free)cpl_free);
    visir_imglist_delete(range, (visir_free)cpl_propertylist_delete);
    cpl_propertylist_delete(mplist);
    cpl_frameset_delete(products);

    return cpl_error_get_code();
}

