/* $Id: casu_defringe.c,v 1.4 2015/11/18 20:05:17 jim Exp $
 *
 * This file is part of the CASU Pipeline utilities
 * Copyright (C) 2015 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/*
 * $Author: jim $
 * $Date: 2015/11/18 20:05:17 $
 * $Revision: 1.4 $
 * $Name:  $
 */

/* Includes */

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

#include <math.h>
#include <cpl.h>
#include "casu_mods.h"
#include "casu_utils.h"
#include "casu_fits.h"
#include "casu_stats.h"
#include "casu_mask.h"

#define NTERMAX 15
#define ITERMAX 7
#define OGAP 0.01
#define GAPMIN 0.25
#define CLIPLEV 4.0

#define KFWHM 3.0
#define NFWHM 2

static int casu_defringe_1(casu_fits **infiles, int nimages, casu_fits *fringe,
                           casu_mask *mask, int nbsize, float *scaleout,
                           int *status);

static void kernel_init(int *nkernel, float **skernel);
static float *convolve(float *data, int nx, int ny, int nkernel, 
                       float *skernel);

/**@{*/

/*---------------------------------------------------------------------------*/
/**
    \ingroup casu_modules
    \brief Correct input data to remove fringes

    \par Name:
        casu_defringe
    \par Purpose:
        Correct input data to remove fringes
    \par Description:
        A list of input images are given along with a fringe frame image.
        Each image is analysed to model out any large scale background 
        variation which would bias the fitting procedure if it were left 
        in. The background corrected fringe data is scaled by a series of
        different factors subtracted from the image data. The median absolute
        deviation of each the difference image is worked out and the run of
        MAD with scale factor is examined to see at what point the scale factor
        minimises the MAD. This scale factor is then used to correct the
        input data.
    \par Language:
        C
    \param infiles
        The input data images (overwritten by result).
    \param nimages
        The number of input images
    \param fringes
        The input fringe frames
    \param nfringes
        The number of input fringe frames
    \param mask
        The bad pixel mask
    \param nbsize
        The size of each cell for the background modelling
    \param status
        An input/output status that is the same as the returned values below.
    \retval CASU_OK
        if everything is ok
    \par QC headers:        
        The following QC keywords are written to the infile extension header
        - \b FRINGE_RATIO
            The ratio of the background RMS of the input image before fringe
            correction vs after.
    \par DRS headers:
        The following QC keywords are written to the infile extension header
        - \b FRINGEi
            The ith fringe frame used in defringing
        - \b FRNGSCi
            The scale factor of the ith fringe frame used in correcting 
            the input image
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

extern int casu_defringe(casu_fits **infiles, int nimages, casu_fits **fringes,
                         int nfringes, casu_mask *mask, int nbsize,
                         int *status) {
    float *sig_before,*scale,*idata,med,mad,ratio;
    unsigned char *bpm;
    int i,j,npts;
    char pname1[64],comment1[64],pname2[64],comment2[64];
    cpl_propertylist *ehu;

    /* Inherited status */

    if (*status != CASU_OK)
        return(*status);

    /* Get some workspace to use for working out the background sigmas */

    sig_before = cpl_malloc(nimages*sizeof(float));
    scale = cpl_malloc(nimages*sizeof(float));

    /* Now work out the background sigma of each image */

    bpm = casu_mask_get_data(mask);
    npts = (int)cpl_image_get_size_x(casu_fits_get_image(infiles[0]))*
        (int)cpl_image_get_size_y(casu_fits_get_image(infiles[0]));
    for (i = 0; i < nimages; i++) {
        idata = cpl_image_get_data_float(casu_fits_get_image(infiles[i]));
        casu_medmad(idata,bpm,(long)npts,&med,&mad);
        sig_before[i] = 1.48*mad;
    }

    /* Now do the defringing */

    for (i = 0; i < nfringes; i++) {
        (void)casu_defringe_1(infiles,nimages,fringes[i],mask,nbsize,scale,
                              status);

        /* Create scale factor and fringe name information for headers */

        (void)sprintf(pname1,"ESO DRS FRINGE%d",i+1);
        (void)sprintf(comment1,"Fringe frame # %d",i+1);
        (void)sprintf(pname2,"ESO DRS FRNGSC%d",i+1);
        (void)sprintf(comment2,"Fringe scale # %d",i+1);

        /* Now loop for each image and write these to the headers */

        for (j = 0; j < nimages; j++) {
            ehu = casu_fits_get_ehu(infiles[j]);
            cpl_propertylist_update_string(ehu,pname1,
                                           casu_fits_get_fullname(fringes[i]));
            cpl_propertylist_set_comment(ehu,pname1,comment1);
            cpl_propertylist_update_float(ehu,pname2,scale[i]);
            cpl_propertylist_set_comment(ehu,pname2,comment2);
        }
    }

    /* Now work out the final background sigma and add the ratio of the
       before to after to the QC header of each image */

    for (i = 0; i < nimages; i++) {
        ehu = casu_fits_get_ehu(infiles[i]);
        idata = cpl_image_get_data_float(casu_fits_get_image(infiles[i]));
        casu_medmad(idata,bpm,(long)npts,&med,&mad);
        ratio = sig_before[i]/(1.48*mad);
        cpl_propertylist_update_float(ehu,"ESO QC FRINGE_RATIO",ratio);
        cpl_propertylist_set_comment(ehu,"ESO QC FRINGE_RATIO",
                                     "Ratio RMS before to after defringing");
    }
    freespace(sig_before);
    freespace(scale);
    GOOD_STATUS
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        casu_defringe_1
    \par Purpose:
        Defringe a series of images with a single mean fringe frame
    \par Description:
        This routine defringes a series of images using a single 
        mean fringe frame and can be called repeatedly if several
        different mean frames are to be used.
    \par Language:
        C
    \param infiles
        The input data images (overwritten by result).
    \param nimages
        The number of input images
    \param fringe
        The input fringe frame
    \param mask
        The bad pixel mask
    \param nbsize
        The size of each cell for the background modelling
    \param scaleout
        The scale factor found for each of the input frames.
    \param status
        An input/output status that is the same as the returned values below.
    \retval CASU_OK
        if everything is ok
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static int casu_defringe_1(casu_fits **infiles, int nimages, casu_fits *fringe,
                           casu_mask *mask, int nbsize, float *scaleout,
                           int *status) {
    float frmed,immed,*frdata,*frorig,*wptr,*imdata,*data,scaleth,scalemin;
    float scaleprev,spreadmin,spreadfbest,gap,offset,clipmax,clip,spreadf;
    float scalefound,scale,scalelist[3],diff,spreadlist[3],a,b,c;
    float *frback,*imback,*skernel,*frdatasm,*datasm;
    long npts,ntot;
    int i,iter,nter,k,j,nx,ny,nkernel;
    casu_fits *im;
    unsigned char *bpm;
    cpl_binary *bpmcpl;

    /* Inherited status */

    if (*status != CASU_OK)
        return(*status);

    /* Get BPM */

    nx = cpl_image_get_size_x(casu_fits_get_image(fringe));
    ny = cpl_image_get_size_y(casu_fits_get_image(fringe));
    npts = (long)(nx*ny);
    bpm = casu_mask_get_data(mask);
    bpmcpl = cpl_malloc(npts*sizeof(cpl_binary));
    for (i = 0; i < npts; i++)
        bpmcpl[i] = bpm[i];

    /* Prepare the fringe frame by creating a background map */

    frorig = cpl_image_get_data(casu_fits_get_image(fringe));
    (void)casu_backmap(frorig,bpmcpl,nx,ny,nbsize,&frmed,&frback,status);

    /* Create a fringe frame image that is background corrected and normalised
       to zero median */

    frdata = cpl_malloc(npts*sizeof(float));
    for (i = 0; i < npts; i++)
        frdata[i] = frorig[i] - frback[i];

    /* Convolve the normalised fringe frame to remove any low-level noise
       patterns */

    kernel_init(&nkernel,&skernel);
    frdatasm = convolve(frdata,nx,ny,nkernel,skernel);

    /* Get some workspace */

    wptr = cpl_malloc(npts*sizeof(float));

    /* Now loop for each of the input images */

    for (i = 0; i < nimages; i++) {
        im = infiles[i];

        /* Create a background map and correct for it */

        imdata = cpl_image_get_data_float(casu_fits_get_image(im));
        (void)casu_backmap(imdata,bpmcpl,nx,ny,nbsize,&immed,&imback,status);
        data = cpl_malloc(npts*sizeof(float));
        for (j = 0; j < npts; j++)
            data[j] = imdata[j] - imback[j];

        /* Now smooth the image data */

        datasm = convolve(data,nx,ny,nkernel,skernel);

        /* The overall medians are used as a first guess of the scaling 
           between the two images */

        scaleth = immed/frmed;

        /* Set up some values for tracking the goodness of fit */

        scalemin = 0.0;
        scaleprev = 1.0e6;
        spreadmin = 1.0e3;
        spreadfbest = 1.0e6;
        iter = 0;
        nter = 0;
        gap = scaleth;
        offset = 0.5;
        clipmax = 1.0e3;

        /* Begin the iteration loop */

        while (nter < NTERMAX && iter < ITERMAX && (fabs(offset)*gap > OGAP ||
               gap > GAPMIN)) {
            iter++;
            nter++;

            /* Clip levels */

            clip = min(clipmax,CLIPLEV*1.48*spreadmin);

            /* Speed up convergence if sitting on top of a solution. 
               Slow it down if outside range */

            if (fabs(scalemin - scaleprev) < 0.5*gap) 
                iter += 1;
            scaleprev = scalemin;
            if (fabs(offset) > 0.9)
                iter -= 1;
            gap = scaleth*pow(2.0,(double)(1-iter));

            /* Initialise a few things */

            spreadf = 1.0e6;
            scalefound = 2.0;
 
            /* Do three calculations -- just below, at and just above
               the current best guess scale factor */

            for (k = 0; k < 3; k++) {
                scale = scalemin + gap*(float)(k-1);
                scalelist[k] = scale;
                ntot = 0;
    
                /* Do a scaled subtraction of the fringe from the data */

                for (j = 0; j < npts; j++) {
                    if (bpm[j] == 0) {
                        diff = fabs(datasm[j] - scale*frdatasm[j]);
                        if (diff < clip)
                            wptr[ntot++] = diff;
                    }
                }

                /* Find the MAD */

                spreadlist[k] = casu_med(wptr,NULL,ntot);
                if (spreadlist[k] < spreadf) {
                    spreadf = spreadlist[k]; 
                    scalefound = scale;
                }
            }

            /* Right, how have we done on this iteration? If the spread
               has started to increase then this is the last iteration */

            if (spreadf > spreadfbest) {
                nter = NTERMAX + 1;

            /* Otherwise interpolate to find the best solution */

            } else {
                a = spreadlist[1];
                b = 0.5*(spreadlist[2] - spreadlist[0]);
                c = 0.5*(spreadlist[2] + spreadlist[0] - 2.0*spreadlist[1]);
                offset = max(min((-0.5*b/c),1.0),-1.0);
                spreadmin = a + b*offset + c*offset*offset;
                scalemin = scalelist[1] + offset*gap;

                /* Make sure we're not going for a maximum instead of
                   a minimum */

                if (spreadmin > spreadf) {
                    spreadmin = spreadf;
                    scalemin = scalefound;
                }
            }
 
            /* Define the best spread found so far */

            spreadfbest = min(spreadfbest,spreadf);

        } /* End of iteration loop */

        /* Trap for no refinement */

        if (iter == 0)
            scalemin = scaleth;
    
        /* Subtract the fringe frame now with the defined scale factor */

        for (j = 0; j < npts; j++) 
            imdata[j] -= scalemin*(frorig[j] - frmed);
        scaleout[i] = scalemin;

        /* Tidy */

        freespace(data);
        freespace(datasm);
        freespace(imback);
    }

    /* Do a bit more tidying */

    freespace(frdata);
    freespace(frdatasm);
    freespace(frback);
    freespace(wptr);
    freespace(bpmcpl);
    freespace(skernel);
    GOOD_STATUS
}

static void kernel_init(int *nkernel, float **skernel) {
    int nk2,n,i,j;
    double gsigsq,di,dj;
    float renorm;

    /* Set the kernel size and get some memory */

    *nkernel = (int)(KFWHM*NFWHM + 0.5);
    if (! (*nkernel & 1))
        (*nkernel)++;
    *skernel = cpl_malloc((*nkernel)*(*nkernel)*sizeof(float));
    nk2 = *nkernel/2;

    /* Set the normalisation constants */

    gsigsq = 1.0/(2.0*pow((double)KFWHM/2.35,2.0));
    renorm = 0.0;

    /* Now work out the weights */

    n = -1;
    for (i = -nk2; i <= nk2; i++) {
        di = (double)i;
        di *= gsigsq*di;
        for (j = -nk2; j <= nk2; j++) {
            dj = (double)j;
            dj *= gsigsq*dj;
            n++;
            (*skernel)[n] = (float)exp(-(di+dj));
            renorm += (*skernel)[n];
        }
    }

    /* Now normalise the weights */

    n = -1;
    for (i = -nk2; i <= nk2; i++) {
        for (j = -nk2; j <= nk2; j++) {
            n++;
            (*skernel)[n] /= renorm;
        }
    }
}

static float *convolve(float *data, int nx, int ny, int nkernel, 
                       float *skernel) {
    float *work,*d;
    int nk2,ix,iy,jx,jy,n;

    /* Get some space for the smoothed image */

    work = cpl_calloc(nx*ny,sizeof(float));
    nk2 = nkernel/2;

    /* Do the convolution */

    for (iy = nk2; iy < ny-nk2; iy++) {
        for (ix = nk2; ix < nx-nk2; ix++) {
            n = -1;
            for (jy = iy-nk2; jy <= iy+nk2; jy++) {
                d = data +jy*nx;
                for (jx = ix-nk2; jx <= ix+nk2; jx++) {
                    n++;
                    work[iy*nx+ix] += skernel[n]*d[jx];
                }
            }
        }
    }
    return(work);
}   

/**@}*/

/*

$Log: casu_defringe.c,v $
Revision 1.4  2015/11/18 20:05:17  jim
Removed spurious character that snuck in somehow...

Revision 1.3  2015/09/11 09:27:53  jim
Fixed some problems with the docs

Revision 1.2  2015/08/07 13:06:54  jim
Fixed copyright to ESO

Revision 1.1.1.1  2015/06/12 10:44:32  jim
Initial import

Revision 1.2  2015/04/30 12:08:24  jim
nothing

Revision 1.1  2015/04/08 14:57:19  jim
Initial entry


*/
